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     {
00029     }
00030 
00031     void ChainJntToJacSolver::updateInternalDataStructures() {
00032         locked_joints_.resize(chain.getNrOfJoints(),false);
00033     }
00034     ChainJntToJacSolver::~ChainJntToJacSolver()
00035     {
00036     }
00037 
00038     int ChainJntToJacSolver::setLockedJoints(const std::vector<bool> locked_joints)
00039     {
00040         if(locked_joints_.size() != chain.getNrOfJoints())
00041             return (error = E_NOT_UP_TO_DATE);
00042         if(locked_joints.size()!=locked_joints_.size())
00043             return (error = E_SIZE_MISMATCH);
00044         locked_joints_=locked_joints;
00045         return (error = E_NOERROR);
00046     }
00047 
00048     int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr)
00049     {
00050         if(locked_joints_.size() != chain.getNrOfJoints())
00051             return (error = E_NOT_UP_TO_DATE);
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 columns are computed
00059         SetToZero(jac) ;
00060 
00061         if( q_in.rows()!=chain.getNrOfJoints() || jac.columns() != chain.getNrOfJoints())
00062             return (error = E_SIZE_MISMATCH);
00063         else if(segmentNr>chain.getNrOfSegments())
00064             return (error = E_OUT_OF_RANGE);
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 


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22