Impedance ref link and pose reference with custom reference frame #25
Conversation
|
I can look into this. @jsaltducaju: Feel free to comment here and in #24 as well |
There was a problem hiding this comment.
Pull Request Overview
This PR introduces support for configurable control frames in the Cartesian impedance controller, allowing stiffness and damping to be specified relative to frames other than the root frame. It also adds pose transformation capabilities and fixes a spelling error.
- Adds a
control_frameparameter to specify the reference frame for impedance control - Implements pose transformation between different coordinate frames
- Corrects the spelling of "effector" in a log message
Reviewed Changes
Copilot reviewed 6 out of 6 changed files in this pull request and generated 5 comments.
Show a summary per file
| File | Description |
|---|---|
| src/cartesian_impedance_controller_ros.cpp | Adds control frame initialization, pose transformation logic, frame_id assignment, and fixes spelling error |
| src/cartesian_impedance_controller.cpp | Updates torque calculation to incorporate control frame rotation matrix |
| include/cartesian_impedance_controller/rbdyn_wrapper.h | Adds control frame tracking and rotation matrix computation |
| include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h | Adds transformPose method declaration and control_frame member variable |
| include/cartesian_impedance_controller/cartesian_impedance_controller.h | Adds T_control_w_adj_ member and updates calculateCommandedTorques signature |
| README.md | Documents the new control_frame parameter |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| } | ||
| } | ||
|
|
||
| _R_control_root.setIdentity(); |
There was a problem hiding this comment.
_R_control_root is initialized after the early return when the end effector is found, meaning it only gets initialized when the exception is about to be thrown. This initialization should occur before the loop at line 34, or at the beginning of the init_rbdyn method to ensure it's always set to identity on initialization.
| mc_rbdyn_urdf::URDFParserResult _rbdyn_urdf; | ||
| std::vector<size_t> _rbd_indices; | ||
| size_t _ef_index; | ||
| size_t _cf_index; |
There was a problem hiding this comment.
The _cf_index member variable is declared but never initialized. If set_control_frame() is not called or called with a non-existent frame, _cf_index will contain garbage, leading to undefined behavior when checked at line 63. Initialize it to 0 (root link) in init_rbdyn or as a default member initializer.
| size_t _cf_index; | |
| size_t _cf_index = 0; |
| v_pos_rot[2]+transform.getOrigin().getZ(); | ||
|
|
||
| Eigen::Quaterniond eig_transform_quat(transform.getRotation().getW(), transform.getRotation().getX(), transform.getRotation().getY(), transform.getRotation().getZ()); | ||
| *quat = quat->inverse() * eig_transform_quat; |
There was a problem hiding this comment.
The quaternion multiplication order appears incorrect for transforming a quaternion from one frame to another. To transform a quaternion from frame A to frame B, you should use: *quat = eig_transform_quat * (*quat), not quat->inverse() * eig_transform_quat. The current implementation inverts the input quaternion before multiplication, which doesn't correctly represent the standard frame transformation.
| *quat = quat->inverse() * eig_transform_quat; | |
| *quat = eig_transform_quat * (*quat); |
| std::string end_effector_; //!< End-effector link name | ||
| std::string robot_description_; //!< URDF of the robot | ||
| std::string root_frame_; //!< Base frame obtained from URDF | ||
| std::string control_frame_; //!< Frame wrt impedance is referred |
There was a problem hiding this comment.
Grammatical error in comment: should be 'Frame with respect to which impedance is referred' or 'Frame to which impedance refers'.
| std::string control_frame_; //!< Frame wrt impedance is referred | |
| std::string control_frame_; //!< Frame with respect to which impedance is referred |
| { | ||
| F << config.f_x, config.f_y, config.f_z, config.tau_x, config.tau_y, config.tau_z; | ||
| if (!transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) | ||
| if (this->wrench_ee_frame_.compare( this->root_frame_) != 0 && !transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) |
There was a problem hiding this comment.
[nitpick] There's an inconsistency in how frame comparison is performed. This line uses 'compare() != 0' while lines 303 and 348 use 'compare() != 0' as well, but this is the only location where an extra space exists after 'root_frame_'. For consistency and readability across the codebase, consider using '!=' operator directly instead of the compare() method (e.g., 'this->wrench_ee_frame_ != this->root_frame_').
| if (this->wrench_ee_frame_.compare( this->root_frame_) != 0 && !transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) | |
| if (this->wrench_ee_frame_ != this->root_frame_ && !transformWrench(&F, this->wrench_ee_frame_, this->root_frame_)) |
2 participants
check #24