jt_task_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, 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 #include <boost/thread.hpp>
00036 #include <boost/shared_ptr.hpp>
00037 #include <boost/scoped_ptr.hpp>
00038 
00039 #include <Eigen/Core>
00040 #include <Eigen/Geometry>
00041 #include <Eigen/LU>
00042 
00043 #include <kdl/chainfksolver.hpp>
00044 #include <kdl/chainfksolverpos_recursive.hpp>
00045 #include <kdl/chain.hpp>
00046 #include <kdl/chainjnttojacsolver.hpp>
00047 #include <kdl/frames.hpp>
00048 
00049 #include <ros/ros.h>
00050 
00051 #include <geometry_msgs/PoseStamped.h>
00052 #include <geometry_msgs/TwistStamped.h>
00053 #include <pr2_manipulation_controllers/JTTaskControllerState.h>
00054 #include <object_manipulation_msgs/CartesianGains.h>
00055 
00056 #include <pluginlib/class_list_macros.h>
00057 #include <angles/angles.h>
00058 #include <control_toolbox/pid.h>
00059 #include <eigen_conversions/eigen_kdl.h>
00060 #include <eigen_conversions/eigen_msg.h>
00061 #include <pr2_controller_interface/controller.h>
00062 #include <pr2_mechanism_model/chain.h>
00063 #include <realtime_tools/realtime_publisher.h>
00064 #include <tf/transform_listener.h>
00065 
00066 #include <rosrt/rosrt.h>
00067 
00068 namespace pr2_manipulation_controllers {
00069 
00070 template <int Joints>
00071 struct Kin
00072 {
00073   typedef Eigen::Matrix<double, Joints, 1> JointVec;
00074   typedef Eigen::Matrix<double, 6, Joints> Jacobian;
00075 
00076   Kin(const KDL::Chain &kdl_chain) :
00077     fk_solver_(kdl_chain), jac_solver_(kdl_chain),
00078     kdl_q(Joints), kdl_J(Joints)
00079   {
00080   }
00081   ~Kin()
00082   {
00083   }
00084 
00085   void fk(const JointVec &q, Eigen::Affine3d &x)
00086   {
00087     kdl_q.data = q;
00088     KDL::Frame kdl_x;
00089     fk_solver_.JntToCart(kdl_q, kdl_x);
00090     tf::transformKDLToEigen(kdl_x, x);
00091   }
00092   void jac(const JointVec &q, Jacobian &J)
00093   {
00094     kdl_q.data = q;
00095     jac_solver_.JntToJac(kdl_q, kdl_J);
00096     J = kdl_J.data;
00097   }
00098 
00099   KDL::ChainFkSolverPos_recursive fk_solver_;
00100   KDL::ChainJntToJacSolver jac_solver_;
00101   KDL::JntArray kdl_q;
00102   KDL::Jacobian kdl_J;
00103 };
00104 
00105 class JTTaskController : public pr2_controller_interface::Controller
00106 {
00107 public:
00108   // Ensure 128-bit alignment for Eigen
00109   // See also http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
00110   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00111 private:
00112   enum { Joints = 7 };
00113   typedef Eigen::Matrix<double, Joints, 1> JointVec;
00114   typedef Eigen::Matrix<double, 6, 1> CartVec;
00115   typedef Eigen::Matrix<double, 6, Joints> Jacobian;
00116   typedef pr2_manipulation_controllers::JTTaskControllerState StateMsg;
00117 public:
00118   JTTaskController();
00119   ~JTTaskController();
00120 
00121   bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
00122   void starting();
00123   void update();
00124 
00125   Eigen::Affine3d x_desi_, x_desi_filtered_;
00126   CartVec wrench_desi_;
00127 
00128   ros::NodeHandle node_;
00129   ros::Subscriber sub_gains_;
00130   ros::Subscriber sub_posture_;
00131   ros::Subscriber sub_pose_;
00132   tf::TransformListener tf_;
00133 
00134   rosrt::Publisher<StateMsg> pub_state_;
00135   rosrt::Publisher<geometry_msgs::PoseStamped> pub_x_, pub_x_desi_;
00136   rosrt::Publisher<geometry_msgs::Twist> pub_xd_, pub_xd_desi_;
00137   rosrt::Publisher<geometry_msgs::Twist> pub_x_err_, pub_wrench_;
00138   rosrt::Publisher<std_msgs::Float64MultiArray> pub_tau_;
00139 
00140   std::string root_name_, tip_name_;
00141   ros::Time last_time_;
00142   int loop_count_;
00143   pr2_mechanism_model::RobotState *robot_state_;
00144 
00145   pr2_mechanism_model::Chain chain_;
00146   boost::scoped_ptr<Kin<Joints> > kin_;
00147   
00148   Eigen::Matrix<double,6,1> Kp, Kd;  //aleeper
00149   
00150   // Todo Rename this! The rotation matrix for tool->gain frame
00151   Eigen::Matrix<double,6,6> St;  //aleeper
00152   bool use_tip_frame_; // aleeper
00153   double pose_command_filter_;
00154   double vel_saturation_trans_, vel_saturation_rot_;
00155   JointVec saturation_;
00156   JointVec joint_dd_ff_;
00157   double joint_vel_filter_;
00158   double jacobian_inverse_damping_;
00159   JointVec q_posture_;
00160   double k_posture_;
00161   bool use_posture_;
00162 
00163   // Minimum resolutions
00164   double res_force_, res_position_;
00165   double res_torque_, res_orientation_;
00166 
00167   Eigen::Affine3d last_pose_;
00168   CartVec last_wrench_;
00169   double last_stiffness_, last_compliance_;
00170   double last_Dx_, last_Df_;
00171 
00172 
00173   JointVec qdot_filtered_;
00174 
00175 /*  void setGains(const std_msgs::Float64MultiArray::ConstPtr &msg)
00176   {
00177     if (msg->data.size() >= 6)
00178       for (size_t i = 0; i < 6; ++i)
00179         Kp[i] = msg->data[i];
00180     if (msg->data.size() == 12)
00181       for (size_t i = 0; i < 6; ++i)
00182         Kd[i] = msg->data[6+i];
00183 
00184     ROS_INFO("New gains: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
00185              Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
00186   }
00187 */
00188   void setGains(const object_manipulation_msgs::CartesianGains::ConstPtr &msg)
00189   {
00190 
00191     //ROS_INFO_STREAM("Received CartesianGains msg: " << *msg);
00192     //ROS_INFO("root: [%s] tip: [%s]", root_name_.c_str(), tip_name_.c_str()); 
00193     
00194     // Store gains...
00195     if (msg->gains.size() >= 6)
00196       for (size_t i = 0; i < 6; ++i)
00197         Kp[i] = msg->gains[i];
00198     if (msg->gains.size() == 12)
00199       for (size_t i = 0; i < 6; ++i)
00200         Kd[i] = msg->gains[6+i];
00201 
00202     // Store frame information
00203     if(!msg->header.frame_id.compare(root_name_))
00204     {
00205       use_tip_frame_ = false;
00206       ROS_INFO("New gains in root frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
00207                root_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
00208       St.setIdentity();
00209     }
00210     else if(!msg->header.frame_id.compare(tip_name_))
00211     {
00212       use_tip_frame_ = true;
00213       ROS_INFO("New gains in tip frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
00214                tip_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
00215       
00216     }
00217     else
00218     {
00219       use_tip_frame_ = false;
00220       
00221       geometry_msgs::PoseStamped in_root;
00222       in_root.pose.orientation.w = 1.0;
00223       in_root.header.frame_id = msg->header.frame_id;
00224 
00225       try {
00226         tf_.waitForTransform(root_name_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
00227         tf_.transformPose(root_name_, in_root, in_root);
00228       }
00229       catch (const tf::TransformException &ex)
00230       {
00231         ROS_ERROR("Failed to transform: %s", ex.what());
00232         return;
00233       }
00234       
00235       Eigen::Affine3d t;
00236       
00237       tf::poseMsgToEigen(in_root.pose, t);
00238 
00239       St << 
00240           t(0,0),t(0,1),t(0,2),0,0,0,
00241           t(1,0),t(1,1),t(1,2),0,0,0,
00242           t(2,0),t(2,1),t(2,2),0,0,0,
00243           0,0,0,t(0,0),t(0,1),t(0,2),
00244           0,0,0,t(1,0),t(1,1),t(1,2),
00245           0,0,0,t(2,0),t(2,1),t(2,2);
00246     
00247       St.transposeInPlace();
00248   
00249       ROS_INFO("New gains in arbitrary frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
00250              msg->header.frame_id.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
00251     }
00252   }
00253 
00254   void commandPosture(const std_msgs::Float64MultiArray::ConstPtr &msg)
00255   {
00256     if (msg->data.size() == 0) {
00257       use_posture_ = false;
00258       ROS_INFO("Posture turned off");
00259     }
00260     else if ((int)msg->data.size() != q_posture_.size()) {
00261       ROS_ERROR("Posture message had the wrong size: %d", (int)msg->data.size());
00262       return;
00263     }
00264     else
00265     {
00266       use_posture_ = true;
00267       for (int j = 0; j < Joints; ++j)
00268         q_posture_[j] = msg->data[j];
00269     }
00270   }
00271 
00272   void commandPose(const geometry_msgs::PoseStamped::ConstPtr &command)
00273   {
00274     geometry_msgs::PoseStamped in_root;
00275     try {
00276       tf_.waitForTransform(root_name_, command->header.frame_id, command->header.stamp, ros::Duration(0.1));
00277       tf_.transformPose(root_name_, *command, in_root);
00278     }
00279     catch (const tf::TransformException &ex)
00280     {
00281       ROS_ERROR("Failed to transform: %s", ex.what());
00282       return;
00283     }
00284 
00285     tf::poseMsgToEigen(in_root.pose, x_desi_);
00286   }
00287 };
00288 
00289 
00290 JTTaskController::JTTaskController()
00291   : robot_state_(NULL), use_posture_(false)
00292 {}
00293 
00294 JTTaskController::~JTTaskController()
00295 {
00296   sub_gains_.shutdown();
00297   sub_posture_.shutdown();
00298   sub_pose_.shutdown();
00299 }
00300 
00301 
00302 bool JTTaskController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
00303 {
00304   rosrt::init();
00305   node_ = n;
00306 
00307   ROS_INFO_STREAM("JTTask controller compiled at " << __TIME__ );
00308   // get name of root and tip from the parameter server
00309   // std::string tip_name; // aleeper: Should use the class member instead!
00310   if (!node_.getParam("root_name", root_name_)){
00311     ROS_ERROR("JTTaskController: No root name found on parameter server (namespace: %s)",
00312               node_.getNamespace().c_str());
00313     return false;
00314   }
00315   if (!node_.getParam("tip_name", tip_name_)){
00316     ROS_ERROR("JTTaskController: No tip name found on parameter server (namespace: %s)",
00317               node_.getNamespace().c_str());
00318     return false;
00319   }
00320 
00321   // test if we got robot pointer
00322   assert(robot_state);
00323   robot_state_ = robot_state;
00324 
00325   // Chain of joints
00326   if (!chain_.init(robot_state_, root_name_, tip_name_))
00327     return false;
00328   if (!chain_.allCalibrated())
00329   {
00330     ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
00331     return false;
00332   }
00333 
00334 
00335   // Kinematics
00336   KDL::Chain kdl_chain;
00337   chain_.toKDL(kdl_chain);
00338   kin_.reset(new Kin<Joints>(kdl_chain));
00339 
00340   // Cartesian gains
00341   double kp_trans, kd_trans, kp_rot, kd_rot;
00342   if (!node_.getParam("cart_gains/trans/p", kp_trans) ||
00343       !node_.getParam("cart_gains/trans/d", kd_trans))
00344   {
00345     ROS_ERROR("P and D translational gains not specified (namespace: %s)", node_.getNamespace().c_str());
00346     return false;
00347   }
00348   if (!node_.getParam("cart_gains/rot/p", kp_rot) ||
00349       !node_.getParam("cart_gains/rot/d", kd_rot))
00350   {
00351     ROS_ERROR("P and D rotational gains not specified (namespace: %s)", node_.getNamespace().c_str());
00352     return false;
00353   }
00354   Kp << kp_trans, kp_trans, kp_trans,  kp_rot, kp_rot, kp_rot;
00355   Kd << kd_trans, kd_trans, kd_trans,  kd_rot, kd_rot, kd_rot;
00356 
00357   // aleeper
00358   use_tip_frame_ = false;
00359   if (!node_.getParam("use_tip_frame", use_tip_frame_)){
00360     ROS_WARN("JTTaskController: use_tip_frame was not specified, assuming 'false': %s)",
00361               node_.getNamespace().c_str());
00362   }
00363   St.setIdentity();
00364 
00365   node_.param("pose_command_filter", pose_command_filter_, 1.0);
00366 
00367   // Velocity saturation
00368   node_.param("vel_saturation_trans", vel_saturation_trans_, 0.0);
00369   node_.param("vel_saturation_rot", vel_saturation_rot_, 0.0);
00370 
00371   node_.param("jacobian_inverse_damping", jacobian_inverse_damping_, 0.0);
00372   node_.param("joint_vel_filter", joint_vel_filter_, 1.0);
00373 
00374   // Joint gains
00375   for (int i = 0; i < Joints; ++i)
00376     node_.param("joint_feedforward/" + chain_.getJoint(i)->joint_->name, joint_dd_ff_[i], 0.0);
00377   for (int i = 0; i < Joints; ++i)
00378     node_.param("joint_max_effort/" + chain_.getJoint(i)->joint_->name, saturation_[i], 0.0);
00379 
00380   // Posture gains
00381   node_.param("k_posture", k_posture_, 1.0);
00382 
00383   node_.param("resolution/force", res_force_, 0.01);
00384   node_.param("resolution/position", res_position_, 0.001);
00385   node_.param("resolution/torque", res_torque_, 0.01);
00386   node_.param("resolution/orientation", res_orientation_, 0.001);
00387 
00388 
00389   sub_gains_ = node_.subscribe("gains", 5, &JTTaskController::setGains, this);
00390   sub_posture_ = node_.subscribe("command_posture", 5, &JTTaskController::commandPosture, this);
00391   sub_pose_ = node_.subscribe("command_pose", 1, &JTTaskController::commandPose, this);
00392 
00393   StateMsg state_template;
00394   state_template.header.frame_id = root_name_;
00395   state_template.x.header.frame_id = root_name_;
00396   state_template.x_desi.header.frame_id = root_name_;
00397   state_template.x_desi_filtered.header.frame_id = root_name_;
00398   state_template.tau_pose.resize(Joints);
00399   state_template.tau_posture.resize(Joints);
00400   state_template.tau.resize(Joints);
00401   state_template.J.layout.dim.resize(2);
00402   state_template.J.data.resize(6*Joints);
00403   state_template.N.layout.dim.resize(2);
00404   state_template.N.data.resize(Joints*Joints);
00405   pub_state_.initialize(node_.advertise<StateMsg>("state", 10), 10, state_template);
00406 
00407   geometry_msgs::PoseStamped pose_template;
00408   pose_template.header.frame_id = root_name_;
00409   pub_x_.initialize(node_.advertise<geometry_msgs::PoseStamped>("state/x", 10),
00410                     10, pose_template);
00411   pub_x_desi_.initialize(node_.advertise<geometry_msgs::PoseStamped>("state/x_desi", 10),
00412                          10, pose_template);
00413   pub_x_err_.initialize(node_.advertise<geometry_msgs::Twist>("state/x_err", 10),
00414                         10, geometry_msgs::Twist());
00415   pub_xd_.initialize(node_.advertise<geometry_msgs::Twist>("state/xd", 10),
00416                      10, geometry_msgs::Twist());
00417   pub_xd_desi_.initialize(node_.advertise<geometry_msgs::Twist>("state/xd_desi", 10),
00418                           10, geometry_msgs::Twist());
00419   pub_wrench_.initialize(node_.advertise<geometry_msgs::Twist>("state/wrench", 10),
00420                          10, geometry_msgs::Twist());
00421 
00422   std_msgs::Float64MultiArray joints_template;
00423   joints_template.layout.dim.resize(1);
00424   joints_template.layout.dim[0].size = Joints;
00425   joints_template.layout.dim[0].stride = 1;
00426   joints_template.data.resize(7);
00427   pub_tau_.initialize(node_.advertise<std_msgs::Float64MultiArray>("state/tau", 10),
00428                       10, joints_template);
00429 
00430   return true;
00431 }
00432 
00433 void JTTaskController::starting()
00434 {
00435   //Kp << 800.0, 800.0, 800.0,   80.0, 80.0, 80.0;
00436   //Kd << 12.0, 12.0, 12.0,   0.0, 0.0, 0.0;
00437 
00438   JointVec q;
00439   chain_.getPositions(q);
00440   kin_->fk(q, x_desi_);
00441   x_desi_filtered_ = x_desi_;
00442   last_pose_ = x_desi_;
00443   q_posture_ = q;
00444   qdot_filtered_.setZero();
00445   last_wrench_.setZero();
00446 
00447   last_stiffness_ = 0;
00448   last_compliance_ = 0;
00449   last_Dx_ = 0;
00450   last_Df_ = 0;
00451 
00452   loop_count_ = 0;
00453 }
00454 
00455 
00456 static void computePoseError(const Eigen::Affine3d &xact, const Eigen::Affine3d &xdes, Eigen::Matrix<double,6,1> &err)
00457 {
00458   err.head<3>() = xact.translation() - xdes.translation();
00459   err.tail<3>()   = 0.5 * (xdes.linear().col(0).cross(xact.linear().col(0)) +
00460                           xdes.linear().col(1).cross(xact.linear().col(1)) +
00461                           xdes.linear().col(2).cross(xact.linear().col(2)));
00462 }
00463 
00464 void JTTaskController::update()
00465 {
00466   // get time
00467   ros::Time time = robot_state_->getTime();
00468   ros::Duration dt = time - last_time_;
00469   last_time_ = time;
00470   ++loop_count_;
00471 
00472   // ======== Measures current arm state
00473 
00474   JointVec q;
00475   chain_.getPositions(q);
00476 
00477   Eigen::Affine3d x;
00478   kin_->fk(q, x);
00479 
00480   Jacobian J;
00481   kin_->jac(q, J);
00482 
00483 
00484   JointVec qdot_raw;
00485   chain_.getVelocities(qdot_raw);
00486   for (int i = 0; i < Joints; ++i)
00487     qdot_filtered_[i] += joint_vel_filter_ * (qdot_raw[i] - qdot_filtered_[i]);
00488   JointVec qdot = qdot_filtered_;
00489   CartVec xdot = J * qdot;
00490 
00491   // ======== Controls to the current pose setpoint
00492 
00493   // Compute x_desi_filtered_ based on pose_command_filter time constant
00494   {
00495     Eigen::Vector3d p0(x_desi_filtered_.translation());
00496     Eigen::Vector3d p1(x_desi_.translation());
00497     Eigen::Quaterniond q0(x_desi_filtered_.linear());
00498     Eigen::Quaterniond q1(x_desi_.linear());
00499     q0.normalize();
00500     q1.normalize();
00501 
00502     tf::Quaternion tf_q0(q0.x(), q0.y(), q0.z(), q0.w());
00503     tf::Quaternion tf_q1(q1.x(), q1.y(), q1.z(), q1.w());
00504     tf::Quaternion tf_q = tf_q0.slerp(tf_q1, pose_command_filter_);
00505 
00506     Eigen::Vector3d p = p0 + pose_command_filter_ * (p1 - p0);
00507     //Eigen::Quaterniond q = q0.slerp(pose_command_filter_, q1);
00508     Eigen::Quaterniond q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z());
00509     //x_desi_filtered_ = q * Eigen::Translation3d(p);
00510     x_desi_filtered_ = Eigen::Translation3d(p) * q;
00511   }
00512   CartVec x_err;
00513   //computePoseError(x, x_desi_, x_err);
00514   computePoseError(x, x_desi_filtered_, x_err);
00515 
00516   // If updating every cycle, set matrix based on end-effector pose (transform)
00517   // Use rotation matrix components of x (Affine3d) to rotate linear and rotational
00518   // components of wrench
00519   if(use_tip_frame_)
00520   { 
00521       St << 
00522           x(0,0),x(0,1),x(0,2),0,0,0,
00523           x(1,0),x(1,1),x(1,2),0,0,0,
00524           x(2,0),x(2,1),x(2,2),0,0,0,
00525           0,0,0,x(0,0),x(0,1),x(0,2),
00526           0,0,0,x(1,0),x(1,1),x(1,2),
00527           0,0,0,x(2,0),x(2,1),x(2,2);
00528       St.transposeInPlace();
00529   }
00530 
00531   // HERE WE CONVERT CALCULATIONS TO THE FRAME IN WHICH GAINS ARE SPECIFIED!
00532   //xdot_desi is not derivative of x_desi_filtered_--it is a velocity controller on the position error
00533   //F = -k_d*xdot - k_p*(x - x_desi)
00534   //xdot_desi = -k_p/k_d(x - x_desi)
00535 
00536   // This can/should be refactored to look like R' * K * R.
00537   CartVec xdot_desi = -1.0 * (Kp.array() / Kd.array()) * (St * x_err).array() ;  // aleeper
00538 
00539 
00540   // Caps the cartesian velocity
00541   // We can consider capping force/torque, but there are multiple places to
00542   // cap and proxy things.
00543   if (vel_saturation_trans_ > 0.0) // negative value disables cap
00544   {
00545     if (fabs(xdot_desi.head<3>().norm()) > vel_saturation_trans_)
00546       xdot_desi.head<3>() *= (vel_saturation_trans_ / xdot_desi.head<3>().norm());
00547   }
00548   if (vel_saturation_rot_ > 0.0) // negative value disables cap
00549   {
00550     if (fabs(xdot_desi.tail<3>().norm()) > vel_saturation_rot_)
00551       xdot_desi.tail<3>() *= (vel_saturation_rot_ / xdot_desi.tail<3>().norm());
00552   }
00553 
00554   CartVec F = Kd.array() * (xdot_desi.array() - (St * xdot).array());  // aleeper
00555 
00556   // HERE WE CONVERT BACK TO THE ROOT FRAME SINCE THE JACOBIAN IS IN ROOT FRAME.
00557   JointVec tau_pose = J.transpose() * St.transpose() * F;
00558 
00559   // ======== J psuedo-inverse and Nullspace computation
00560 
00561   // Computes pseudo-inverse of J
00562   Eigen::Matrix<double,6,6> I6; I6.setIdentity();
00563   Eigen::Matrix<double,6,6> JJt = J * J.transpose();
00564   Eigen::Matrix<double,6,6> JJt_inv;
00565   JJt_inv = JJt.inverse();
00566   Eigen::Matrix<double,6,6> JJt_damped = J * J.transpose() + jacobian_inverse_damping_ * I6;
00567   Eigen::Matrix<double,6,6> JJt_inv_damped;
00568   JJt_inv_damped = JJt_damped.inverse();
00569   Eigen::Matrix<double,Joints,6> J_pinv = J.transpose() * JJt_inv_damped;
00570 
00571   // Computes the nullspace of J
00572   Eigen::Matrix<double,Joints,Joints> I;
00573   I.setIdentity();
00574   Eigen::Matrix<double,Joints,Joints> N = I - J_pinv * J;
00575 
00576   // ======== Posture control
00577 
00578   // Computes the desired joint torques for achieving the posture
00579   JointVec tau_posture;
00580   tau_posture.setZero();
00581   if (use_posture_)
00582   {
00583     JointVec posture_err = q_posture_ - q;
00584     for (size_t j = 0; j < Joints; ++j)
00585     {
00586       if (chain_.getJoint(j)->joint_->type == urdf::Joint::CONTINUOUS)
00587         posture_err[j] = angles::normalize_angle(posture_err[j]);
00588     }
00589 
00590     for (size_t j = 0; j < Joints; ++j) {
00591       if (fabs(q_posture_[j] - 9999) < 1e-5)
00592         posture_err[j] = 0.0;
00593     }
00594 
00595     JointVec qdd_posture = k_posture_ * posture_err;
00596     tau_posture = joint_dd_ff_.array() * (N * qdd_posture).array();
00597   }
00598 
00599   JointVec tau = tau_pose + tau_posture;
00600 
00601   // ======== Torque Saturation
00602   double sat_scaling = 1.0;
00603   for (int i = 0; i < Joints; ++i) {
00604     if (saturation_[i] > 0.0)
00605       sat_scaling = std::min(sat_scaling, fabs(saturation_[i] / tau[i]));
00606   }
00607   JointVec tau_sat = sat_scaling * tau;
00608 
00609   // ======== Apply torques to arm
00610   chain_.addEfforts(tau_sat);
00611 
00612   // ================ END OF CONTROLLER ================
00613 
00614   // ======== Environment stiffness
00615 
00616   CartVec df = F - last_wrench_;
00617   CartVec dx;
00618   computePoseError(last_pose_, x, dx);
00619 
00620   // Just in the Z direction for now
00621 
00622   double Df, Dx;
00623   if (fabs(dx[2]) >= res_position_)
00624     Df = df[2] * res_position_ / fabs(dx[2]);
00625   else
00626     Df = (1. - fabs(dx[2])/res_position_) * last_Df_ + df[2];
00627   if (fabs(df[2]) >= res_force_)
00628     Dx = dx[2] * res_force_ / fabs(df[2]);
00629   else
00630     Dx = (1. - fabs(df[2])/res_force_) * last_Dx_ + dx[2];
00631   last_Df_ = Df;
00632   last_Dx_ = Dx;
00633 
00634   double stiffness, compliance;
00635   if (fabs(dx[2]) >= res_position_)
00636     stiffness = fabs(df[2]) / fabs(dx[2]);
00637   else
00638     stiffness = (1 - fabs(dx[2])/res_position_) * last_stiffness_ + fabs(df[2]) / res_position_;
00639   if (fabs(df[2]) >= res_force_)
00640     compliance = fabs(dx[2]) / fabs(df[2]);
00641   else
00642     compliance = (1 - fabs(df[2])/res_force_) * last_compliance_ + fabs(dx[2]) / res_force_;
00643 
00644   last_pose_ = x;
00645   last_wrench_ = F;
00646   last_stiffness_ = stiffness;
00647   last_compliance_ = compliance;
00648 
00649   if (loop_count_ % 10 == 0)
00650   {
00651     geometry_msgs::PoseStamped::Ptr pose_msg;
00652     geometry_msgs::Twist::Ptr twist_msg;
00653     std_msgs::Float64MultiArray::Ptr q_msg;
00654 
00655     if (pose_msg = pub_x_.allocate()) {  // X
00656       pose_msg->header.stamp = time;
00657       tf::poseEigenToMsg(x, pose_msg->pose);
00658       pub_x_.publish(pose_msg);
00659     }
00660 
00661     if (pose_msg = pub_x_desi_.allocate()) {  // X desi
00662       pose_msg->header.stamp = time;
00663       tf::poseEigenToMsg(x_desi_, pose_msg->pose);
00664       pub_x_desi_.publish(pose_msg);
00665     }
00666 
00667     if (twist_msg = pub_x_err_.allocate()) {  // X err
00668       tf::twistEigenToMsg(x_err, *twist_msg);
00669       pub_x_err_.publish(twist_msg);
00670     }
00671 
00672     if (twist_msg = pub_xd_.allocate()) {  // Xdot
00673       tf::twistEigenToMsg(xdot, *twist_msg);
00674       pub_xd_.publish(twist_msg);
00675     }
00676 
00677     if (twist_msg = pub_xd_desi_.allocate()) {  // Xdot desi
00678       tf::twistEigenToMsg(xdot_desi, *twist_msg);
00679       pub_xd_desi_.publish(twist_msg);
00680     }
00681 
00682     if (twist_msg = pub_wrench_.allocate()) {  // F
00683       tf::twistEigenToMsg(F, *twist_msg);
00684       pub_wrench_.publish(twist_msg);
00685     }
00686 
00687     if (q_msg = pub_tau_.allocate()) {  // tau
00688       for (size_t i = 0; i < Joints; ++i)
00689         q_msg->data[i] = tau[i];
00690       pub_tau_.publish(q_msg);
00691     }
00692 
00693     StateMsg::Ptr state_msg;
00694     if (state_msg = pub_state_.allocate()) {
00695       state_msg->header.stamp = time;
00696       state_msg->x.header.stamp = time;
00697       tf::poseEigenToMsg(x, state_msg->x.pose);
00698       state_msg->x_desi.header.stamp = time;
00699       tf::poseEigenToMsg(x_desi_, state_msg->x_desi.pose);
00700       state_msg->x_desi_filtered.header.stamp = time;
00701       tf::poseEigenToMsg(x_desi_filtered_, state_msg->x_desi_filtered.pose);
00702       tf::twistEigenToMsg(x_err, state_msg->x_err);
00703       tf::twistEigenToMsg(xdot, state_msg->xd);
00704       tf::twistEigenToMsg(xdot_desi, state_msg->xd_desi);
00705       tf::wrenchEigenToMsg(F, state_msg->F);
00706       tf::matrixEigenToMsg(J, state_msg->J);
00707       tf::matrixEigenToMsg(N, state_msg->N);
00708       for (size_t j = 0; j < Joints; ++j) {
00709         state_msg->tau_pose[j] = tau_pose[j];
00710         state_msg->tau_posture[j] = tau_posture[j];
00711         state_msg->tau[j] = tau[j];
00712       }
00713       state_msg->stiffness = stiffness;
00714       state_msg->compliance = compliance;
00715       state_msg->Df = Df / res_position_;
00716       state_msg->Dx = Dx / res_force_;
00717       state_msg->df = df[2];
00718       state_msg->dx = dx[2];
00719       pub_state_.publish(state_msg);
00720     }
00721   }
00722 }
00723 
00724 } //namespace
00725 
00726 PLUGINLIB_EXPORT_CLASS(pr2_manipulation_controllers::JTTaskController,pr2_controller_interface::Controller)
00727 


pr2_manipulation_controllers
Author(s): Kaijen Hsiao, Stu Glaser, Adam Leeper
autogenerated on Mon Oct 6 2014 12:10:46