52 : robot_state_(NULL), use_posture_(false)
70 ROS_ERROR(
"JTCartesianController: No root name found on parameter server (namespace: %s)",
75 ROS_ERROR(
"JTCartesianController: No tip name found on parameter server (namespace: %s)",
94 ROS_ERROR(
"The JTCartesianController works with %d joints, but the chain from %s to %s has %d joints.",
100 KDL::Chain kdl_chain;
105 double kp_trans, kd_trans, kp_rot, kd_rot;
118 Kp << kp_trans, kp_trans, kp_trans, kp_rot, kp_rot, kp_rot;
119 Kd << kd_trans, kd_trans, kd_trans, kd_rot, kd_rot, kd_rot;
151 state_template.x.header.frame_id =
root_name_;
152 state_template.x_desi.header.frame_id =
root_name_;
153 state_template.x_desi_filtered.header.frame_id =
root_name_;
154 state_template.tau_pose.resize(
Joints);
155 state_template.tau_posture.resize(
Joints);
156 state_template.tau.resize(
Joints);
157 state_template.J.layout.dim.resize(2);
158 state_template.J.data.resize(6*
Joints);
159 state_template.N.layout.dim.resize(2);
192 static void computePoseError(
const Eigen::Affine3d &xact,
const Eigen::Affine3d &xdes, Eigen::Matrix<double,6,1> &err)
194 err.head<3>() = xact.translation() - xdes.translation();
195 err.tail<3>() = 0.5 * (xdes.linear().col(0).cross(xact.linear().col(0)) +
196 xdes.linear().col(1).cross(xact.linear().col(1)) +
197 xdes.linear().col(2).cross(xact.linear().col(2)));
231 Eigen::Vector3d p1(
x_desi_.translation());
233 Eigen::Quaterniond q1(
x_desi_.linear());
243 Eigen::Quaterniond q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z());
251 CartVec xdot_desi = (
Kp.array() /
Kd.array()) * x_err.array() * -1.0;
265 CartVec F =
Kd.array() * (xdot_desi - xdot).array();
267 JointVec tau_pose = J.transpose() * F;
272 Eigen::Matrix<double,6,6> I6; I6.setIdentity();
276 Eigen::Matrix<double,6,6> JJt_inv_damped = JJt_damped.inverse();
277 Eigen::Matrix<double,Joints,6> J_pinv = J.transpose() * JJt_inv_damped;
280 Eigen::Matrix<double,Joints,Joints> I;
282 Eigen::Matrix<double,Joints,Joints> N = I - J_pinv * J;
288 tau_posture.setZero();
292 for (
size_t j = 0; j <
Joints; ++j)
298 for (
size_t j = 0; j <
Joints; ++j) {
300 posture_err[j] = 0.0;
304 tau_posture =
joint_dd_ff_.array() * (N * qdd_posture).array();
307 JointVec tau = tau_pose + tau_posture;
310 double sat_scaling = 1.0;
313 sat_scaling = std::min(sat_scaling, fabs(
saturation_[i] / tau[i]));
315 JointVec tau_sat = sat_scaling * tau;
341 for (
size_t j = 0; j <
Joints; ++j) {