chainjnttojacsolver.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 #include "chainjnttojacsolver.hpp"
00023 
00024 namespace KDL
00025 {
00026     ChainJntToJacSolver::ChainJntToJacSolver(const Chain& _chain):
00027         chain(_chain),locked_joints_(chain.getNrOfJoints(),false),
00028         nr_of_unlocked_joints_(chain.getNrOfJoints())
00029     {
00030     }
00031 
00032     ChainJntToJacSolver::~ChainJntToJacSolver()
00033     {
00034     }
00035 
00036     int ChainJntToJacSolver::setLockedJoints(const std::vector<bool> locked_joints)
00037     {
00038         if(locked_joints.size()!=locked_joints_.size())
00039             return -1;
00040         locked_joints_=locked_joints;
00041         nr_of_unlocked_joints_=0;
00042         for(unsigned int i=0;i<locked_joints_.size();i++){
00043             if(!locked_joints_[i])
00044                 nr_of_unlocked_joints_++;
00045         }
00046 
00047         return 0;
00048     }
00049 
00050     int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr)
00051     {
00052         unsigned int segmentNr;
00053         if(seg_nr<0)
00054             segmentNr=chain.getNrOfSegments();
00055         else
00056             segmentNr = seg_nr;
00057 
00058         //Initialize Jacobian to zero since only segmentNr colunns are computed
00059         SetToZero(jac) ;
00060 
00061         if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jac.columns())
00062             return (error = E_JAC_FAILED);
00063         else if(segmentNr>chain.getNrOfSegments())
00064             return (error = E_JAC_FAILED);
00065 
00066         T_tmp = Frame::Identity();
00067         SetToZero(t_tmp);
00068         int j=0;
00069         int k=0;
00070         Frame total;
00071         for (unsigned int i=0;i<segmentNr;i++) {
00072             //Calculate new Frame_base_ee
00073             if(chain.getSegment(i).getJoint().getType()!=Joint::None){
00074                 //pose of the new end-point expressed in the base
00075                 total = T_tmp*chain.getSegment(i).pose(q_in(j));
00076                 //changing base of new segment's twist to base frame if it is not locked
00077                 //t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0);
00078                 if(!locked_joints_[j])
00079                     t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
00080             }else{
00081                 total = T_tmp*chain.getSegment(i).pose(0.0);
00082 
00083             }
00084 
00085             //Changing Refpoint of all columns to new ee
00086             changeRefPoint(jac,total.p-T_tmp.p,jac);
00087 
00088             //Only increase jointnr if the segment has a joint
00089             if(chain.getSegment(i).getJoint().getType()!=Joint::None){
00090                 //Only put the twist inside if it is not locked
00091                 if(!locked_joints_[j])
00092                     jac.setColumn(k++,t_tmp);
00093                 j++;
00094             }
00095 
00096             T_tmp = total;
00097         }
00098         return (error = E_NOERROR);
00099     }
00100 
00101     const char* ChainJntToJacSolver::strError(const int error) const
00102     {
00103         if (E_JAC_FAILED == error) return "Jac Failed";
00104         else return SolverI::strError(error);
00105     }
00106 }
00107 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14