chainjnttojacsolver.cpp
Go to the documentation of this file.
1 // Copyright (C) 2020 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 
32  locked_joints_.resize(chain.getNrOfJoints(),false);
33  }
35  {
36  }
37 
38  int ChainJntToJacSolver::setLockedJoints(const std::vector<bool> locked_joints)
39  {
40  if(locked_joints_.size() != chain.getNrOfJoints())
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  {
50  if(locked_joints_.size() != chain.getNrOfJoints())
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 
unsigned int columns() const
Definition: jacobian.cpp:76
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
Chain size changed.
Definition: solveri.hpp:97
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:68
Input size does not match internal state.
Definition: solveri.hpp:99
bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:91
unsigned int rows() const
Definition: jntarray.cpp:72
std::vector< bool > locked_joints_
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
const Joint & getJoint() const
Definition: segment.hpp:118
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1069
int setLockedJoints(const std::vector< bool > locked_joints)
virtual void updateInternalDataStructures()
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
Frame pose(const double &q) const
Definition: segment.cpp:57
Requested index out of range.
Definition: solveri.hpp:103
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:150
Vector p
origine of the Frame
Definition: frames.hpp:572
const JointType & getType() const
Definition: joint.hpp:159
static Frame Identity()
Definition: frames.hpp:701
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
ChainJntToJacSolver(const Chain &chain)
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149


orocos_kdl
Author(s):
autogenerated on Tue Sep 1 2020 03:18:51