jt_cartesian_controller.cpp
Go to the documentation of this file.
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_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   // get name of root and tip from the parameter server
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   // test if we got robot pointer
00081   assert(robot_state);
00082   robot_state_ = robot_state;
00083 
00084   // Chain of joints
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   // Kinematics
00100   KDL::Chain kdl_chain;
00101   chain_.toKDL(kdl_chain);
00102   kin_.reset(new Kin<Joints>(kdl_chain));
00103 
00104   // Cartesian gains
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   // Velocity saturation
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   // Joint gains
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   // Posture gains
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   //Kp << 800.0, 800.0, 800.0,   80.0, 80.0, 80.0;
00177   //Kd << 12.0, 12.0, 12.0,   0.0, 0.0, 0.0;
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   // get time
00203   ros::Time time = robot_state_->getTime();
00204   ros::Duration dt = time - last_time_;
00205   last_time_ = time;
00206   ++loop_count_;
00207 
00208   // ======== Measures current arm state
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   // ======== Controls to the current pose setpoint
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     //Eigen::Quaterniond q = q0.slerp(pose_command_filter_, q1);
00243     Eigen::Quaterniond q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z());
00244     //x_desi_filtered_ = q * Eigen::Translation3d(p);
00245     x_desi_filtered_ = Eigen::Translation3d(p) * q;
00246   }
00247   CartVec x_err;
00248   //computePoseError(x, x_desi_, x_err);
00249   computePoseError(x, x_desi_filtered_, x_err);
00250 
00251   CartVec xdot_desi = (Kp.array() / Kd.array()) * x_err.array() * -1.0;
00252 
00253   // Caps the cartesian velocity
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   // ======== J psuedo-inverse and Nullspace computation
00270 
00271   // Computes pseudo-inverse of J
00272   Eigen::Matrix<double,6,6> I6; I6.setIdentity();
00273   //Eigen::Matrix<double,6,6> JJt = J * J.transpose();
00274   //Eigen::Matrix<double,6,6> JJt_inv = JJt.inverse();
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   // Computes the nullspace of J
00280   Eigen::Matrix<double,Joints,Joints> I;
00281   I.setIdentity();
00282   Eigen::Matrix<double,Joints,Joints> N = I - J_pinv * J;
00283 
00284   // ======== Posture control
00285 
00286   // Computes the desired joint torques for achieving the posture
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   // ======== Torque Saturation
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 } //namespace


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Thu Aug 27 2015 14:22:46