$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 // Author: Stuart Glaser 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_DECLARE_CLASS(robot_mechanism_controllers, JTCartesianController, 00047 controller::JTCartesianController, pr2_controller_interface::Controller) 00048 00049 00050 namespace controller { 00051 00052 JTCartesianController::JTCartesianController() 00053 : robot_state_(NULL), use_posture_(false) 00054 {} 00055 00056 JTCartesianController::~JTCartesianController() 00057 { 00058 sub_gains_.shutdown(); 00059 sub_posture_.shutdown(); 00060 sub_pose_.shutdown(); 00061 } 00062 00063 00064 bool JTCartesianController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n) 00065 { 00066 node_ = n; 00067 00068 // get name of root and tip from the parameter server 00069 std::string tip_name; 00070 if (!node_.getParam("root_name", root_name_)){ 00071 ROS_ERROR("JTCartesianController: No root name found on parameter server (namespace: %s)", 00072 node_.getNamespace().c_str()); 00073 return false; 00074 } 00075 if (!node_.getParam("tip_name", tip_name)){ 00076 ROS_ERROR("JTCartesianController: No tip name found on parameter server (namespace: %s)", 00077 node_.getNamespace().c_str()); 00078 return false; 00079 } 00080 00081 // test if we got robot pointer 00082 assert(robot_state); 00083 robot_state_ = robot_state; 00084 00085 // Chain of joints 00086 if (!chain_.init(robot_state_, root_name_, tip_name)) 00087 return false; 00088 if (!chain_.allCalibrated()) 00089 { 00090 ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str()); 00091 return false; 00092 } 00093 if (chain_.size() != Joints) 00094 { 00095 ROS_ERROR("The JTCartesianController works with %d joints, but the chain from %s to %s has %d joints.", 00096 Joints, root_name_.c_str(), tip_name.c_str(), chain_.size()); 00097 return false; 00098 } 00099 00100 // Kinematics 00101 KDL::Chain kdl_chain; 00102 chain_.toKDL(kdl_chain); 00103 kin_.reset(new Kin<Joints>(kdl_chain)); 00104 00105 // Cartesian gains 00106 double kp_trans, kd_trans, kp_rot, kd_rot; 00107 if (!node_.getParam("cart_gains/trans/p", kp_trans) || 00108 !node_.getParam("cart_gains/trans/d", kd_trans)) 00109 { 00110 ROS_ERROR("P and D translational gains not specified (namespace: %s)", node_.getNamespace().c_str()); 00111 return false; 00112 } 00113 if (!node_.getParam("cart_gains/rot/p", kp_rot) || 00114 !node_.getParam("cart_gains/rot/d", kd_rot)) 00115 { 00116 ROS_ERROR("P and D rotational gains not specified (namespace: %s)", node_.getNamespace().c_str()); 00117 return false; 00118 } 00119 Kp << kp_trans, kp_trans, kp_trans, kp_rot, kp_rot, kp_rot; 00120 Kd << kd_trans, kd_trans, kd_trans, kd_rot, kd_rot, kd_rot; 00121 00122 node_.param("pose_command_filter", pose_command_filter_, 1.0); 00123 00124 // Velocity saturation 00125 node_.param("vel_saturation_trans", vel_saturation_trans_, 0.0); 00126 node_.param("vel_saturation_rot", vel_saturation_rot_, 0.0); 00127 00128 node_.param("jacobian_inverse_damping", jacobian_inverse_damping_, 0.0); 00129 node_.param("joint_vel_filter", joint_vel_filter_, 1.0); 00130 00131 // Joint gains 00132 for (int i = 0; i < Joints; ++i) 00133 node_.param("joint_feedforward/" + chain_.getJoint(i)->joint_->name, joint_dd_ff_[i], 0.0); 00134 for (int i = 0; i < Joints; ++i) 00135 node_.param("saturation/" + chain_.getJoint(i)->joint_->name, saturation_[i], 0.0); 00136 00137 // Posture gains 00138 node_.param("k_posture", k_posture_, 1.0); 00139 00140 node_.param("resolution/force", res_force_, 0.01); 00141 node_.param("resolution/position", res_position_, 0.001); 00142 node_.param("resolution/torque", res_torque_, 0.01); 00143 node_.param("resolution/orientation", res_orientation_, 0.001); 00144 00145 00146 sub_gains_ = node_.subscribe("gains", 5, &JTCartesianController::setGains, this); 00147 sub_posture_ = node_.subscribe("command_posture", 5, &JTCartesianController::commandPosture, this); 00148 sub_pose_ = node_.subscribe("command_pose", 1, &JTCartesianController::commandPose, this); 00149 00150 StateMsg state_template; 00151 state_template.header.frame_id = root_name_; 00152 state_template.x.header.frame_id = root_name_; 00153 state_template.x_desi.header.frame_id = root_name_; 00154 state_template.x_desi_filtered.header.frame_id = root_name_; 00155 state_template.tau_pose.resize(Joints); 00156 state_template.tau_posture.resize(Joints); 00157 state_template.tau.resize(Joints); 00158 state_template.J.layout.dim.resize(2); 00159 state_template.J.data.resize(6*Joints); 00160 state_template.N.layout.dim.resize(2); 00161 state_template.N.data.resize(Joints*Joints); 00162 pub_state_.init(node_, "state", 10); 00163 pub_state_.lock(); 00164 pub_state_.msg_ = state_template; 00165 pub_state_.unlock(); 00166 00167 pub_x_desi_.init(node_, "state/x_desi", 10); 00168 pub_x_desi_.lock(); 00169 pub_x_desi_.msg_.header.frame_id = root_name_; 00170 pub_x_desi_.unlock(); 00171 00172 return true; 00173 } 00174 00175 void JTCartesianController::starting() 00176 { 00177 //Kp << 800.0, 800.0, 800.0, 80.0, 80.0, 80.0; 00178 //Kd << 12.0, 12.0, 12.0, 0.0, 0.0, 0.0; 00179 00180 JointVec q; 00181 chain_.getPositions(q); 00182 kin_->fk(q, x_desi_); 00183 x_desi_filtered_ = x_desi_; 00184 last_pose_ = x_desi_; 00185 q_posture_ = q; 00186 qdot_filtered_.setZero(); 00187 last_wrench_.setZero(); 00188 00189 loop_count_ = 0; 00190 } 00191 00192 00193 static void computePoseError(const Eigen::Affine3d &xact, const Eigen::Affine3d &xdes, Eigen::Matrix<double,6,1> &err) 00194 { 00195 err.head<3>() = xact.translation() - xdes.translation(); 00196 err.tail<3>() = 0.5 * (xdes.linear().col(0).cross(xact.linear().col(0)) + 00197 xdes.linear().col(1).cross(xact.linear().col(1)) + 00198 xdes.linear().col(2).cross(xact.linear().col(2))); 00199 } 00200 00201 void JTCartesianController::update() 00202 { 00203 // get time 00204 ros::Time time = robot_state_->getTime(); 00205 ros::Duration dt = time - last_time_; 00206 last_time_ = time; 00207 ++loop_count_; 00208 00209 // ======== Measures current arm state 00210 00211 JointVec q; 00212 chain_.getPositions(q); 00213 00214 Eigen::Affine3d x; 00215 kin_->fk(q, x); 00216 00217 Jacobian J; 00218 kin_->jac(q, J); 00219 00220 00221 JointVec qdot_raw; 00222 chain_.getVelocities(qdot_raw); 00223 for (int i = 0; i < Joints; ++i) 00224 qdot_filtered_[i] += joint_vel_filter_ * (qdot_raw[i] - qdot_filtered_[i]); 00225 JointVec qdot = qdot_filtered_; 00226 CartVec xdot = J * qdot; 00227 00228 // ======== Controls to the current pose setpoint 00229 00230 { 00231 Eigen::Vector3d p0(x_desi_filtered_.translation()); 00232 Eigen::Vector3d p1(x_desi_.translation()); 00233 Eigen::Quaterniond q0(x_desi_filtered_.linear()); 00234 Eigen::Quaterniond q1(x_desi_.linear()); 00235 q0.normalize(); 00236 q1.normalize(); 00237 00238 tf::Quaternion tf_q0(q0.x(), q0.y(), q0.z(), q0.w()); 00239 tf::Quaternion tf_q1(q1.x(), q1.y(), q1.z(), q1.w()); 00240 tf::Quaternion tf_q = tf_q0.slerp(tf_q1, pose_command_filter_); 00241 00242 Eigen::Vector3d p = p0 + pose_command_filter_ * (p1 - p0); 00243 //Eigen::Quaterniond q = q0.slerp(pose_command_filter_, q1); 00244 Eigen::Quaterniond q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z()); 00245 //x_desi_filtered_ = q * Eigen::Translation3d(p); 00246 x_desi_filtered_ = Eigen::Translation3d(p) * q; 00247 } 00248 CartVec x_err; 00249 //computePoseError(x, x_desi_, x_err); 00250 computePoseError(x, x_desi_filtered_, x_err); 00251 00252 CartVec xdot_desi = (Kp.array() / Kd.array()) * x_err.array() * -1.0; 00253 00254 // Caps the cartesian velocity 00255 if (vel_saturation_trans_ > 0.0) 00256 { 00257 if (fabs(xdot_desi.head<3>().norm()) > vel_saturation_trans_) 00258 xdot_desi.head<3>() *= (vel_saturation_trans_ / xdot_desi.head<3>().norm()); 00259 } 00260 if (vel_saturation_rot_ > 0.0) 00261 { 00262 if (fabs(xdot_desi.tail<3>().norm()) > vel_saturation_rot_) 00263 xdot_desi.tail<3>() *= (vel_saturation_rot_ / xdot_desi.tail<3>().norm()); 00264 } 00265 00266 CartVec F = Kd.array() * (xdot_desi - xdot).array(); 00267 00268 JointVec tau_pose = J.transpose() * F; 00269 00270 // ======== J psuedo-inverse and Nullspace computation 00271 00272 // Computes pseudo-inverse of J 00273 Eigen::Matrix<double,6,6> I6; I6.setIdentity(); 00274 //Eigen::Matrix<double,6,6> JJt = J * J.transpose(); 00275 //Eigen::Matrix<double,6,6> JJt_inv = JJt.inverse(); 00276 Eigen::Matrix<double,6,6> JJt_damped = J * J.transpose() + jacobian_inverse_damping_ * I6; 00277 Eigen::Matrix<double,6,6> JJt_inv_damped = JJt_damped.inverse(); 00278 Eigen::Matrix<double,Joints,6> J_pinv = J.transpose() * JJt_inv_damped; 00279 00280 // Computes the nullspace of J 00281 Eigen::Matrix<double,Joints,Joints> I; 00282 I.setIdentity(); 00283 Eigen::Matrix<double,Joints,Joints> N = I - J_pinv * J; 00284 00285 // ======== Posture control 00286 00287 // Computes the desired joint torques for achieving the posture 00288 JointVec tau_posture; 00289 tau_posture.setZero(); 00290 if (use_posture_) 00291 { 00292 JointVec posture_err = q_posture_ - q; 00293 for (size_t j = 0; j < Joints; ++j) 00294 { 00295 if (chain_.getJoint(j)->joint_->type == urdf::Joint::CONTINUOUS) 00296 posture_err[j] = angles::normalize_angle(posture_err[j]); 00297 } 00298 00299 for (size_t j = 0; j < Joints; ++j) { 00300 if (fabs(q_posture_[j] - 9999) < 1e-5) 00301 posture_err[j] = 0.0; 00302 } 00303 00304 JointVec qdd_posture = k_posture_ * posture_err; 00305 tau_posture = joint_dd_ff_.array() * (N * qdd_posture).array(); 00306 } 00307 00308 JointVec tau = tau_pose + tau_posture; 00309 00310 // ======== Torque Saturation 00311 double sat_scaling = 1.0; 00312 for (int i = 0; i < Joints; ++i) { 00313 if (saturation_[i] > 0.0) 00314 sat_scaling = std::min(sat_scaling, fabs(saturation_[i] / tau[i])); 00315 } 00316 JointVec tau_sat = sat_scaling * tau; 00317 00318 chain_.addEfforts(tau_sat); 00319 00320 if (loop_count_ % 10 == 0) 00321 { 00322 if (pub_x_desi_.trylock()) { 00323 pub_x_desi_.msg_.header.stamp = time; 00324 tf::poseEigenToMsg(x_desi_, pub_x_desi_.msg_.pose); 00325 pub_x_desi_.unlockAndPublish(); 00326 } 00327 00328 if (pub_state_.trylock()) { 00329 pub_state_.msg_.header.stamp = time; 00330 pub_state_.msg_.x.header.stamp = time; 00331 tf::poseEigenToMsg(x, pub_state_.msg_.x.pose); 00332 pub_state_.msg_.x_desi.header.stamp = time; 00333 tf::poseEigenToMsg(x_desi_, pub_state_.msg_.x_desi.pose); 00334 pub_state_.msg_.x_desi_filtered.header.stamp = time; 00335 tf::poseEigenToMsg(x_desi_filtered_, pub_state_.msg_.x_desi_filtered.pose); 00336 tf::twistEigenToMsg(x_err, pub_state_.msg_.x_err); 00337 tf::twistEigenToMsg(xdot, pub_state_.msg_.xd); 00338 tf::twistEigenToMsg(xdot_desi, pub_state_.msg_.xd_desi); 00339 tf::wrenchEigenToMsg(F, pub_state_.msg_.F); 00340 tf::matrixEigenToMsg(J, pub_state_.msg_.J); 00341 tf::matrixEigenToMsg(N, pub_state_.msg_.N); 00342 for (size_t j = 0; j < Joints; ++j) { 00343 pub_state_.msg_.tau_pose[j] = tau_pose[j]; 00344 pub_state_.msg_.tau_posture[j] = tau_posture[j]; 00345 pub_state_.msg_.tau[j] = tau[j]; 00346 } 00347 pub_state_.unlockAndPublish(); 00348 } 00349 } 00350 } 00351 00352 } //namespace