r2_controllers_gazebo: TreeChain.h Source File

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 #pragma once
00035 
00038 class TreeChain{
00039         std::vector<int> chain2Tree; // hash table to transform chain index to tree index
00040         KDL::Chain chain;
00041         KDL::JntArray jnts;
00042         KDL::JntArrayVel jntsVel;
00043         KDL::JntArray result;
00044         KDL::JntArray full_result;
00045         KDL::Jacobian J;        // jacobian sized for just the chain
00046         KDL::Jacobian fullJ;    // jacobian sized for the entire tree, zeros in rows/columns not in the chain
00047         //typedef Eigen::Matrix<double, 6, 1> Vector6d;
00048         std::vector<double>* pK; //ptr to get immediate updates to gains
00049         std::vector<double>* pD; //ptr to get immediate updates to gains
00050         
00051         int jnt_size;
00052         int tree_size;
00053         
00054         void update( const std::vector<double>& treeJnts, const std::vector<double>& treeJntsVel ){
00055                 for( int x=0; x<jnt_size; ++x ){
00056                         int idx = chain2Tree[x];
00057                         jnts(x) = treeJnts[ idx ];
00058                         jntsVel.q(x) = jnts(x);
00059                         jntsVel.qdot(x) = treeJntsVel[ idx ];
00060                 }
00061                 KDL::ChainJntToJacSolver solver(chain);
00062                 solver.JntToJac( jnts, J );
00063                 for( int y=0; y<jnt_size; ++y ){
00064                         int iy = chain2Tree[y];
00065                         for( int x=0; x<6; ++x ){
00066                                 fullJ( x, iy ) = J( x, y );
00067                         }
00068                 }
00069         }
00070         
00071         public:
00072         int size()const{ return jnt_size; }
00073         int treeIdx( int chainIdx )const{
00074                 return chain2Tree[ chainIdx ];
00075         }
00076         
00077         const KDL::Jacobian& getJ()const{ return fullJ; }
00078         TreeChain(): jnt_size(0), tree_size(0){}
00079         void init( const KDL::Tree& tree, const std::string& root, const std::string& tip, std::vector<double>& Kgains, std::vector<double>& Dgains){
00080                 bool r = tree.getChain( root, tip, chain );
00081                 (void)r;
00082                 assert(r);
00083                 jnt_size = chain.getNrOfJoints();
00084                 tree_size = tree.getNrOfJoints();
00085                 
00086                 jnts.resize(jnt_size);
00087                 jntsVel.resize(jnt_size);
00088                 
00089                 result.resize(jnt_size);
00090                 KDL::SetToZero(result);
00091                 
00092                 full_result.resize(tree_size);
00093                 KDL::SetToZero(full_result);
00094                 
00095                 J.resize(jnt_size);
00096                 fullJ.resize(tree_size);
00097                 SetToZero(fullJ);
00098                 
00099                 pK = &Kgains;
00100                 pD = &Dgains;
00101                 //for( int x=0; x<6; ++x ){
00102                 //      K[x] = Kgains[x];
00103                 //      D[x] = Dgains[x];
00104                 //}
00105 
00106                 chain2Tree.resize(jnt_size);
00107                 const KDL::SegmentMap& sm = tree.getSegments();
00108                 
00109                 int tj=0;
00110                 for( KDL::SegmentMap::const_iterator i= sm.begin(); i!=sm.end(); ++i ){
00111                         const KDL::Segment& seg = i->second.segment;
00112                         const KDL::Joint& joint = seg.getJoint();
00113                         if( joint.getType() == KDL::Joint::None )
00114                                 continue;
00115                         int cj =0;
00116                         const int segCnt = (int)chain.getNrOfSegments();
00117                         for( int x=0; x<segCnt; ++x ){
00118                                 const KDL::Segment& c_seg = chain.getSegment(x);
00119                                 if( c_seg.getJoint().getType() == KDL::Joint::None )
00120                                         continue;
00121                                 if( c_seg.getJoint().getName() == joint.getName() ){
00122                                         chain2Tree[cj] = tj;
00123                                         break;
00124                                 }
00125                                 ++cj;
00126                         }
00127                         ++tj;
00128                 }
00129         }
00130         
00131         const KDL::JntArray& moveCart( Eigen::Matrix<double, 7, 1>& cartCmd, const std::vector<double>& treeJnts, const std::vector<double>& treeJntsVel ){
00132                 update( treeJnts, treeJntsVel );
00133                 
00134                 Eigen::Matrix<double, 7, 1> current = fk();
00135                 Eigen::Matrix<double, 6, 1> v = fk_vel();
00136                 
00137                 KDL::Frame f1( KDL::Rotation::Quaternion( current[4], current[5], current[6], current[3] ), KDL::Vector( current[0], current[1], current[2] ) );
00138                 KDL::Frame f2( KDL::Rotation::Quaternion( cartCmd[4], cartCmd[5], cartCmd[6], cartCmd[3] ), KDL::Vector( cartCmd[0], cartCmd[1], cartCmd[2] ) );
00139                 
00140                 KDL::Twist twist = KDL::diff( f1, f2 );         
00141                 Eigen::Matrix<double, 6, 1> delta;
00142                 
00143                 std::vector<double>& K = *pK;
00144                 std::vector<double>& D = *pD;
00145                 
00146                 delta << twist.vel(0) * K[0] - v[0]*D[0], 
00147                                  twist.vel(1) * K[1] - v[1]*D[1], 
00148                                  twist.vel(2) * K[2] - v[2]*D[2], 
00149                                  twist.rot(0) * K[3] - v[3]*D[3], 
00150                                  twist.rot(1) * K[4] - v[4]*D[4], 
00151                                  twist.rot(2) * K[5] - v[5]*D[5];
00152                 
00153                 result.data = J.data.transpose()*delta;
00154                 
00155                 for( int x=0; x<jnt_size; ++x ){
00156                         full_result( chain2Tree[x] ) = result( x );
00157                 }
00158                 return full_result;
00159         }
00160         const KDL::JntArray& moveCart( KDL::Twist& cartVelCmd, const std::vector<double>& treeJnts, const std::vector<double>& treeJntsVel ){
00161                 update( treeJnts, treeJntsVel );
00162                 
00163                 Eigen::Matrix<double, 6, 1> v = fk_vel();
00164                 Eigen::Matrix<double, 6, 1> delta;
00165                 std::vector<double>& D = *pD;
00166                 delta << (cartVelCmd.vel(0) - v[0] )*D[0], 
00167                                  (cartVelCmd.vel(1) - v[1] )*D[1], 
00168                                  (cartVelCmd.vel(2) - v[2] )*D[2], 
00169                                  (cartVelCmd.rot(0) - v[3] )*D[3], 
00170                                  (cartVelCmd.rot(1) - v[4] )*D[4], 
00171                                  (cartVelCmd.rot(2) - v[5] )*D[5];
00172                 result.data = J.data.transpose()*delta;
00173                 for( int x=0; x<jnt_size; ++x ){
00174                         full_result( chain2Tree[x] ) = result( x );
00175                 }
00176                 return full_result;
00177         }
00178         const KDL::JntArray& moveCart( Eigen::Matrix<double, 7, 1>& cartCmd, KDL::Twist& cartVelCmd, const std::vector<double>& treeJnts, const std::vector<double>& treeJntsVel ){
00179                 update( treeJnts, treeJntsVel );
00180                 
00181                 Eigen::Matrix<double, 7, 1> current = fk();
00182                 Eigen::Matrix<double, 6, 1> v = fk_vel();
00183                 
00184                 KDL::Frame f1( KDL::Rotation::Quaternion( current[4], current[5], current[6], current[3] ), KDL::Vector( current[0], current[1], current[2] ) );
00185                 KDL::Frame f2( KDL::Rotation::Quaternion( cartCmd[4], cartCmd[5], cartCmd[6], cartCmd[3] ), KDL::Vector( cartCmd[0], cartCmd[1], cartCmd[2] ) );
00186                 
00187                 KDL::Twist twist = KDL::diff( f1, f2 );
00188                 Eigen::Matrix<double, 6, 1> delta;
00189                 
00190                 std::vector<double>& K = *pK;
00191                 std::vector<double>& D = *pD;
00192                 
00193                 delta << twist.vel(0) * K[0] + ( cartVelCmd.vel(0) - v[0] ) *D[0], 
00194                                  twist.vel(1) * K[1] + ( cartVelCmd.vel(1) - v[1] ) *D[1], 
00195                                  twist.vel(2) * K[2] + ( cartVelCmd.vel(2) - v[2] ) *D[2], 
00196                                  twist.rot(0) * K[3] + ( cartVelCmd.rot(0) - v[3] ) *D[3], 
00197                                  twist.rot(1) * K[4] + ( cartVelCmd.rot(1) - v[4] ) *D[4], 
00198                                  twist.rot(2) * K[5] + ( cartVelCmd.rot(2) - v[5] ) *D[5];
00199                 
00200                 result.data = J.data.transpose()*delta;
00201                 
00202                 for( int x=0; x<jnt_size; ++x ){
00203                         full_result( chain2Tree[x] ) = result( x );
00204                 }
00205                 return full_result;
00206         }
00207         
00208         Eigen::Matrix<double,7,1> fk( const std::vector<double>& treeJnts ){
00209                 for( int x=0; x<jnt_size; ++x )
00210                         jnts(x) = treeJnts[ chain2Tree[x] ];
00211                 return fk();
00212         }
00213         
00214         Eigen::Matrix<double,7, 1> fk(){
00215                 Eigen::Matrix<double,7,1> result;
00216                         
00217                 KDL::ChainFkSolverPos_recursive solver(chain);
00218                 KDL::Frame frame;
00219                 solver.JntToCart( jnts, frame );
00220                 
00221                 result[0] = frame.p(0);
00222                 result[1] = frame.p(1);
00223                 result[2] = frame.p(2);
00224                 double& w = result[3];
00225                 double& x = result[4];
00226                 double& y = result[5];
00227                 double& z = result[6];
00228                 frame.M.GetQuaternion( x, y, z, w );
00229                 
00230                 return result;
00231         }
00232         Eigen::Matrix<double,6, 1> fk_vel(){
00234                 Eigen::Matrix<double,6,1> result;
00235                 
00236                 KDL::ChainFkSolverVel_recursive solver(chain);
00237                 KDL::FrameVel frame;
00238                 
00239                 solver.JntToCart( jntsVel, frame );
00240                 result[0] = frame.p.v(0);
00241                 result[1] = frame.p.v(1);
00242                 result[2] = frame.p.v(2);
00243                 result[3] = frame.M.w(0);
00244                 result[4] = frame.M.w(1);
00245                 result[5] = frame.M.w(2);
00246                 for( int x=0; x<6; ++x ){
00247                         assert( result[x] == result[x] );
00248                 }
00249                 return result;
00250         }
00251                 
00252                 
00253         
00254         
00255 };
00256 
00257