Interpolation #34
Interpolation #34
Conversation
Modified cartesian_impedance_controller_ros.h. Added 2 parameters for position and orientation
Modified cartesian_impedance_controller_ros.cpp. Implementation of linear Interpolation and Slerp algorithm for orientation.
There was a problem hiding this comment.
Pull Request Overview
This pull request refactors trajectory execution to use Cartesian interpolation instead of step-wise waypoint following, and adds MoveIt integration support. The controller now pre-computes Cartesian poses for trajectory points and smoothly interpolates between them using linear interpolation for position and SLERP for orientation.
Key changes:
- Implements smooth Cartesian interpolation between trajectory waypoints with LERP for position and SLERP for orientation
- Pre-computes and caches all Cartesian poses at trajectory start for efficiency
- Adds MoveIt ros_control interface plugin configuration
Reviewed Changes
Copilot reviewed 4 out of 4 changed files in this pull request and generated 3 comments.
| File | Description |
|---|---|
| src/cartesian_impedance_controller_ros.cpp | Refactors trajStart() to pre-compute Cartesian poses and trajUpdate() to interpolate smoothly between waypoints |
| include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h | Adds member variables to cache pre-computed Cartesian positions and orientations |
| cartesian_impedance_moveit_plugin.xml | New plugin configuration file for MoveIt integration |
| package.xml | Adds MoveIt dependency and exports the new plugin configuration |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| base_class_type="moveit_ros_control_interface::ControllerHandleAllocator" | ||
| > | ||
| <description> | ||
| Controller description |
There was a problem hiding this comment.
The description field is generic and uninformative. It should provide a meaningful description of the Cartesian Impedance Controller's purpose and capabilities for MoveIt users.
| Controller description | |
| A Cartesian Impedance Controller that enables compliant motion in Cartesian space by regulating end-effector stiffness and damping. Suitable for tasks requiring force control and safe interaction with the environment. |
| double dt = t_next - t_prev; | ||
| double alpha = 1.0; // Default alpha if segment duration is invalid | ||
| if (dt > 1e-6) // Use a small epsilon to avoid floating point issues near zero | ||
| { | ||
| alpha = (t_curr - t_prev) / dt; | ||
| } else { | ||
| ROS_WARN_THROTTLE(1.0, "Trajectory segment duration is near-zero or negative. Clamping alpha=1."); | ||
| } |
There was a problem hiding this comment.
The comment on line 681 is misleading. The default alpha is set to 1.0 regardless of whether the segment duration is invalid. When dt <= 1e-6, alpha remains at the initial value of 1.0 (not clamped to it), which jumps directly to the next waypoint. Consider clarifying the comment to reflect that alpha=1.0 is used as a fallback when segment duration is too small or negative.
| ROS_ERROR("Trajectory point %zu has incorrect number of joints (%zu vs %zu). Aborting trajectory.", | ||
| i, trajectory.points.at(i).positions.size(), this->n_joints_); |
There was a problem hiding this comment.
The error message should clarify which value is 'expected' vs 'received' for better debugging. Consider rephrasing to: 'Trajectory point %zu has incorrect number of joints (expected %zu, got %zu). Aborting trajectory.' to make the order unambiguous.
| ROS_ERROR("Trajectory point %zu has incorrect number of joints (%zu vs %zu). Aborting trajectory.", | |
| i, trajectory.points.at(i).positions.size(), this->n_joints_); | |
| ROS_ERROR("Trajectory point %zu has incorrect number of joints (expected %zu, got %zu). Aborting trajectory.", | |
| i, this->n_joints_, trajectory.points.at(i).positions.size()); |
1 participant
Implementation of linear interpolation in combination with the slerp algorithm for orientation. For this two vectors
traj_positions_andtraj_orientations_were added tocartesian_impedance_controller_ros.h. The according filecartesian_impedance_controller_ros.cppwas modified, specifically the functiontrajUpdate().