From 27b55fe7eee4c8468f8fcfe1d20aa9af130cb84f Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sun, 11 May 2025 09:57:02 -0400 Subject: [PATCH 1/4] Fix 6DOF arm example --- example_7/controller/r6bot_controller.cpp | 5 ++-- .../reference_generator/send_trajectory.cpp | 28 ++++++++----------- 2 files changed, 14 insertions(+), 19 deletions(-) diff --git a/example_7/controller/r6bot_controller.cpp b/example_7/controller/r6bot_controller.cpp index d5b576b9f..9e494605c 100644 --- a/example_7/controller/r6bot_controller.cpp +++ b/example_7/controller/r6bot_controller.cpp @@ -141,10 +141,11 @@ void interpolate_trajectory_point( double traj_len = static_cast(traj_msg.points.size()); auto last_time = traj_msg.points.back().time_from_start; double total_time = last_time.sec + last_time.nanosec * 1E-9; + double cur_time_sec = cur_time.seconds(); - size_t ind = static_cast(cur_time.seconds() * (traj_len / total_time)); + size_t ind = static_cast(cur_time_sec * (traj_len / total_time)); ind = std::min(ind, static_cast(traj_len) - 2); - double delta = cur_time.seconds() - static_cast(ind) * (total_time / traj_len); + double delta = std::min(cur_time_sec - static_cast(ind) * (total_time / traj_len), 1.0); interpolate_point(traj_msg.points[ind], traj_msg.points[ind + 1], point_interp, delta); } diff --git a/example_7/reference_generator/send_trajectory.cpp b/example_7/reference_generator/send_trajectory.cpp index ec853cb93..36e2bc2f0 100644 --- a/example_7/reference_generator/send_trajectory.cpp +++ b/example_7/reference_generator/send_trajectory.cpp @@ -63,15 +63,14 @@ int main(int argc, char ** argv) double total_time = 3.0; int trajectory_len = 200; - int loop_rate = static_cast(std::round(trajectory_len / total_time)); - double dt = 1.0 / loop_rate; + double dt = total_time / static_cast(trajectory_len - 1); for (int i = 0; i < trajectory_len; i++) { // set endpoint twist - double t = i; - twist.vel.x(2 * 0.3 * cos(2 * M_PI * t / trajectory_len)); - twist.vel.y(-0.3 * sin(2 * M_PI * t / trajectory_len)); + double t = i / (static_cast(trajectory_len - 1)); + twist.vel.x(2 * 0.3 * cos(2 * M_PI * t)); + twist.vel.y(-0.3 * sin(2 * M_PI * t)); // convert cart to joint velocities ik_vel_solver_->CartToJnt(joint_positions, twist, joint_velocities); @@ -88,22 +87,17 @@ int main(int argc, char ** argv) joint_positions.data += joint_velocities.data * dt; // set timing information - trajectory_point_msg.time_from_start.sec = i / loop_rate; - trajectory_point_msg.time_from_start.nanosec = static_cast( - 1E9 / loop_rate * - static_cast(t - loop_rate * (i / loop_rate))); // implicit integer division - + double time_point = total_time * t; + double time_point_sec = std::floor(time_point); + trajectory_point_msg.time_from_start.sec = static_cast(time_point_sec); + trajectory_point_msg.time_from_start.nanosec = + static_cast((time_point - time_point_sec) * 1E9); trajectory_msg.points.push_back(trajectory_point_msg); } // send zero velocities in the end - std::fill(trajectory_point_msg.velocities.begin(), trajectory_point_msg.velocities.end(), 0.0); - trajectory_point_msg.time_from_start.sec = trajectory_len / loop_rate; - trajectory_point_msg.time_from_start.nanosec = static_cast( - 1E9 / loop_rate * - static_cast( - trajectory_len - loop_rate * (trajectory_len / loop_rate))); // implicit integer division - trajectory_msg.points.push_back(trajectory_point_msg); + auto & last_point_msg = trajectory_msg.points.back(); + std::fill(last_point_msg.velocities.begin(), last_point_msg.velocities.end(), 0.0); pub->publish(trajectory_msg); while (rclcpp::ok()) From f6e25362199f3edf1a65864c2cbed454273995dc Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sat, 17 May 2025 09:54:09 -0400 Subject: [PATCH 2/4] PR comments, log messages, and remove [[nodiscard]] warnings --- .../bringup/launch/r6bot_controller.launch.py | 1 + example_7/controller/r6bot_controller.cpp | 29 +++++++++++++++---- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/example_7/bringup/launch/r6bot_controller.launch.py b/example_7/bringup/launch/r6bot_controller.launch.py index f397e5311..9273189d4 100644 --- a/example_7/bringup/launch/r6bot_controller.launch.py +++ b/example_7/bringup/launch/r6bot_controller.launch.py @@ -64,6 +64,7 @@ def generate_launch_description(): executable="ros2_control_node", parameters=[robot_controllers], output="both", + emulate_tty=True, ) robot_state_pub_node = Node( package="robot_state_publisher", diff --git a/example_7/controller/r6bot_controller.cpp b/example_7/controller/r6bot_controller.cpp index 9e494605c..902918dd3 100644 --- a/example_7/controller/r6bot_controller.cpp +++ b/example_7/controller/r6bot_controller.cpp @@ -84,6 +84,7 @@ controller_interface::CallbackReturn RobotController::on_configure(const rclcpp_ auto callback = [this](const std::shared_ptr traj_msg) -> void { + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory."); traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); new_msg_ = true; }; @@ -136,14 +137,16 @@ void interpolate_point( void interpolate_trajectory_point( const trajectory_msgs::msg::JointTrajectory & traj_msg, const rclcpp::Duration & cur_time, - trajectory_msgs::msg::JointTrajectoryPoint & point_interp) + trajectory_msgs::msg::JointTrajectoryPoint & point_interp, bool & reached_end) { double traj_len = static_cast(traj_msg.points.size()); auto last_time = traj_msg.points.back().time_from_start; double total_time = last_time.sec + last_time.nanosec * 1E-9; double cur_time_sec = cur_time.seconds(); + reached_end = (cur_time_sec >= total_time); - size_t ind = static_cast(cur_time_sec * (traj_len / total_time)); + size_t ind = + static_cast(cur_time_sec * (traj_len / total_time)); // Assumes evenly spaced points. ind = std::min(ind, static_cast(traj_len) - 2); double delta = std::min(cur_time_sec - static_cast(ind) * (total_time / traj_len), 1.0); interpolate_point(traj_msg.points[ind], traj_msg.points[ind + 1], point_interp, delta); @@ -161,14 +164,30 @@ controller_interface::return_type RobotController::update( if (trajectory_msg_ != nullptr) { - interpolate_trajectory_point(*trajectory_msg_, time - start_time_, point_interp_); + bool reached_end; + interpolate_trajectory_point(*trajectory_msg_, time - start_time_, point_interp_, reached_end); + + // If we have reached the end of the trajectory, reset it and set velocities to zero. + if (reached_end) + { + RCLCPP_INFO(get_node()->get_logger(), "Trajectory execution complete."); + trajectory_msg_.reset(); + std::fill(point_interp_.velocities.begin(), point_interp_.velocities.end(), 0.0); + } + for (size_t i = 0; i < joint_position_command_interface_.size(); i++) { - joint_position_command_interface_[i].get().set_value(point_interp_.positions[i]); + if (!joint_position_command_interface_[i].get().set_value(point_interp_.positions[i])) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to set position value for index %ld", i); + } } for (size_t i = 0; i < joint_velocity_command_interface_.size(); i++) { - joint_velocity_command_interface_[i].get().set_value(point_interp_.velocities[i]); + if (!joint_velocity_command_interface_[i].get().set_value(point_interp_.velocities[i])) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to set velocity value for index %ld", i); + } } } From b111ae3c5f74152e488d9f50a188b522ccb06d6b Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sat, 17 May 2025 09:56:55 -0400 Subject: [PATCH 3/4] Remove emulate_tty --- example_7/bringup/launch/r6bot_controller.launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/example_7/bringup/launch/r6bot_controller.launch.py b/example_7/bringup/launch/r6bot_controller.launch.py index 9273189d4..f397e5311 100644 --- a/example_7/bringup/launch/r6bot_controller.launch.py +++ b/example_7/bringup/launch/r6bot_controller.launch.py @@ -64,7 +64,6 @@ def generate_launch_description(): executable="ros2_control_node", parameters=[robot_controllers], output="both", - emulate_tty=True, ) robot_state_pub_node = Node( package="robot_state_publisher", From b946079b2485cd8ba209c6cec6b0f2f66de4ceae Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sat, 17 May 2025 16:22:36 -0400 Subject: [PATCH 4/4] Early return --- example_7/controller/r6bot_controller.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/example_7/controller/r6bot_controller.cpp b/example_7/controller/r6bot_controller.cpp index 902918dd3..06e0ccfb2 100644 --- a/example_7/controller/r6bot_controller.cpp +++ b/example_7/controller/r6bot_controller.cpp @@ -145,6 +145,14 @@ void interpolate_trajectory_point( double cur_time_sec = cur_time.seconds(); reached_end = (cur_time_sec >= total_time); + // If we reached the end of the trajectory, set the velocities to zero. + if (reached_end) + { + point_interp.positions = traj_msg.points.back().positions; + std::fill(point_interp.velocities.begin(), point_interp.velocities.end(), 0.0); + return; + } + size_t ind = static_cast(cur_time_sec * (traj_len / total_time)); // Assumes evenly spaced points. ind = std::min(ind, static_cast(traj_len) - 2); @@ -167,12 +175,11 @@ controller_interface::return_type RobotController::update( bool reached_end; interpolate_trajectory_point(*trajectory_msg_, time - start_time_, point_interp_, reached_end); - // If we have reached the end of the trajectory, reset it and set velocities to zero. + // If we have reached the end of the trajectory, reset it.. if (reached_end) { RCLCPP_INFO(get_node()->get_logger(), "Trajectory execution complete."); trajectory_msg_.reset(); - std::fill(point_interp_.velocities.begin(), point_interp_.velocities.end(), 0.0); } for (size_t i = 0; i < joint_position_command_interface_.size(); i++)