Fix 6DOF arm example (#804)

Author: sea-bass

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<trajectory_msgs::msg::JointTrajectory> traj_msg) -> void
   {
+    RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory.");
     traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);
     new_msg_ = true;
   };
@@ -136,15 +137,26 @@ 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<double>(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<size_t>(cur_time.seconds() * (traj_len / 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<size_t>(cur_time_sec * (traj_len / total_time));  // Assumes evenly spaced points.
   ind = std::min(ind, static_cast<size_t>(traj_len) - 2);
-  double delta = cur_time.seconds() - static_cast<double>(ind) * (total_time / traj_len);
+  double delta = std::min(cur_time_sec - static_cast<double>(ind) * (total_time / traj_len), 1.0);
   interpolate_point(traj_msg.points[ind], traj_msg.points[ind + 1], point_interp, delta);
 }
 
@@ -160,14 +172,29 @@ 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..
+    if (reached_end)
+    {
+      RCLCPP_INFO(get_node()->get_logger(), "Trajectory execution complete.");
+      trajectory_msg_.reset();
+    }
+
     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);
+      }
     }
   }
 

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<int>(std::round(trajectory_len / total_time));
-  double dt = 1.0 / loop_rate;
+  double dt = total_time / static_cast<double>(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<double>(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<int>(
-      1E9 / loop_rate *
-      static_cast<double>(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<int>(time_point_sec);
+    trajectory_point_msg.time_from_start.nanosec =
+      static_cast<int>((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<int>(
-    1E9 / loop_rate *
-    static_cast<double>(
-      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())