Fix 6DOF arm example

example_7/controller/r6bot_controller.cpp

@@ -141,10 +141,11 @@ void interpolate_trajectory_point(
   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();
 
-  size_t ind = static_cast<size_t>(cur_time.seconds() * (traj_len / total_time));
+  size_t ind = static_cast<size_t>(cur_time_sec * (traj_len / total_time));
   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);
 }
 

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())