Skip to content

cartesian controller can work in base_frame #10

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
danielsanjosepro merged 3 commits into from
Oct 18, 2025
Merged

cartesian controller can work in base_frame #10

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9dbf87a
now the cartesian controller can handle the desired pose in base_fram…
Oct 16, 2025
caadb0c
Include description of new attributes
danielsanjosepro Oct 18, 2025
0f5a9d0
Removing a few old comments
danielsanjosepro Oct 18, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions include/crisp_controllers/cartesian_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,6 +258,15 @@ class CartesianController
* @return true if publisher count is safe (<=1), false otherwise
*/
bool check_topic_publisher_count(const std::string& topic_name);

/**
* @brief End-effector pose expressed in the base frame (base_frame)
*/
pinocchio::SE3 end_effector_pose_b;
/**
* @brief Pinocchio frame ID for params_.base_frame
*/
int base_frame_id;
};

} // namespace crisp_controllers
65 changes: 32 additions & 33 deletions src/cartesian_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,20 +63,15 @@ CartesianController::update(const rclcpp::Time &time,
model_.getJointId(joint_name); // pinocchio joind id might be different
auto joint = model_.joints[joint_id];

/*q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(),*/
/* params_.filter.q);*/
q[i] = state_interfaces_[i].get_value();
if (continous_joint_types.count(
joint.shortname())) { // Then we are handling a continous
// joint that is SO(2)
if (continous_joint_types.count(joint.shortname())) {
// Then we are handling a continous joint that is SO(2)
q_pin[joint.idx_q()] = std::cos(q[i]);
q_pin[joint.idx_q() + 1] = std::sin(q[i]);
} else { // simple revolute joint case
} else {
// simple revolute joint case
q_pin[joint.idx_q()] = q[i];
}
/*dq[i] = exponential_moving_average(*/
/* dq[i], state_interfaces_[num_joints + i].get_value(),*/
/* params_.filter.dq);*/
dq[i] = state_interfaces_[num_joints + i].get_value();
}

Expand All @@ -94,24 +89,16 @@ CartesianController::update(const rclcpp::Time &time,
pinocchio::log6(target_pose_), pinocchio::log6(new_target_pose),
params_.filter.target_pose));

/*target_pose_ = pinocchio::SE3(target_orientation_.toRotationMatrix(),
* target_position_);*/
end_effector_pose = data_.oMf[end_effector_frame_id];
const pinocchio::SE3 base_frame_pose = data_.oMf[base_frame_id];

end_effector_pose_b = base_frame_pose.inverse() * end_effector_pose;

// We consider translation and rotation separately to avoid unatural screw
// motions
if (params_.use_local_jacobian) {
error.head(3) =
end_effector_pose.rotation().transpose() *
(target_pose_.translation() - end_effector_pose.translation());
error.tail(3) = pinocchio::log3(end_effector_pose.rotation().transpose() *
target_pose_.rotation());
} else {
error.head(3) =
target_pose_.translation() - end_effector_pose.translation();
error.tail(3) = pinocchio::log3(target_pose_.rotation() *
end_effector_pose.rotation().transpose());
}
error.head(3) = target_pose_.translation() - end_effector_pose_b.translation();
error.tail(3) = pinocchio::log3(target_pose_.rotation() * end_effector_pose_b.rotation().transpose());


if (params_.limit_error) {
max_delta_ << params_.task.error_clip.x, params_.task.error_clip.y, params_.task.error_clip.z,
Expand All @@ -120,11 +107,19 @@ CartesianController::update(const rclcpp::Time &time,
}

J.setZero();
auto reference_frame = params_.use_local_jacobian
? pinocchio::ReferenceFrame::LOCAL
: pinocchio::ReferenceFrame::WORLD;
pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id,
reference_frame, J);
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();

Copilot AI Oct 16, 2025

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Suggested change
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();

Copilot uses AI. Check for mistakes.
J.noalias() = Ad_bw * J_world;
}
Comment on lines +110 to +122

Copilot AI Oct 16, 2025

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copilot uses AI. Check for mistakes.

Eigen::MatrixXd J_pinv(model_.nv, 6);
J_pinv = pseudo_inverse(J, params_.nullspace.regularization);
Expand Down Expand Up @@ -159,7 +154,7 @@ CartesianController::update(const rclcpp::Time &time,
}

if (model_.nq != model_.nv) {
// TODO: Then we have some continouts joints, not being handled for now
// TODO: Joint liimits for continous joints not implemented yet
tau_joint_limits = Eigen::VectorXd::Zero(model_.nv);
} else {
tau_joint_limits = get_joint_limit_torque(q, model_.lowerPositionLimit,
Expand Down Expand Up @@ -197,8 +192,6 @@ CartesianController::update(const rclcpp::Time &time,
if (params_.limit_torques) {
tau_d = saturateTorqueRate(tau_d, tau_previous, params_.max_delta_tau);
}
/*tau_d = exponential_moving_average(tau_d, tau_previous,*/
/* params_.filter.output_torque);*/

if (not params_.stop_commands) {
for (size_t i = 0; i < num_joints; ++i) {
Expand Down Expand Up @@ -308,6 +301,7 @@ CallbackReturn CartesianController::on_configure(
}

end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
base_frame_id = model_.getFrameId(params_.base_frame);
q = Eigen::VectorXd::Zero(model_.nv);
q_pin = Eigen::VectorXd::Zero(model_.nq);
dq = Eigen::VectorXd::Zero(model_.nv);
Expand Down Expand Up @@ -477,8 +471,13 @@ CallbackReturn CartesianController::on_activate(

end_effector_pose = data_.oMf[end_effector_frame_id];

target_position_ = end_effector_pose.translation();
target_orientation_ = Eigen::Quaterniond(end_effector_pose.rotation());
const pinocchio::SE3 T_wb = data_.oMf[base_frame_id];
const pinocchio::SE3 T_bw = T_wb.inverse();

end_effector_pose_b = T_bw * end_effector_pose;

target_position_ = end_effector_pose_b.translation();
target_orientation_ = Eigen::Quaterniond(end_effector_pose_b.rotation());
target_pose_ =
pinocchio::SE3(target_orientation_.toRotationMatrix(), target_position_);

Expand Down