Fix 6DOF arm example (backport #804) (#806)

* Backport changes from #623

* Backport changes from #753

* Fix 6DOF arm example (#804)

* Revert changes to handles

---------

Co-authored-by: Christoph Froehlich <[email protected]>
Co-authored-by: Sebastian Castro <[email protected]>

Author: 3 people

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 = traj_msg.points.size();
-  auto last_time = traj_msg.points[traj_len - 1].time_from_start;
+  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 = cur_time.seconds() * (traj_len / total_time);
-  ind = std::min(static_cast<double>(ind), traj_len - 2);
-  double delta = cur_time.seconds() - ind * (total_time / traj_len);
+  // 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 = 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,7 +172,16 @@ 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]);

example_7/reference_generator/send_trajectory.cpp

@@ -48,7 +48,7 @@ int main(int argc, char ** argv)
 
   trajectory_msgs::msg::JointTrajectory trajectory_msg;
   trajectory_msg.header.stamp = node->now();
-  for (size_t i = 0; i < chain.getNrOfSegments(); i++)
+  for (unsigned int i = 0; i < chain.getNrOfSegments(); i++)
   {
     auto joint = chain.getSegment(i).getJoint();
     if (joint.getType() != KDL::Joint::Fixed)
@@ -63,15 +63,14 @@ int main(int argc, char ** argv)
 
   double total_time = 3.0;
   int trajectory_len = 200;
-  int loop_rate = 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,14 +87,18 @@ 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
+  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())
   {