cartesian controller can work in base_frame#10
Conversation
…e instead of world frame defined in urdf
There was a problem hiding this comment.
Pull Request Overview
Fix frame inconsistency so the Cartesian controller computes errors and Jacobians in the configured base_frame instead of the URDF world frame.
- Compute end-effector pose in base frame and use it for error calculation
- Map Jacobians into base frame (from LOCAL or WORLD) via adjoint transforms
- Initialize and use base_frame_id; initialize target pose in base frame on activate
Reviewed Changes
Copilot reviewed 2 out of 2 changed files in this pull request and generated 5 comments.
| File | Description |
|---|---|
| src/cartesian_controller.cpp | Transforms end-effector pose and Jacobian into base_frame; updates error computation to base frame; initializes target pose in base frame on activation |
| include/crisp_controllers/cartesian_controller.hpp | Adds end_effector_pose_b and base_frame_id members to support base_frame computations |
Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.
| if (params_.use_local_jacobian) { | ||
| Eigen::MatrixXd J_local(6, model_.nv); | ||
| pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, | ||
| pinocchio::ReferenceFrame::LOCAL, J_local); | ||
| const Eigen::Matrix<double,6,6> Ad_be = end_effector_pose_b.toActionMatrix(); | ||
| J.noalias() = Ad_be * J_local; | ||
| } else { | ||
| Eigen::MatrixXd J_world(6, model_.nv); | ||
| pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, | ||
| pinocchio::ReferenceFrame::WORLD, J_world); | ||
| const Eigen::Matrix<double,6,6> Ad_bw = base_frame_pose.inverse().toActionMatrix(); | ||
| J.noalias() = Ad_bw * J_world; | ||
| } |
There was a problem hiding this comment.
J_local and J_world are dynamically allocated on every update cycle, which introduces unnecessary heap allocations in the control loop. Consider preallocating scratch matrices as members (resized in on_configure/on_activate) and reusing them, or reuse J as a temporary buffer (dropping noalias if aliasing occurs) to avoid per-iteration allocations.
|
|
||
| end_effector_pose_b = base_frame_pose.inverse() * end_effector_pose; | ||
| // target_pose_b = base_frame_pose.inverse() * target_pose_; | ||
| // pinocchio::SE3 new_target_pose_b = base_frame_pose.inverse() * new_target_pose; |
There was a problem hiding this comment.
base_frame_pose.inverse() is computed twice in the same update; compute it once and reuse to avoid duplicate SE3 inversions. For example: const auto T_bw = base_frame_pose.inverse(); end_effector_pose_b = T_bw * end_effector_pose; and const auto Ad_bw = T_bw.toActionMatrix();
| end_effector_pose_b = base_frame_pose.inverse() * end_effector_pose; | |
| // target_pose_b = base_frame_pose.inverse() * target_pose_; | |
| // pinocchio::SE3 new_target_pose_b = base_frame_pose.inverse() * new_target_pose; | |
| const pinocchio::SE3 T_bw = base_frame_pose.inverse(); | |
| end_effector_pose_b = T_bw * end_effector_pose; | |
| // target_pose_b = T_bw * target_pose_; | |
| // pinocchio::SE3 new_target_pose_b = T_bw * new_target_pose; |
| Eigen::MatrixXd J_world(6, model_.nv); | ||
| pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, | ||
| pinocchio::ReferenceFrame::WORLD, J_world); | ||
| const Eigen::Matrix<double,6,6> Ad_bw = base_frame_pose.inverse().toActionMatrix(); |
There was a problem hiding this comment.
base_frame_pose.inverse() is computed twice in the same update; compute it once and reuse to avoid duplicate SE3 inversions. For example: const auto T_bw = base_frame_pose.inverse(); end_effector_pose_b = T_bw * end_effector_pose; and const auto Ad_bw = T_bw.toActionMatrix();
| const Eigen::Matrix<double,6,6> Ad_bw = base_frame_pose.inverse().toActionMatrix(); | |
| const auto T_bw = base_frame_pose.inverse(); | |
| const Eigen::Matrix<double,6,6> Ad_bw = T_bw.toActionMatrix(); |
| // target_pose_b = base_frame_pose.inverse() * target_pose_; | ||
| // pinocchio::SE3 new_target_pose_b = base_frame_pose.inverse() * new_target_pose; |
There was a problem hiding this comment.
[nitpick] Remove commented-out code to keep the control loop clear. If needed for future reference, capture this in a design note or a dedicated helper function.
|
Thanks for the PR! I already tested the updated controller and it seems to work fine. I will take a look at the code but otherwise it looks fine! :) |
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
danielsanjosepro
left a comment
danielsanjosepro
left a comment
There was a problem hiding this comment.
@AlexD15216 I removed a few old comments and added a short description for the new attributes but all in all it is good. Thanks for contributing!
… indicated in the parameters and not just world frame (learnsyslab#10) * now the cartesian controller can handle the desired pose in base_frame instead of world frame defined in urdf * Include description of new attributes Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * Removing a few old comments --------- Co-authored-by: yibo <yibo.di@iis.fraunhofer.de> Co-authored-by: Daniel San José Pro <42489409+danielsanjosepro@users.noreply.github.com> Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
This fixes a frame inconsistency in cartesian controller where target_pose are supposed to expressed in the base_frame (defined in config), but the end_effector_pose were retrieved (via Pinocchio) and used for error computing only in the world frame defined in the URDF.
This is related to Issue #9.