orocos_kdl
chainjnttojacsolver.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #include "chainjnttojacsolver.hpp"
23 
24 namespace KDL
25 {
27  chain(_chain),locked_joints_(chain.getNrOfJoints(),false)
28  {
29  }
30 
33  }
35  {
36  }
37 
39  {
41  return (error = E_NOT_UP_TO_DATE);
42  if(locked_joints.size()!=locked_joints_.size())
43  return (error = E_SIZE_MISMATCH);
44  locked_joints_=locked_joints;
45  return (error = E_NOERROR);
46  }
47 
48  int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr)
49  {
51  return (error = E_NOT_UP_TO_DATE);
52  unsigned int segmentNr;
53  if(seg_nr<0)
54  segmentNr=chain.getNrOfSegments();
55  else
56  segmentNr = seg_nr;
57 
58  //Initialize Jacobian to zero since only segmentNr columns are computed
59  SetToZero(jac) ;
60 
61  if( q_in.rows()!=chain.getNrOfJoints() || jac.columns() != chain.getNrOfJoints())
62  return (error = E_SIZE_MISMATCH);
63  else if(segmentNr>chain.getNrOfSegments())
64  return (error = E_OUT_OF_RANGE);
65 
68  int j=0;
69  int k=0;
70  Frame total;
71  for (unsigned int i=0;i<segmentNr;i++) {
72  //Calculate new Frame_base_ee
74  //pose of the new end-point expressed in the base
75  total = T_tmp*chain.getSegment(i).pose(q_in(j));
76  //changing base of new segment's twist to base frame if it is not locked
77  //t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0);
78  if(!locked_joints_[j])
79  t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
80  }else{
81  total = T_tmp*chain.getSegment(i).pose(0.0);
82 
83  }
84 
85  //Changing Refpoint of all columns to new ee
86  changeRefPoint(jac,total.p-T_tmp.p,jac);
87 
88  //Only increase jointnr if the segment has a joint
90  //Only put the twist inside if it is not locked
91  if(!locked_joints_[j])
92  jac.setColumn(k++,t_tmp);
93  j++;
94  }
95 
96  T_tmp = total;
97  }
98  return (error = E_NOERROR);
99  }
100 }
101 
KDL::ChainJntToJacSolver::ChainJntToJacSolver
ChainJntToJacSolver(const Chain &chain)
Definition: chainjnttojacsolver.cpp:26
std::vector::resize
T resize(T... args)
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:74
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
KDL::Jacobian::setColumn
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:148
KDL::ChainJntToJacSolver::chain
const Chain & chain
Definition: chainjnttojacsolver.hpp:68
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
std::vector< bool >
std::vector::size
T size(T... args)
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
KDL::ChainJntToJacSolver::JntToJac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
Definition: chainjnttojacsolver.cpp:48
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
KDL::ChainJntToJacSolver::T_tmp
Frame T_tmp
Definition: chainjnttojacsolver.hpp:70
KDL::SolverI::E_OUT_OF_RANGE
@ E_OUT_OF_RANGE
Requested index out of range.
Definition: solveri.hpp:103
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:67
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::ChainJntToJacSolver::~ChainJntToJacSolver
virtual ~ChainJntToJacSolver()
Definition: chainjnttojacsolver.cpp:34
KDL::Segment::pose
Frame pose(const double &q) const
Definition: segment.cpp:57
KDL::ChainJntToJacSolver::setLockedJoints
int setLockedJoints(const std::vector< bool > locked_joints)
Definition: chainjnttojacsolver.cpp:38
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
KDL::ChainJntToJacSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainjnttojacsolver.cpp:31
KDL::ChainJntToJacSolver::t_tmp
Twist t_tmp
Definition: chainjnttojacsolver.hpp:69
KDL::ChainJntToJacSolver::locked_joints_
std::vector< bool > locked_joints_
Definition: chainjnttojacsolver.hpp:71
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
KDL::changeRefPoint
bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:89
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159
chainjnttojacsolver.hpp
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::Jacobian
Definition: jacobian.hpp:36