r2_impedance_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, General Motors.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Darren Earl, Stephen Hart
00032  */
00033 
00034 
00035 #include "r2_impedance_controller.h"
00036 #include <kdl_parser/kdl_parser.hpp>
00037 #include "pluginlib/class_list_macros.h"
00038 #include "tf_conversions/tf_kdl.h"
00039 #include "tf/transform_datatypes.h"
00040 
00041 using namespace std;
00042 using namespace KDL;
00043 using namespace r2_controller_ns;
00044 
00045 
00046 Eigen::MatrixXd WholeBodyCalc::calcPinv(const Eigen::MatrixXd &a){
00047 using namespace Eigen;
00048         // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
00049         const MatrixXd* m;
00050         MatrixXd t;
00051         MatrixXd m_pinv;
00052 
00053         // transpose so SVD decomp can work...
00054         if ( a.rows()<a.cols() ){
00055                 t = a.transpose();
00056                 m = &t;
00057         } else {
00058                 m = &a;
00059         }
00060 
00061         // SVD
00062         //Eigen::JacobiSVD<MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
00063         Eigen::JacobiSVD<MatrixXd> svd = m->jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
00064         Eigen::MatrixXd vSingular = svd.singularValues();
00065         // Build a diagonal matrix with the Inverted Singular values
00066         // The pseudo inverted singular matrix is easy to compute :
00067         // is formed by replacing every nonzero entry by its reciprocal (inversing).
00068         Eigen::MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
00069         for (int iRow =0; iRow<vSingular.rows(); iRow++) {
00070                                                          // Todo : Put epsilon in parameter
00071                 if ( fabs(vSingular(iRow))<=1e-10 ) {
00072                         vPseudoInvertedSingular(iRow,0)=0.;
00073                 }
00074                 else {
00075                         vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
00076                 }
00077         }
00078         // A little optimization here
00079         Eigen::MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
00080         // Pseudo-Inversion : V * S * U'
00081         m_pinv = (svd.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU  ;
00082 
00083         // transpose back if necessary
00084         if ( a.rows()<a.cols() )
00085                 return m_pinv.transpose();
00086         
00087         return m_pinv;
00088 }
00089 
00090 
00091 
00092 
00093 bool R2ImpedanceController::init(pr2_mechanism_model::RobotState* robot_state, ros::NodeHandle& n )
00094 {
00095         boost::lock_guard<boost::mutex> lock(thread_mutex);
00096         node = n;
00097         this->robot_state = robot_state;
00098         kdl_parser::treeFromParam( "robot_description", cc.robot_tree );
00099         
00100         int jnt_size = cc.robot_tree.getNrOfJoints();
00101         robotStateJoints.resize( jnt_size );
00102         
00103         double grav[3];
00104         bool found = 1;
00105         found &= n.getParam("/gravity/x", grav[0] );
00106         found &= n.getParam("/gravity/y", grav[1] );
00107         found &= n.getParam("/gravity/z", grav[2] );
00108         if( found ){
00109                 cc.init(grav);
00110         } else {
00111                 double default_grav[] = { 0, 0, -9.8 };
00112                 cc.init( default_grav );
00113         }
00114         
00115         {  //map RobotState joints to KDL::Tree joints
00116                 const SegmentMap& sm = cc.robot_tree.getSegments();
00117                 int x=0;
00118                 for( SegmentMap::const_iterator i = sm.begin(); i!=sm.end(); ++i ){
00119                         const Segment& seg = i->second.segment;
00120                         const Joint& joint = seg.getJoint();
00121                         if( joint.getType() == Joint::None )
00122                                 continue;
00123                         cc.name2idx[ joint.getName() ] = x;
00124                         cc.idx2name[x] = joint.getName();
00125                         pr2_mechanism_model::JointState* js = robot_state->getJointState( joint.getName() );
00126                         robotStateJoints[x] = js;
00127                         cc.jntsUpperLimit[x] = js->joint_->limits->upper;
00128                         cc.jntsLowerLimit[x] = js->joint_->limits->lower;
00129                         cc.jntsCenterPoint[x] = (cc.jntsUpperLimit[x] + cc.jntsLowerLimit[x])*.5;
00130                         cc.jntsUpperLimit[x] -= .01;
00131                         cc.jntsLowerLimit[x] += .01;
00132                         
00133                         
00134                         
00135                         cc.desired[x] = cc.jntsCenterPoint[x];
00136                         x++;
00137                         
00138                 }
00139         }
00140         load_params(); // need to do this after name2idx is filled
00141         init_ros_msgs();
00142         for( int x=0; x<jnt_size; ++x ){
00143                 cc.K[x] = cc.K_high[x];
00144                 cc.D[x] = cc.D_high[x];
00145         }
00146         
00147         {
00148                 const SegmentMap& sm = cc.robot_tree.getSegments();
00149                 int x=0;
00150                 for( SegmentMap::const_iterator i= sm.begin(); i!=sm.end(); ++i ){
00151                         const Segment& seg = i->second.segment;
00152                         const Joint& joint = seg.getJoint();
00153                         if( joint.getType() == Joint::None )
00154                                 continue;
00155                         int idx = cc.name2idx[ joint.getName() ];
00156                         cout << "\t" << joint.getName();
00157                         cout << " " << idx;
00158                         cout << " "<< cc.K_high[x] << " " << cc.K_low[x];
00159                         cout << " " << cc.D_high[x] << " " << cc.D_low[x];
00160                         cout << " " << cc.K[x] << " " << cc.D[x] << endl;
00161                         
00162                         ++x;
00163                 }
00164         }
00165         
00166         return true;
00167 }
00168 void R2ImpedanceController::CtrlCalc::init(double gravity[3]){
00169         rne_calc.reset(new TreeIdSolver_RNE( robot_tree, KDL::Vector(gravity[0], gravity[1], gravity[2]) ));
00170         
00171 
00172         wbc = WholeBodyCalc( robot_tree );
00173         jnt_size = robot_tree.getNrOfJoints();
00174         
00175         D_high.resize( jnt_size );
00176         D_low.resize( jnt_size );
00177         K_high.resize( jnt_size );
00178         K_low.resize( jnt_size );
00179         D.resize( jnt_size );
00180         K.resize( jnt_size );
00181         desired.resize( jnt_size );
00182         desiredVel.resize( jnt_size );
00183         joint_pos_control.resize( jnt_size );
00184         joint_vel_control.resize( jnt_size );
00185         
00186         treeJnts.resize( jnt_size );
00187         treeJntsVel.resize( jnt_size );
00188         treeJntsAvg.resize( jnt_size );
00189         treeJntsVelAvg.resize(jnt_size);
00190         
00191         jntsUpperLimit.resize( jnt_size );
00192         jntsLowerLimit.resize( jnt_size );
00193         jntsCenterPoint.resize( jnt_size );
00194         torques.resize(jnt_size);
00195         idx2name.resize(jnt_size);
00196         cartK_left.resize(6);
00197         cartD_left.resize(6);
00198         cartK_right.resize(6);
00199         cartD_right.resize(6);
00200         for( int x=0; x<jnt_size; ++x ){
00201                 D_high[x]     = 0;
00202                 D_low[x]      = 0;
00203                 K_high[x]     = 0;
00204                 K_low[x]      = 0;
00205                 D[x]          = 0;
00206                 K[x]          = 0;
00207                 desired[x]    = 0;
00208                 desiredVel[x] = 0;
00209                 torques(x)    = 0;
00210                 treeJnts[x]   = 0;
00211                 treeJntsVel[x]= 0;
00212                 
00213                 joint_pos_control[x] = true;
00214                 joint_vel_control[x] = false;
00215         }
00216     left_cart = false;
00217         right_cart = false;
00218         neck_cart = false;
00219         
00220         //root_name = "robot_base";
00221     root_name = "/r2/robot_reference";
00222         
00223 
00224 }
00225 
00227 static void append( vector<string>& first, const vector<string>& second ){
00228         first.insert( first.end(), second.begin(), second.end() );
00229 }
00231 static vector<string> createNames( const string& base, int count ){
00232         vector<string> results(count);
00233         for( int x=0; x<count; ++x ){
00234                 stringstream ss;
00235                 ss << base << x;
00236                 results[x] = ss.str();
00237         }
00238         return results;
00239 } 
00240 
00241 static vector<string> createHandNames( const string& a ){
00242         vector<string> hand;
00243     append( hand, createNames( a+"/thumb/joint", 4 ));
00244     append( hand, createNames( a+"/index/joint", 4 ));
00245     append( hand, createNames( a+"/middle/joint", 4 ));
00246     append( hand, createNames( a+"/ring/joint", 3));
00247     append( hand, createNames( a+"/little/joint", 3 ));
00248 
00249         return hand;
00250 }
00251 
00252 void R2ImpedanceController::load_params(){
00253         
00254         //gains are laid out in the yaml file in these sections
00255     vector<string> right = createNames( "/r2/right_arm/joint", 7 );
00256     vector<string> rightHand = createHandNames( "/r2/right_arm/hand" );
00257     vector<string> left = createNames( "/r2/left_arm/joint", 7 );
00258     vector<string> leftHand = createHandNames( "/r2/left_arm/hand" );
00259     vector<string> waist;       waist.push_back( "/r2/waist/joint0" );
00260     vector<string> neck = createNames( "/r2/neck/joint", 3 );
00261         vector<string> rCart = createNames( "right/x",6);
00262         vector<string> lCart = createNames( "left/x",6);
00263         
00265         class Helper{
00266                 std::map<string, int>& map;
00267                 vector<double>& v;
00268                 public:
00269                 Helper( std::map<string, int>& Map, std::vector<double>& V  ):map(Map), v(V){};
00270                 
00271                 void operator()( const vector<string>& names, const vector<double>& values ){
00272                         int size = names.size();
00273                         for( int x=0; x< size; ++x ){
00274                                 v[ map[ names[x] ] ] = values[x];
00275                         }
00276                 }
00277         };
00278         Helper Khigh( cc.name2idx, cc.K_high );
00279         Helper Dhigh( cc.name2idx, cc.D_high );
00280         Helper Klow( cc.name2idx, cc.K_low );
00281         Helper Dlow( cc.name2idx, cc.D_low );
00282         
00283         Khigh( waist, getGainParams( waist, "Waist_Joint_Stiffness_High" ) );
00284         Dhigh( waist, getGainParams( waist, "Waist_Joint_Damping_High" ));
00285         Klow( waist, getGainParams( waist, "Waist_Joint_Stiffness_Low" ) );
00286         Dlow( waist, getGainParams( waist, "Waist_Joint_Damping_Low" ) );
00287         
00288         Khigh( neck, getGainParams( neck, "Neck_Joint_Stiffness" ));
00289         Dhigh( neck, getGainParams( neck, "Neck_Joint_Damping" ));
00290         Klow( neck, getGainParams( neck, "Neck_Joint_Stiffness" ));
00291         Dlow( neck, getGainParams( neck, "Neck_Joint_Damping" ));
00292         
00293         Khigh( left, getGainParams( left, "LeftArm_Joint_Stiffness_High" ) );
00294         Dhigh( left, getGainParams( left, "LeftArm_Joint_Damping_High" ) );
00295         Khigh( leftHand, getGainParams( leftHand, "LeftHand_Joint_Stiffness_High" ));
00296         Dhigh( leftHand, getGainParams( leftHand, "LeftHand_Joint_Damping_High" ));
00297         Klow( left, getGainParams( left, "LeftArm_Joint_Stiffness_Low" ));
00298         Dlow( left, getGainParams( left, "LeftArm_Joint_Damping_Low" ));
00299         Klow( leftHand, getGainParams( leftHand, "LeftHand_Joint_Stiffness_Low" ));
00300         Dlow( leftHand, getGainParams( leftHand, "LeftHand_Joint_Damping_Low" ));
00301         
00302         Khigh( right, getGainParams( right, "RightArm_Joint_Stiffness_High" ) );
00303         Dhigh( right, getGainParams( right, "RightArm_Joint_Damping_High" ) );
00304         Khigh( rightHand, getGainParams( rightHand, "RightHand_Joint_Stiffness_High" ));
00305         Dhigh( rightHand, getGainParams( rightHand, "RightHand_Joint_Damping_High" ));
00306         Klow( right, getGainParams( right, "RightArm_Joint_Stiffness_Low" ));
00307         Dlow( right, getGainParams( right, "RightArm_Joint_Damping_Low" ));
00308         Klow( rightHand, getGainParams( rightHand, "RightHand_Joint_Stiffness_Low" ));
00309         Dlow( rightHand, getGainParams( rightHand, "RightHand_Joint_Damping_Low" ));
00310         
00311         
00312         cc.cartK_left = getGainParams( lCart, "Left_Cart_Stiffness" );
00313         cc.cartD_left = getGainParams( lCart, "Left_Cart_Damping" );
00314         cc.cartK_right = getGainParams( rCart, "Right_Cart_Stiffness" );
00315         cc.cartD_right = getGainParams( rCart, "Right_Cart_Damping" );
00316         
00317 };
00318 
00319 vector<double> R2ImpedanceController::getGainParams(const std::vector<string>& param_names, const string& param_name) {
00320         int param_size = param_names.size();
00321         vector<double> params; params.reserve( param_size );
00322         using namespace XmlRpc;
00323         XmlRpc::XmlRpcValue param_vals;
00324         if (!node.getParam(param_name, param_vals)) {
00325                 ROS_ERROR("No %s given. (namespace: %s)", param_name.c_str(), node.getNamespace().c_str());
00326                 assert(false);
00327                 return params;
00328         }
00329         if (param_size != (int)(param_vals.size())) {
00330                 ROS_ERROR("Incorrect number of %s specified.  (namespace: %s), needs: %d, has %d", param_name.c_str(), node.getNamespace().c_str(), param_size, param_vals.size());
00331                 assert(false);
00332                 return params;
00333         }
00334         for (int i = 0; i < param_size; ++i) {
00335                 XmlRpcValue &param_value = param_vals[i];
00336                 if (param_value.getType() != XmlRpcValue::TypeDouble) {
00337                         ROS_ERROR("Array of params names should contain all doubles.  (namespace: %s)",
00338                                 node.getNamespace().c_str());
00339                         assert(false);
00340                         return params;
00341                 }
00342                 params.push_back( (double)param_value );
00343         }
00344         return params;
00345 }
00346 
00347 
00348 void R2ImpedanceController::init_ros_msgs(){
00349         
00350         left_tip_pose_publisher.reset( new realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped>(node, "left/pose_state", 5));
00351         right_tip_pose_publisher.reset(new realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped>(node, "right/pose_state", 5));
00352 
00353         left_pose_error_publisher.reset( new realtime_tools::RealtimePublisher<geometry_msgs::Twist>(node, "left/twist_error", 5));
00354         right_pose_error_publisher.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist>(node, "right/twist_error", 5));
00355 
00356         // subscribe to pose commands
00357         joint_command_sub  = node.subscribe("joint_commands",  5, &R2ImpedanceController::joint_command,  this);
00358         // backwards compatability
00359         left_joint_command_sub  = node.subscribe("left_arm/joint_command",  5, &R2ImpedanceController::joint_left_command,  this);
00360         right_joint_command_sub = node.subscribe("right_arm/joint_command", 5, &R2ImpedanceController::joint_right_command, this);
00361         neck_joint_command_sub  = node.subscribe("neck/joint_command",      5, &R2ImpedanceController::joint_neck_command,  this);
00362         waist_joint_command_sub = node.subscribe("waist/joint_command",     5, &R2ImpedanceController::joint_waist_command, this);
00363 
00364         //left_pose_command_sub = node.subscribe("left/pose_command", 5, &R2ImpedanceController::pose_left_command, this);
00365         left_pose_command_sub.subscribe(node, "left/pose_command", 1);
00366         left_pose_command_filter.reset(new tf::MessageFilter<geometry_msgs::PoseStamped>(left_pose_command_sub, tfListener, cc.root_name, 10, node ));
00367         left_pose_command_filter->registerCallback(boost::bind(&R2ImpedanceController::pose_left_command, this, _1));
00368 
00369         right_pose_command_sub.subscribe(node, "right/pose_command", 1);
00370         right_pose_command_filter.reset(new tf::MessageFilter<geometry_msgs::PoseStamped>(right_pose_command_sub, tfListener, cc.root_name, 10, node));
00371         right_pose_command_filter->registerCallback(boost::bind(&R2ImpedanceController::pose_right_command, this, _1));
00372 
00373         left_pose_vel_command_sub.subscribe(node, "left/pose_twist_command", 1);
00374         left_pose_vel_command_filter.reset(new tf::MessageFilter<nasa_r2_common_msgs::PoseTwistStamped>(left_pose_vel_command_sub, tfListener, cc.root_name, 10, node ));
00375         left_pose_vel_command_filter->registerCallback(boost::bind(&R2ImpedanceController::pose_vel_left_command, this, _1));
00376 
00377         right_pose_vel_command_sub.subscribe(node, "right/pose_twist_command", 1);
00378         right_pose_vel_command_filter.reset(new tf::MessageFilter<nasa_r2_common_msgs::PoseTwistStamped>(right_pose_vel_command_sub, tfListener, cc.root_name, 10, node ));
00379         right_pose_vel_command_filter->registerCallback(boost::bind(&R2ImpedanceController::pose_vel_right_command, this, _1));
00380 
00381 
00382         
00383         set_gains_sub = node.subscribe("set_gains", 3, &R2ImpedanceController::set_gains, this );
00384         gains_publisher.reset( new realtime_tools::RealtimePublisher<nasa_r2_common_msgs::Gains>(node, "gains", 5 ));
00385         
00386         
00387         srv_set_joint_mode = node.advertiseService("set_joint_mode", &R2ImpedanceController::set_joint_mode, this);
00388         srv_set_tip_name   = node.advertiseService("set_tip_name",   &R2ImpedanceController::set_tip_name,   this);
00389 
00390         srv_set_power = node.advertiseService("power", &R2ImpedanceController::set_power, this);
00391         srv_set_servo = node.advertiseService("servo", &R2ImpedanceController::set_servo, this);
00392         
00393         
00394         
00395 }
00396 //since JointState actually names the joints, we don't need seperate functions for left/right/neck/waist joints
00397 // if desired, could check for illegal names ( left shouldn't change right values, etc ) but really should just
00398 // consolidate to one topic 
00399 void R2ImpedanceController::joint_left_command(const sensor_msgs::JointState::ConstPtr& msg ){ joint_command(msg);}
00400 void R2ImpedanceController::joint_right_command(const sensor_msgs::JointState::ConstPtr& msg ){joint_command(msg);}
00401 void R2ImpedanceController::joint_neck_command(const sensor_msgs::JointState::ConstPtr& msg ){joint_command(msg);}
00402 void R2ImpedanceController::joint_waist_command(const sensor_msgs::JointState::ConstPtr& msg ){joint_command(msg);}
00403 void R2ImpedanceController::pose_left_command(const geometry_msgs::PoseStamped::ConstPtr& msg){
00404         Frame f = transformPoseMsg(msg);
00405         boost::lock_guard<boost::mutex> lock(thread_mutex);
00406         cc.leftCmd[0] = f.p[0];
00407         cc.leftCmd[1] = f.p[1];
00408         cc.leftCmd[2] = f.p[2];
00409         double& w = cc.leftCmd[3];
00410         double& x = cc.leftCmd[4];
00411         double& y = cc.leftCmd[5];
00412         double& z = cc.leftCmd[6];
00413         f.M.GetQuaternion(x,y,z,w);
00414         cc.left_cart_vel = false;
00415 }
00416 void R2ImpedanceController::pose_right_command(const geometry_msgs::PoseStamped::ConstPtr& msg){
00417         Frame f = transformPoseMsg(msg);
00418         boost::lock_guard<boost::mutex> lock(thread_mutex);
00419         cc.rightCmd[0] = f.p[0];
00420         cc.rightCmd[1] = f.p[1];
00421         cc.rightCmd[2] = f.p[2];
00422         double& w = cc.rightCmd[3];
00423         double& x = cc.rightCmd[4];
00424         double& y = cc.rightCmd[5];
00425         double& z = cc.rightCmd[6];
00426         f.M.GetQuaternion(x,y,z,w);
00427         cc.right_cart_vel = false;
00428 }
00429 void R2ImpedanceController::pose_vel_left_command(const nasa_r2_common_msgs::PoseTwistStamped::ConstPtr& msg ){
00430         pose_vel_command_inner( msg, cc.leftCmd, cc.leftVelCmd, cc.left_cart_vel );
00431 }
00432 void R2ImpedanceController::pose_vel_right_command(const nasa_r2_common_msgs::PoseTwistStamped::ConstPtr& msg ){
00433         pose_vel_command_inner( msg, cc.rightCmd, cc.rightVelCmd, cc.right_cart_vel );
00434 }
00435 void R2ImpedanceController::pose_vel_command_inner(     const nasa_r2_common_msgs::PoseTwistStamped::ConstPtr& msg,
00436                                                         Eigen::Matrix<double,7,1>& cmd,
00437                                                         KDL::Twist& velCmd,
00438                                                         bool& cart_vel)
00439 {
00440         geometry_msgs::PoseStamped p;
00441         p.header = msg->header;
00442         p.pose = msg->pose;
00443         
00444         Frame f;
00445         tf::Stamped<tf::Pose> pose_stamped;
00446         tf::poseStampedMsgToTF(p, pose_stamped);
00447         
00448         //directly from tf.cpp::line 1448, Transformer::transformPose
00449         // need transform for next steps
00450         tf::StampedTransform transform;
00451         tfListener.lookupTransform( cc.root_name, pose_stamped.frame_id_, pose_stamped.stamp_, transform );
00452         pose_stamped.setData( transform * pose_stamped );
00453         pose_stamped.stamp_ = transform.stamp_;
00454         pose_stamped.frame_id_ = cc.root_name;
00455         tf::PoseTFToKDL(pose_stamped, f );
00456         
00457         tf::Quaternion quat = transform.getRotation();
00458         transform.setIdentity();
00459         transform.setRotation( quat );
00460         const geometry_msgs::Vector3& linear = msg->twist.linear;
00461         const geometry_msgs::Vector3& angular = msg->twist.angular;
00462         tf::Vector3 cartVel(linear.x, linear.y, linear.z );
00463         tf::Vector3 cartAngVel( angular.x, angular.y, angular.z );
00464         tf::Vector3 r_cartVel = transform * cartVel;
00465         tf::Vector3 r_cartAngVel = transform * cartAngVel;
00466         
00467         boost::lock_guard<boost::mutex> lock(thread_mutex);
00468         cmd[0] = f.p[0];
00469         cmd[1] = f.p[1];
00470         cmd[2] = f.p[2];
00471         double& w = cmd[3];
00472         double& x = cmd[4];
00473         double& y = cmd[5];
00474         double& z = cmd[6];
00475         f.M.GetQuaternion(x,y,z,w);
00476         
00477         velCmd.vel[0] = r_cartVel[0];
00478         velCmd.vel[1] = r_cartVel[1];
00479         velCmd.vel[2] = r_cartVel[2];
00480         velCmd.rot[0] = r_cartAngVel[0];
00481         velCmd.rot[1] = r_cartAngVel[1];
00482         velCmd.rot[2] = r_cartAngVel[2];
00483         cart_vel=true;
00484 }
00485 KDL::Frame R2ImpedanceController::transformPoseMsg(const geometry_msgs::PoseStamped::ConstPtr& msg){
00486         Frame frame;
00487         tf::Stamped<tf::Pose> pose_stamped;
00488         tf::poseStampedMsgToTF(*msg, pose_stamped);
00489         tfListener.transformPose( cc.root_name, pose_stamped, pose_stamped );
00490         tf::PoseTFToKDL(pose_stamped, frame );
00491         
00492         return frame;
00493 }
00494 void R2ImpedanceController::set_gains(const nasa_r2_common_msgs::Gains::ConstPtr& msg ){
00495         boost::lock_guard<boost::mutex> lock(thread_mutex);
00496         size_t size = msg->joint_names.size();
00497         if( size != msg->K.size() || size!= msg->D.size() ){
00498                 cout << "WARNING: discarding Gains msg due to mismatch in size.  joint_names: " << size << " values: " << msg->K.size() <<" " <<msg->D.size() << endl;
00499                 return;
00500         }
00502         static const string cart_left_names[] = { "cart_left_0", 
00503                                                                                           "cart_left_1",
00504                                                                                           "cart_left_2",
00505                                                                                           "cart_left_3",
00506                                                                                           "cart_left_4",
00507                                                                                           "cart_left_5" };
00508         static const string cart_right_names[] = {  "cart_right_0",
00509                                                                                                 "cart_right_1",
00510                                                                                                 "cart_right_2",
00511                                                                                                 "cart_right_3",
00512                                                                                                 "cart_right_4",
00513                                                                                                 "cart_right_5"};
00514          
00515         
00516         
00517         for( size_t x=0; x<size; ++x ){
00518                 map< string, int>::const_iterator i = cc.name2idx.find( msg->joint_names[x] );
00519                 if( i != cc.name2idx.end() ){
00520                         cc.K[i->second] = msg->K[x];
00521                         cc.D[i->second] = msg->D[x];
00522                 } else {
00523                         for( int y=0; y<6; ++y ){
00524                                 if( msg->joint_names[x] == cart_left_names[y] ){
00525                                         cc.cartK_left[y] = msg->K[x];
00526                                         cc.cartD_left[y] = msg->D[x];
00527                                         break;
00528                                 }
00529                                 if( msg->joint_names[x] == cart_right_names[y] ){
00530                                         cc.cartK_right[y] = msg->K[x];
00531                                         cc.cartD_right[y] = msg->D[x];
00532                                         break;
00533                                 }
00534                         }
00535                 }
00536         }
00537         
00538 }
00539 
00540 
00541 void R2ImpedanceController::joint_command(const sensor_msgs::JointState::ConstPtr& msg){
00542         boost::lock_guard<boost::mutex> lock(thread_mutex);
00543         
00544         bool set_position = !msg->position.empty();
00545         bool set_velocity = !msg->velocity.empty();
00546         
00547         if( set_position && msg->position.size() != msg->name.size() ){
00548                 ROS_DEBUG("bad JointState msg: position and name field size mismatch");
00549                 return;
00550         }
00551         if( set_velocity && msg->velocity.size() != msg->name.size() ){
00552                 ROS_DEBUG("bad JointState msg: velocity and name field size mismatch");
00553                 return;
00554         }
00555         if( set_position ){
00556                 for( size_t x=0; x< msg->name.size(); ++x ){
00557                         const string& name = msg->name[x];
00558                         double p_value = msg->position[x];
00559                         double v_value = 0;
00560                         bool v_desired = false;
00561                         if( set_velocity ){
00562                                 v_value = msg->velocity[x];
00563                                 v_desired = true;
00564                         } 
00565                         
00566                         map<string,int>::const_iterator i = cc.name2idx.find(name);
00567                         if( i != cc.name2idx.end() ){
00568                                 if( p_value > cc.jntsUpperLimit[ i->second ] )
00569                                         p_value = cc.jntsUpperLimit[ i->second ];
00570                                 if( p_value < cc.jntsLowerLimit[ i->second ] )
00571                                         p_value = cc.jntsLowerLimit[ i->second ];
00572                         }
00573                         joint_command_entry( name, p_value, cc.desired );
00574                         joint_command_entry( name, v_value, cc.desiredVel );
00575                         // since vector<bool> is odd, use vector<int> instead for 
00576                         // vector of bools, requires our bool value to turn into an int
00577                         joint_command_entry( name, (true), cc.joint_pos_control );
00578                         joint_command_entry( name, (v_desired), cc.joint_vel_control );
00579                 }
00580         } else {
00581                 for( size_t x=0; x< msg->velocity.size(); ++x ){
00582                         const string& name = msg->name[x];
00583                         const double value = msg->velocity[x];
00584                         
00585                         joint_command_entry(name, false, cc.joint_pos_control );
00586                         joint_command_entry(name, true, cc.joint_vel_control );
00587                         joint_command_entry( name, value, cc.desiredVel );
00588                 }
00589         }
00590 };
00591 void R2ImpedanceController::joint_command_entry( const string& name, double value, vector<double>& desired ){
00592         //need to handle non-independent joint differently
00593     const static string not_independent[] = { "/r2/left_arm/hand/index/joint3",
00594                                               "/r2/left_arm/hand/middle/joint3",
00595                                               "/r2/left_arm/hand/ring/joint1",
00596                                               "/r2/left_arm/hand/ring/joint2",
00597                                               "/r2/left_arm/hand/little/joint1",
00598                                               "/r2/left_arm/hand/little/joint2",
00599                                               "/r2/right_arm/hand/index/joint3",
00600                                               "/r2/right_arm/hand/middle/joint3",
00601                                               "/r2/right_arm/hand/ring/joint1",
00602                                               "/r2/right_arm/hand/ring/joint2",
00603                                               "/r2/right_arm/hand/little/joint1",
00604                                               "/r2/right_arm/hand/little/joint2" };
00605         const static int ni_count = 12;
00606         bool found = false;
00607         for( int x=0; x< ni_count; ++x ){
00608                 if( name == not_independent[x] ){
00609                         found = true;
00610                         break;
00611                 }
00612         }
00613         if( found )     
00614                 return;
00615         
00616         map<string,int>::const_iterator i = cc.name2idx.find(name);
00617         if( i != cc.name2idx.end() ){
00618                 desired[ i->second ] = value;
00619         }
00620         if( name == "/r2/left_arm/hand/index/joint2" ){
00621                 desired[ cc.name2idx[ "/r2/left_arm/hand/index/joint3" ] ] = value;
00622         } else if( name == "/r2/left_arm/hand/middle/joint2" ){
00623                 desired[ cc.name2idx[ "/r2/left_arm/hand/middle/joint3" ] ] = value;
00624         } else if( name == "/r2/left_arm/hand/ring/joint0" ){
00625                 desired[ cc.name2idx[ "/r2/left_arm/hand/ring/joint0" ] ] = value*.5;           //when value is bool, value*.5 == false, which is a problem
00626                 desired[ cc.name2idx[ "/r2/left_arm/hand/ring/joint1" ] ] = value*.5;
00627                 desired[ cc.name2idx[ "/r2/left_arm/hand/ring/joint2" ] ] = value*.5;
00628         } else if( name == "/r2/left_arm/hand/little/joint0" ){
00629                 desired[ cc.name2idx[ "/r2/left_arm/hand/little/joint0" ] ] = value*.5;
00630                 desired[ cc.name2idx[ "/r2/left_arm/hand/little/joint1" ] ] = value*.5;
00631                 desired[ cc.name2idx[ "/r2/left_arm/hand/little/joint2" ] ] = value*.5;
00632         } else if( name == "/r2/right_arm/hand/index/joint2" ){
00633                 desired[ cc.name2idx[ "/r2/right_arm/hand/index/joint3" ] ] = value;
00634         } else if( name == "/r2/right_arm/hand/middle/joint2" ){
00635                 desired[ cc.name2idx[ "/r2/right_arm/hand/middle/joint3" ] ] = value;
00636         } else if( name == "/r2/right_arm/hand/ring/joint0" ){
00637                 desired[ cc.name2idx[ "/r2/right_arm/hand/ring/joint0" ] ] = value*.5;
00638                 desired[ cc.name2idx[ "/r2/right_arm/hand/ring/joint1" ] ] = value*.5;
00639                 desired[ cc.name2idx[ "/r2/right_arm/hand/ring/joint2" ] ] = value*.5;
00640         } else if( name == "/r2/right_arm/hand/little/joint0" ){
00641                 desired[ cc.name2idx[ "/r2/right_arm/hand/little/joint0" ] ] = value*.5;
00642                 desired[ cc.name2idx[ "/r2/right_arm/hand/little/joint1" ] ] = value*.5;
00643                 desired[ cc.name2idx[ "/r2/right_arm/hand/little/joint2" ] ] = value*.5;
00644         }
00645 }
00646 void R2ImpedanceController::joint_command_entry( const string& name, bool value, vector<int>& desired ){
00647         //need to handle non-independent joint differently
00648     const static string not_independent[] = { "/r2/left_arm/hand/index/joint3",
00649                                               "/r2/left_arm/hand/middle/joint3",
00650                                               "/r2/left_arm/hand/ring/joint1",
00651                                               "/r2/left_arm/hand/ring/joint2",
00652                                               "/r2/left_arm/hand/little/joint1",
00653                                               "/r2/left_arm/hand/little/joint2",
00654                                               "/r2/right_arm/hand/index/joint3",
00655                                               "/r2/right_arm/hand/middle/joint3",
00656                                               "/r2/right_arm/hand/ring/joint1",
00657                                               "/r2/right_arm/hand/ring/joint2",
00658                                               "/r2/right_arm/hand/little/joint1",
00659                                               "/r2/right_arm/hand/little/joint2" };
00660         const static int ni_count = 12;
00661         bool found = false;
00662         for( int x=0; x< ni_count; ++x ){
00663                 if( name == not_independent[x] ){
00664                         found = true;
00665                         break;
00666                 }
00667         }
00668         if( found )     
00669                 return;
00670         
00671         map<string,int>::const_iterator i = cc.name2idx.find(name);
00672         if( i != cc.name2idx.end() ){
00673                 desired[ i->second ] = value;
00674         }
00675         if( name == "/r2/left_arm/hand/index/joint2" ){
00676                 desired[ cc.name2idx[ "/r2/left_arm/hand/index/joint3" ] ] = value;
00677         } else if( name == "/r2/left_arm/hand/middle/joint2" ){
00678                 desired[ cc.name2idx[ "/r2/left_arm/hand/middle/joint3" ] ] = value;
00679         } else if( name == "/r2/left_arm/hand/ring/joint0" ){
00680                 desired[ cc.name2idx[ "/r2/left_arm/hand/ring/joint0" ] ] = value;              //when value is bool, value*.5 == false, which is a problem
00681                 desired[ cc.name2idx[ "/r2/left_arm/hand/ring/joint1" ] ] = value;
00682                 desired[ cc.name2idx[ "/r2/left_arm/hand/ring/joint2" ] ] = value;
00683         } else if( name == "/r2/left_arm/hand/little/joint0" ){
00684                 desired[ cc.name2idx[ "/r2/left_arm/hand/little/joint0" ] ] = value;
00685                 desired[ cc.name2idx[ "/r2/left_arm/hand/little/joint1" ] ] = value;
00686                 desired[ cc.name2idx[ "/r2/left_arm/hand/little/joint2" ] ] = value;
00687         } else if( name == "/r2/right_arm/hand/index/joint2" ){
00688                 desired[ cc.name2idx[ "/r2/right_arm/hand/index/joint3" ] ] = value;
00689         } else if( name == "/r2/right_arm/hand/middle/joint2" ){
00690                 desired[ cc.name2idx[ "/r2/right_arm/hand/middle/joint3" ] ] = value;
00691         } else if( name == "/r2/right_arm/hand/ring/joint0" ){
00692                 desired[ cc.name2idx[ "/r2/right_arm/hand/ring/joint0" ] ] = value;
00693                 desired[ cc.name2idx[ "/r2/right_arm/hand/ring/joint1" ] ] = value;
00694                 desired[ cc.name2idx[ "/r2/right_arm/hand/ring/joint2" ] ] = value;
00695         } else if( name == "/r2/right_arm/hand/little/joint0" ){
00696                 desired[ cc.name2idx[ "/r2/right_arm/hand/little/joint0" ] ] = value;
00697                 desired[ cc.name2idx[ "/r2/right_arm/hand/little/joint1" ] ] = value;
00698                 desired[ cc.name2idx[ "/r2/right_arm/hand/little/joint2" ] ] = value;
00699         }
00700 }
00701 
00702 
00703 bool R2ImpedanceController::set_joint_mode(nasa_r2_common_msgs::SetJointMode::Request  &req,  nasa_r2_common_msgs::SetJointMode::Response &res ){ 
00704         boost::lock_guard<boost::mutex> lock(thread_mutex);
00705         class Local{
00706                 public:
00707                 static void setDesired( vector<double>& desired, const vector<double>& current, const TreeChain& tc ){
00708                         int size = tc.size();
00709                         for( int x=0; x< size; ++x ){
00710                                 int idx = tc.treeIdx(x);
00711                                 desired[idx] = current[idx];
00712                         }
00713                 }
00714         };
00715         if( req.arm_name == "left" && cc.left_cart ){
00716                 cc.left_cart = false;
00717                 Local::setDesired( cc.desired, cc.treeJnts, cc.left );
00718         }
00719         if( req.arm_name == "right" && cc.right_cart ){
00720                 cc.right_cart = false;
00721                 Local::setDesired( cc.desired, cc.treeJnts, cc.right );
00722         }
00723         if( req.arm_name == "neck" && cc.neck_cart ){
00724                 cc.neck_cart = false;
00725                 Local::setDesired( cc.desired, cc.treeJnts, cc.neck );
00726         }
00727                 
00728         res.result = true;
00729         cc.reactivate();
00730         
00731         return res.result;
00732 }
00733 bool R2ImpedanceController::set_tip_name(nasa_r2_common_msgs::SetTipName::Request &req,  nasa_r2_common_msgs::SetTipName::Response &res ){
00734         boost::lock_guard<boost::mutex> lock(thread_mutex);
00735         if( req.arm_name == "left" ){
00736                 cc.left.init( cc.robot_tree, cc.root_name, req.tip_name, cc.cartK_left, cc.cartD_left );
00737                 cc.activate( cc.left, cc.left_cart, cc.leftCmd );
00738         res.result = true;
00739         } else if( req.arm_name == "right" ){
00740                 cc.right.init( cc.robot_tree, cc.root_name, req.tip_name, cc.cartK_right, cc.cartD_right );
00741                 cc.activate( cc.right, cc.right_cart, cc.rightCmd );
00742         res.result = true;
00743         } else {
00744                 res.result = false;
00745         }
00746         return res.result;
00747 }
00748 bool R2ImpedanceController::set_power(nasa_r2_common_msgs::Power::Request &req,  nasa_r2_common_msgs::Power::Response &res ){ res.status = true; return true;}
00749 bool R2ImpedanceController::set_servo(nasa_r2_common_msgs::Servo::Request &req,  nasa_r2_common_msgs::Servo::Response &res ){ res.status = true; return true;}
00750 
00751 void R2ImpedanceController::CtrlCalc::activate( TreeChain& tc, bool& flag, Eigen::Matrix<double,7,1>& pose_cmd ){
00752         flag = true;
00753         
00754         for( int x=0; x<tc.size(); ++x ){
00755                 int idx = tc.treeIdx(x);
00756                 K[ idx ] = K_low[ idx ];
00757                 D[ idx ] = D_low[ idx ];
00758         }
00759         pose_cmd = tc.fk( treeJnts );
00760 }
00761 void R2ImpedanceController::CtrlCalc::reactivate(){
00762         for( int x=0; x<jnt_size; ++x ){
00763                 K[x] = K_high[x];
00764                 D[x] = D_high[x];
00765         }
00766         bool dummy_flag;
00767         Eigen::Matrix<double,7,1> dummy_cmd;
00768         if( left_cart )
00769                 activate( left, dummy_flag, dummy_cmd );
00770         if( right_cart )
00771                 activate( right, dummy_flag, dummy_cmd );
00772         if( neck_cart )
00773                 activate( neck, dummy_flag, dummy_cmd );
00774 }
00775 
00776 
00777 KDL::JntArray R2ImpedanceController::CtrlCalc::jointKCmd(const vector<double>& q){
00778         
00779         KDL::JntArray result( jnt_size );
00780         for( int x=0; x< jnt_size; ++x ){
00781                 if( joint_pos_control[x] )
00782                         result(x) = (K[x] * (desired[x] - q[x]));
00783                 else
00784                         result(x) = 0;
00785         }
00786         return result;
00787 }
00788 KDL::JntArray R2ImpedanceController::CtrlCalc::jointDCmd(const vector<double>& dq){
00789         
00790         KDL::JntArray result( jnt_size );
00791         for( int x=0; x< jnt_size; ++x ){
00792                 result(x) = (desiredVel[x]-D[x]) * dq[x];
00793         }
00794         return result;
00795 }
00796 
00797 
00798 void R2ImpedanceController::update(){
00799         boost::lock_guard<boost::mutex> lock(thread_mutex);
00800         
00802         ros::Time last_time = time;
00803         time = robot_state->getTime();
00804         double deltaT = (time-last_time).toSec();
00805         if( deltaT < .000001 )
00806                 deltaT = .000001;
00807         //const double qd_scale_factor = 1;
00808         
00809         
00810         cc.wbc.reset();
00811         //cout << deltaT << " | ";
00812         {
00814                 for( int x=0; x<cc.jnt_size; ++x ){
00815                         double p = robotStateJoints[x]->position_;
00816                         double v = robotStateJoints[x]->velocity_;
00817                         //double v2 = (p-treeJnts[x])/deltaT;
00818                         //if( fabs(v-v2) < .01 )
00819                         //      cout << ".";
00820                         //else
00821                         //      cout << v << " " << v2 <<" [" << p << " " << idx2name[x] <<"] \t";
00823                         if( v == v ){   
00824                                 if( v < -1.0 )
00825                                         v = -1.0;
00826                                 else if( v > 1.0 )
00827                                         v = 1.0;
00828                                 
00829                                 
00830                                 cc.treeJntsVelAvg[x] = v;
00831                         } else
00832                                 cc.treeJntsVelAvg[x] = 0;{
00833                         //      v = (p - treeJnts[x])/deltaT;
00834                         //      if( v == v )
00835                         //              treeJntsVel[x] = v;
00836                         }
00837                         cc.treeJntsAvg[x] = p;
00838                         cc.treeJnts[x] = cc.treeJntsAvg[x];
00839                         cc.treeJntsVel[x] = cc.treeJntsVelAvg[x];
00840                 }
00841         }
00842         
00843         
00845         cc.calculate();
00847         {
00848                 for( int x=0; x<cc.jnt_size; ++x ){
00849                         robotStateJoints[x]->commanded_effort_ = cc.torques(x);
00850                 }
00851                 robot_state->enforceSafety();
00852         }
00853         static int loopCnt = 0;
00854         if( loopCnt++ > 50 ){
00855                 loopCnt=0;
00856                 publish_msgs();
00857         }
00858 }
00859 void R2ImpedanceController::CtrlCalc::calculate(){      
00860         
00861         KDL::SetToZero(torques);
00862         Eigen::VectorXd left_t = Eigen::VectorXd::Zero(jnt_size);
00863         Eigen::VectorXd right_t = Eigen::VectorXd::Zero(jnt_size);
00864         
00865         
00866         Wrenches wrenches( robot_tree.getNrOfSegments() + 1 ); //KDL has a discrepancy between segments in SegmentMap (has an extra ) and getNrOfSegments, for now add one
00867         vector<double> zeros( treeJnts.size() );
00868         for( unsigned int x=0; x<treeJnts.size(); ++x )
00869                 zeros[x] = 0;
00870         
00871         rne_calc->CartToJnt( treeJnts, treeJntsVel, zeros, wrenches, torques);
00872         
00873         if( left_cart ){
00874                 if( left_cart_vel )
00875                         torques.data += wbc.project( left.moveCart(leftCmd, leftVelCmd, treeJnts, treeJntsVel), left ).data;
00876                 else
00877                         torques.data += wbc.project( left.moveCart(leftCmd, treeJnts, treeJntsVel ), left ).data;
00878         }
00879         if( right_cart ){
00880                 if( right_cart_vel )
00881                         torques.data += wbc.project( right.moveCart( rightCmd, rightVelCmd, treeJnts, treeJntsVel), right ).data;
00882                 else
00883                         torques.data += wbc.project( right.moveCart(rightCmd, treeJnts, treeJntsVel), right ).data;
00884         }
00885         if( neck_cart )
00886                 torques.data += wbc.project( neck.moveCart(neckCmd, treeJnts, treeJntsVel), neck ).data;
00887         
00888         torques.data += wbc.project( jointKCmd( treeJnts ) ).data;
00889         
00890         torques.data += wbc.project( jointDCmd( treeJntsVel ) ).data;
00891         //torques.data += jointDCmd( treeJntsVel).data; 
00892 
00893 }
00894 
00895 void R2ImpedanceController::publish_msgs(){
00896         if( gains_publisher && gains_publisher->trylock() ){
00897                 nasa_r2_common_msgs::Gains& msg = gains_publisher->msg_;
00898                 msg.joint_names.clear();
00899                 msg.K.clear();
00900                 msg.D.clear();
00901                 for( map<string,int>::const_iterator i=cc.name2idx.begin(); i!=cc.name2idx.end(); ++i ){
00902                         msg.joint_names.push_back( i->first );
00903                         msg.K.push_back( cc.K[ i->second ] );
00904                         msg.D.push_back( cc.D[ i->second ] );
00905                 }
00906                 string cleft = "cart_left_";
00907                 string cright = "cart_right_";
00908                 for( int x=0; x<6; ++x ){
00909                         stringstream ss;
00910                         ss << x;
00911                         msg.joint_names.push_back( cleft + ss.str() );
00912                         msg.K.push_back( cc.cartK_left[x] );
00913                         msg.D.push_back( cc.cartD_left[x] );
00914                         
00915                         msg.joint_names.push_back( cright + ss.str() );
00916                         msg.K.push_back( cc.cartK_right[x] );
00917                         msg.D.push_back( cc.cartD_right[x] );
00918                 }
00919                 gains_publisher->unlockAndPublish();
00920         }
00921         if( cc.left_cart ){
00922                 Eigen::Matrix<double, 7, 1> fk = cc.left.fk(cc.treeJnts);
00923                 KDL::Frame f1( KDL::Rotation::Quaternion( fk[4], fk[5], fk[6], fk[3] ), KDL::Vector( fk[0], fk[1], fk[2] ) );
00924                 KDL::Frame f2( KDL::Rotation::Quaternion( cc.leftCmd[4], cc.leftCmd[5], cc.leftCmd[6], cc.leftCmd[3] ), KDL::Vector( cc.leftCmd[0], cc.leftCmd[1], cc.leftCmd[2] ) );
00925                 
00926                 KDL::Twist twist = KDL::diff( f1, f2 ); 
00927         
00928                 if( left_pose_error_publisher && left_pose_error_publisher->trylock() ){
00929                         left_pose_error_publisher->msg_.linear.x = twist.vel(0);
00930                         left_pose_error_publisher->msg_.linear.y = twist.vel(1);
00931                         left_pose_error_publisher->msg_.linear.z = twist.vel(2);
00932                         left_pose_error_publisher->msg_.angular.x = twist.rot(0);
00933                         left_pose_error_publisher->msg_.angular.y = twist.rot(1);
00934                         left_pose_error_publisher->msg_.angular.z = twist.rot(2);
00935                         left_pose_error_publisher->unlockAndPublish();
00936                 }
00937                 if( left_tip_pose_publisher && left_tip_pose_publisher->trylock() ){
00938                         tf::Pose tmp;
00939                         tf::PoseKDLToTF( f1, tmp );
00940                         poseStampedTFToMsg( tf::Stamped<tf::Pose>( tmp, ros::Time::now(), cc.root_name), left_tip_pose_publisher->msg_ );
00941                         left_tip_pose_publisher->unlockAndPublish();
00942                 }
00943         }
00944         if( cc.right_cart ){
00945                 Eigen::Matrix<double, 7, 1> fk = cc.right.fk(cc.treeJnts);
00946                 KDL::Frame f1( KDL::Rotation::Quaternion( fk[4], fk[5], fk[6], fk[3] ), KDL::Vector( fk[0], fk[1], fk[2] ) );
00947                 KDL::Frame f2( KDL::Rotation::Quaternion( cc.rightCmd[4], cc.rightCmd[5], cc.rightCmd[6], cc.rightCmd[3] ), KDL::Vector( cc.rightCmd[0], cc.rightCmd[1], cc.rightCmd[2] ) );
00948                 
00949                 KDL::Twist twist = KDL::diff( f1, f2 ); 
00950                 
00951                 if( right_pose_error_publisher && right_pose_error_publisher->trylock() ){
00952                         right_pose_error_publisher->msg_.linear.x  = twist.vel(0);
00953                         right_pose_error_publisher->msg_.linear.y  = twist.vel(1);
00954                         right_pose_error_publisher->msg_.linear.z  = twist.vel(2);
00955                         right_pose_error_publisher->msg_.angular.x = twist.rot(0);
00956                         right_pose_error_publisher->msg_.angular.y = twist.rot(1);
00957                         right_pose_error_publisher->msg_.angular.z = twist.rot(2);
00958                         right_pose_error_publisher->unlockAndPublish();
00959                 }
00960 
00961                 if( right_tip_pose_publisher && right_tip_pose_publisher->trylock() ){
00962                         tf::Pose tmp;
00963                         tf::PoseKDLToTF( f1, tmp );
00964                         poseStampedTFToMsg( tf::Stamped<tf::Pose>( tmp, ros::Time::now(), cc.root_name), right_tip_pose_publisher->msg_ );
00965                         right_tip_pose_publisher->unlockAndPublish();
00966                 }
00967         }
00968         
00969 }
00970 
00971 
00972 
00973 
00974 PLUGINLIB_DECLARE_CLASS(r2_controllers_gazebo, R2ImpedanceController, r2_controller_ns::R2ImpedanceController, pr2_controller_interface::Controller)
00975 
00976 
00977 


r2_controllers_gazebo
Author(s): Stephen Hart
autogenerated on Mon Oct 6 2014 02:48:44