00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <robot_mechanism_controllers/jt_cartesian_controller.h>
00038
00039 #include <Eigen/LU>
00040
00041 #include <ros/ros.h>
00042
00043 #include <angles/angles.h>
00044
00045 #include <pluginlib/class_list_macros.h>
00046 PLUGINLIB_EXPORT_CLASS(controller::JTCartesianController, pr2_controller_interface::Controller)
00047
00048
00049 namespace controller {
00050
00051 JTCartesianController::JTCartesianController()
00052 : robot_state_(NULL), use_posture_(false)
00053 {}
00054
00055 JTCartesianController::~JTCartesianController()
00056 {
00057 sub_gains_.shutdown();
00058 sub_posture_.shutdown();
00059 sub_pose_.shutdown();
00060 }
00061
00062
00063 bool JTCartesianController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
00064 {
00065 node_ = n;
00066
00067
00068 std::string tip_name;
00069 if (!node_.getParam("root_name", root_name_)){
00070 ROS_ERROR("JTCartesianController: No root name found on parameter server (namespace: %s)",
00071 node_.getNamespace().c_str());
00072 return false;
00073 }
00074 if (!node_.getParam("tip_name", tip_name)){
00075 ROS_ERROR("JTCartesianController: No tip name found on parameter server (namespace: %s)",
00076 node_.getNamespace().c_str());
00077 return false;
00078 }
00079
00080
00081 assert(robot_state);
00082 robot_state_ = robot_state;
00083
00084
00085 if (!chain_.init(robot_state_, root_name_, tip_name))
00086 return false;
00087 if (!chain_.allCalibrated())
00088 {
00089 ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
00090 return false;
00091 }
00092 if (chain_.size() != Joints)
00093 {
00094 ROS_ERROR("The JTCartesianController works with %d joints, but the chain from %s to %s has %d joints.",
00095 Joints, root_name_.c_str(), tip_name.c_str(), chain_.size());
00096 return false;
00097 }
00098
00099
00100 KDL::Chain kdl_chain;
00101 chain_.toKDL(kdl_chain);
00102 kin_.reset(new Kin<Joints>(kdl_chain));
00103
00104
00105 double kp_trans, kd_trans, kp_rot, kd_rot;
00106 if (!node_.getParam("cart_gains/trans/p", kp_trans) ||
00107 !node_.getParam("cart_gains/trans/d", kd_trans))
00108 {
00109 ROS_ERROR("P and D translational gains not specified (namespace: %s)", node_.getNamespace().c_str());
00110 return false;
00111 }
00112 if (!node_.getParam("cart_gains/rot/p", kp_rot) ||
00113 !node_.getParam("cart_gains/rot/d", kd_rot))
00114 {
00115 ROS_ERROR("P and D rotational gains not specified (namespace: %s)", node_.getNamespace().c_str());
00116 return false;
00117 }
00118 Kp << kp_trans, kp_trans, kp_trans, kp_rot, kp_rot, kp_rot;
00119 Kd << kd_trans, kd_trans, kd_trans, kd_rot, kd_rot, kd_rot;
00120
00121 node_.param("pose_command_filter", pose_command_filter_, 1.0);
00122
00123
00124 node_.param("vel_saturation_trans", vel_saturation_trans_, 0.0);
00125 node_.param("vel_saturation_rot", vel_saturation_rot_, 0.0);
00126
00127 node_.param("jacobian_inverse_damping", jacobian_inverse_damping_, 0.0);
00128 node_.param("joint_vel_filter", joint_vel_filter_, 1.0);
00129
00130
00131 for (int i = 0; i < Joints; ++i)
00132 node_.param("joint_feedforward/" + chain_.getJoint(i)->joint_->name, joint_dd_ff_[i], 0.0);
00133 for (int i = 0; i < Joints; ++i)
00134 node_.param("saturation/" + chain_.getJoint(i)->joint_->name, saturation_[i], 0.0);
00135
00136
00137 node_.param("k_posture", k_posture_, 1.0);
00138
00139 node_.param("resolution/force", res_force_, 0.01);
00140 node_.param("resolution/position", res_position_, 0.001);
00141 node_.param("resolution/torque", res_torque_, 0.01);
00142 node_.param("resolution/orientation", res_orientation_, 0.001);
00143
00144
00145 sub_gains_ = node_.subscribe("gains", 5, &JTCartesianController::setGains, this);
00146 sub_posture_ = node_.subscribe("command_posture", 5, &JTCartesianController::commandPosture, this);
00147 sub_pose_ = node_.subscribe("command_pose", 1, &JTCartesianController::commandPose, this);
00148
00149 StateMsg state_template;
00150 state_template.header.frame_id = root_name_;
00151 state_template.x.header.frame_id = root_name_;
00152 state_template.x_desi.header.frame_id = root_name_;
00153 state_template.x_desi_filtered.header.frame_id = root_name_;
00154 state_template.tau_pose.resize(Joints);
00155 state_template.tau_posture.resize(Joints);
00156 state_template.tau.resize(Joints);
00157 state_template.J.layout.dim.resize(2);
00158 state_template.J.data.resize(6*Joints);
00159 state_template.N.layout.dim.resize(2);
00160 state_template.N.data.resize(Joints*Joints);
00161 pub_state_.init(node_, "state", 10);
00162 pub_state_.lock();
00163 pub_state_.msg_ = state_template;
00164 pub_state_.unlock();
00165
00166 pub_x_desi_.init(node_, "state/x_desi", 10);
00167 pub_x_desi_.lock();
00168 pub_x_desi_.msg_.header.frame_id = root_name_;
00169 pub_x_desi_.unlock();
00170
00171 return true;
00172 }
00173
00174 void JTCartesianController::starting()
00175 {
00176
00177
00178
00179 JointVec q;
00180 chain_.getPositions(q);
00181 kin_->fk(q, x_desi_);
00182 x_desi_filtered_ = x_desi_;
00183 last_pose_ = x_desi_;
00184 q_posture_ = q;
00185 qdot_filtered_.setZero();
00186 last_wrench_.setZero();
00187
00188 loop_count_ = 0;
00189 }
00190
00191
00192 static void computePoseError(const Eigen::Affine3d &xact, const Eigen::Affine3d &xdes, Eigen::Matrix<double,6,1> &err)
00193 {
00194 err.head<3>() = xact.translation() - xdes.translation();
00195 err.tail<3>() = 0.5 * (xdes.linear().col(0).cross(xact.linear().col(0)) +
00196 xdes.linear().col(1).cross(xact.linear().col(1)) +
00197 xdes.linear().col(2).cross(xact.linear().col(2)));
00198 }
00199
00200 void JTCartesianController::update()
00201 {
00202
00203 ros::Time time = robot_state_->getTime();
00204 ros::Duration dt = time - last_time_;
00205 last_time_ = time;
00206 ++loop_count_;
00207
00208
00209
00210 JointVec q;
00211 chain_.getPositions(q);
00212
00213 Eigen::Affine3d x;
00214 kin_->fk(q, x);
00215
00216 Jacobian J;
00217 kin_->jac(q, J);
00218
00219
00220 JointVec qdot_raw;
00221 chain_.getVelocities(qdot_raw);
00222 for (int i = 0; i < Joints; ++i)
00223 qdot_filtered_[i] += joint_vel_filter_ * (qdot_raw[i] - qdot_filtered_[i]);
00224 JointVec qdot = qdot_filtered_;
00225 CartVec xdot = J * qdot;
00226
00227
00228
00229 {
00230 Eigen::Vector3d p0(x_desi_filtered_.translation());
00231 Eigen::Vector3d p1(x_desi_.translation());
00232 Eigen::Quaterniond q0(x_desi_filtered_.linear());
00233 Eigen::Quaterniond q1(x_desi_.linear());
00234 q0.normalize();
00235 q1.normalize();
00236
00237 tf::Quaternion tf_q0(q0.x(), q0.y(), q0.z(), q0.w());
00238 tf::Quaternion tf_q1(q1.x(), q1.y(), q1.z(), q1.w());
00239 tf::Quaternion tf_q = tf_q0.slerp(tf_q1, pose_command_filter_);
00240
00241 Eigen::Vector3d p = p0 + pose_command_filter_ * (p1 - p0);
00242
00243 Eigen::Quaterniond q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z());
00244
00245 x_desi_filtered_ = Eigen::Translation3d(p) * q;
00246 }
00247 CartVec x_err;
00248
00249 computePoseError(x, x_desi_filtered_, x_err);
00250
00251 CartVec xdot_desi = (Kp.array() / Kd.array()) * x_err.array() * -1.0;
00252
00253
00254 if (vel_saturation_trans_ > 0.0)
00255 {
00256 if (fabs(xdot_desi.head<3>().norm()) > vel_saturation_trans_)
00257 xdot_desi.head<3>() *= (vel_saturation_trans_ / xdot_desi.head<3>().norm());
00258 }
00259 if (vel_saturation_rot_ > 0.0)
00260 {
00261 if (fabs(xdot_desi.tail<3>().norm()) > vel_saturation_rot_)
00262 xdot_desi.tail<3>() *= (vel_saturation_rot_ / xdot_desi.tail<3>().norm());
00263 }
00264
00265 CartVec F = Kd.array() * (xdot_desi - xdot).array();
00266
00267 JointVec tau_pose = J.transpose() * F;
00268
00269
00270
00271
00272 Eigen::Matrix<double,6,6> I6; I6.setIdentity();
00273
00274
00275 Eigen::Matrix<double,6,6> JJt_damped = J * J.transpose() + jacobian_inverse_damping_ * I6;
00276 Eigen::Matrix<double,6,6> JJt_inv_damped = JJt_damped.inverse();
00277 Eigen::Matrix<double,Joints,6> J_pinv = J.transpose() * JJt_inv_damped;
00278
00279
00280 Eigen::Matrix<double,Joints,Joints> I;
00281 I.setIdentity();
00282 Eigen::Matrix<double,Joints,Joints> N = I - J_pinv * J;
00283
00284
00285
00286
00287 JointVec tau_posture;
00288 tau_posture.setZero();
00289 if (use_posture_)
00290 {
00291 JointVec posture_err = q_posture_ - q;
00292 for (size_t j = 0; j < Joints; ++j)
00293 {
00294 if (chain_.getJoint(j)->joint_->type == urdf::Joint::CONTINUOUS)
00295 posture_err[j] = angles::normalize_angle(posture_err[j]);
00296 }
00297
00298 for (size_t j = 0; j < Joints; ++j) {
00299 if (fabs(q_posture_[j] - 9999) < 1e-5)
00300 posture_err[j] = 0.0;
00301 }
00302
00303 JointVec qdd_posture = k_posture_ * posture_err;
00304 tau_posture = joint_dd_ff_.array() * (N * qdd_posture).array();
00305 }
00306
00307 JointVec tau = tau_pose + tau_posture;
00308
00309
00310 double sat_scaling = 1.0;
00311 for (int i = 0; i < Joints; ++i) {
00312 if (saturation_[i] > 0.0)
00313 sat_scaling = std::min(sat_scaling, fabs(saturation_[i] / tau[i]));
00314 }
00315 JointVec tau_sat = sat_scaling * tau;
00316
00317 chain_.addEfforts(tau_sat);
00318
00319 if (loop_count_ % 10 == 0)
00320 {
00321 if (pub_x_desi_.trylock()) {
00322 pub_x_desi_.msg_.header.stamp = time;
00323 tf::poseEigenToMsg(x_desi_, pub_x_desi_.msg_.pose);
00324 pub_x_desi_.unlockAndPublish();
00325 }
00326
00327 if (pub_state_.trylock()) {
00328 pub_state_.msg_.header.stamp = time;
00329 pub_state_.msg_.x.header.stamp = time;
00330 tf::poseEigenToMsg(x, pub_state_.msg_.x.pose);
00331 pub_state_.msg_.x_desi.header.stamp = time;
00332 tf::poseEigenToMsg(x_desi_, pub_state_.msg_.x_desi.pose);
00333 pub_state_.msg_.x_desi_filtered.header.stamp = time;
00334 tf::poseEigenToMsg(x_desi_filtered_, pub_state_.msg_.x_desi_filtered.pose);
00335 tf::twistEigenToMsg(x_err, pub_state_.msg_.x_err);
00336 tf::twistEigenToMsg(xdot, pub_state_.msg_.xd);
00337 tf::twistEigenToMsg(xdot_desi, pub_state_.msg_.xd_desi);
00338 tf::wrenchEigenToMsg(F, pub_state_.msg_.F);
00339 tf::matrixEigenToMsg(J, pub_state_.msg_.J);
00340 tf::matrixEigenToMsg(N, pub_state_.msg_.N);
00341 for (size_t j = 0; j < Joints; ++j) {
00342 pub_state_.msg_.tau_pose[j] = tau_pose[j];
00343 pub_state_.msg_.tau_posture[j] = tau_posture[j];
00344 pub_state_.msg_.tau[j] = tau[j];
00345 }
00346 pub_state_.unlockAndPublish();
00347 }
00348 }
00349 }
00350
00351 }