orocos_kdl
chainjnttojacdotsolver.cpp
Go to the documentation of this file.
1 /*
2  Computes the Jacobian time derivative
3  Copyright (C) 2015 Antoine Hoarau <hoarau [at] isir.upmc.fr>
4 
5  This library is free software; you can redistribute it and/or
6  modify it under the terms of the GNU Lesser General Public
7  License as published by the Free Software Foundation; either
8  version 2.1 of the License, or (at your option) any later version.
9 
10  This library is distributed in the hope that it will be useful,
11  but WITHOUT ANY WARRANTY; without even the implied warranty of
12  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13  Lesser General Public License for more details.
14 
15  You should have received a copy of the GNU Lesser General Public
16  License along with this library; if not, write to the Free Software
17  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18 */
19 
20 
22 
23 namespace KDL
24 {
25 
29 
33 
35  chain(_chain),
36  locked_joints_(chain.getNrOfJoints(),false),
37  nr_of_unlocked_joints_(chain.getNrOfJoints()),
38  jac_solver_(chain),
39  jac_(chain.getNrOfJoints()),
40  jac_dot_(chain.getNrOfJoints()),
41  representation_(HYBRID),
42  fk_solver_(chain)
43 {
44 }
45 
53 }
54 
55 int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Twist& jac_dot_q_dot, int seg_nr)
56 {
57  error = JntToJacDot(q_in,jac_dot_,seg_nr);
58  if (error != E_NOERROR) {
59  return error;
60  }
61  MultiplyJacobian(jac_dot_,q_in.qdot,jac_dot_q_dot);
62  return (error = E_NOERROR);
63 }
64 
65 int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, int seg_nr)
66 {
68  return (error = E_NOT_UP_TO_DATE);
69 
70  unsigned int segmentNr;
71  if(seg_nr<0)
72  segmentNr=chain.getNrOfSegments();
73  else
74  segmentNr = seg_nr;
75 
76  //Initialize Jacobian to zero since only segmentNr columns are computed
77  SetToZero(jdot) ;
78 
79  if(q_in.q.rows()!=chain.getNrOfJoints() || nr_of_unlocked_joints_!=jdot.columns())
80  return (error = E_SIZE_MISMATCH);
81  else if(segmentNr>chain.getNrOfSegments())
82  return (error = E_OUT_OF_RANGE);
83 
84  // First compute the jacobian in the Hybrid representation
85  if (jac_solver_.JntToJac(q_in.q,jac_,segmentNr) != E_NOERROR)
86  return (error = E_JACSOLVER_FAILED);
87 
88  // Change the reference frame and/or the reference point
89  if (representation_ != HYBRID) // If HYBRID do nothing is this is the default.
90  {
91  if (fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr) != E_NOERROR)
92  return (error = E_FKSOLVERPOS_FAILED);
93  if (representation_ == BODYFIXED) {
94  // Ref Frame {ee}, Ref Point {ee}
96  } else if (representation_ == INERTIAL) {
97  // Ref Frame {bs}, Ref Point {bs}
99  } else {
100  return (error = E_JAC_DOT_FAILED);
101  }
102  }
103 
104  // Let's compute Jdot in the corresponding representation
105  int k=0;
106  for(unsigned int i=0;i<segmentNr;++i)
107  {
108  //Only increase joint nr if the segment has a joint
110 
111  for(unsigned int j=0;j<chain.getNrOfJoints();++j)
112  {
113  // Column J is the sum of all partial derivatives ref (41)
114  if(!locked_joints_[j])
116  }
117  jdot.setColumn(k++,jac_dot_k_);
119  }
120  }
121 
122  return (error = E_NOERROR);
123 }
124 
126  const unsigned int& joint_idx,
127  const unsigned int& column_idx,
128  const int& representation)
129 {
130  switch(representation)
131  {
132  case HYBRID:
133  return getPartialDerivativeHybrid(J,joint_idx,column_idx);
134  case BODYFIXED:
135  return getPartialDerivativeBodyFixed(J,joint_idx,column_idx);
136  case INERTIAL:
137  return getPartialDerivativeInertial(J,joint_idx,column_idx);
138  default:
139  SetToZero(this->t_djdq_);
140  return t_djdq_;
141  }
142 }
143 
145  const unsigned int& joint_idx,
146  const unsigned int& column_idx)
147 {
148  int j=joint_idx;
149  int i=column_idx;
150 
151  jac_j_ = bs_J_ee.getColumn(j);
152  jac_i_ = bs_J_ee.getColumn(i);
153 
155 
156  if(j < i)
157  {
158  // P_{\Delta}({}_{bs}J^{j}) ref (20)
161  }else if(j > i)
162  {
163  // M_{\Delta}({}_{bs}J^{j}) ref (23)
166  }else if(j == i)
167  {
168  // ref (40)
171  }
172  return t_djdq_;
173 }
174 
176  const unsigned int& joint_idx,
177  const unsigned int& column_idx)
178 {
179  int j=joint_idx;
180  int i=column_idx;
181 
183 
184  if(j > i)
185  {
186  jac_j_ = ee_J_ee.getColumn(j);
187  jac_i_ = ee_J_ee.getColumn(i);
188 
189  // - S_d_(ee_J^j) * ee_J^ee ref (23)
192  t_djdq_ = -t_djdq_;
193  }
194 
195  return t_djdq_;
196 }
198  const unsigned int& joint_idx,
199  const unsigned int& column_idx)
200 {
201  int j=joint_idx;
202  int i=column_idx;
203 
205 
206  if(j < i)
207  {
208  jac_j_ = bs_J_bs.getColumn(j);
209  jac_i_ = bs_J_bs.getColumn(i);
210 
211  // S_d_(bs_J^j) * bs_J^bs ref (23)
214  }
215 
216  return t_djdq_;
217 }
218 void ChainJntToJacDotSolver::setRepresentation(const int& representation)
219 {
220  if(representation == HYBRID ||
221  representation == BODYFIXED ||
222  representation == INERTIAL)
223  this->representation_ = representation;
224 }
225 
226 
228 {
229  if(locked_joints.size()!=locked_joints_.size())
230  return E_SIZE_MISMATCH;
231  locked_joints_=locked_joints;
233  for(unsigned int i=0;i<locked_joints_.size();i++){
234  if(!locked_joints_[i])
236  }
237 
238  return (error = E_NOERROR);
239 }
240 
241 const char* ChainJntToJacDotSolver::strError(const int error) const
242 {
243  if (E_JAC_DOT_FAILED == error) return "Jac Dot Failed";
244  else if (E_JACSOLVER_FAILED == error) return "Jac Solver Failed";
245  else if (E_FKSOLVERPOS_FAILED == error) return "FK Position Solver Failed";
246  return SolverI::strError(error);
247 }
248 
250 {
251 }
252 } // namespace KDL
KDL::ChainJntToJacDotSolver::F_bs_ee_
Frame F_bs_ee_
Definition: chainjnttojacdotsolver.hpp:176
std::vector::resize
T resize(T... args)
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:74
KDL::ChainJntToJacDotSolver::E_FKSOLVERPOS_FAILED
static const int E_FKSOLVERPOS_FAILED
Definition: chainjnttojacdotsolver.hpp:53
KDL::SolverI::strError
virtual const char * strError(const int error) const
Definition: solveri.hpp:125
KDL::ChainJntToJacDotSolver::representation_
int representation_
Definition: chainjnttojacdotsolver.hpp:174
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::ChainJntToJacDotSolver::setLockedJoints
int setLockedJoints(const std::vector< bool > &locked_joints)
Definition: chainjnttojacdotsolver.cpp:227
KDL::Jacobian::setColumn
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:148
KDL::ChainJntToJacDotSolver::jac_j_
Twist jac_j_
Definition: chainjnttojacdotsolver.hpp:178
KDL::ChainJntToJacDotSolver::E_JACSOLVER_FAILED
static const int E_JACSOLVER_FAILED
Definition: chainjnttojacdotsolver.hpp:52
KDL::Jacobian::changeBase
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:103
KDL::ChainJntToJacDotSolver::BODYFIXED
static const int BODYFIXED
Definition: chainjnttojacdotsolver.hpp:58
KDL::ChainJntToJacDotSolver::jac_i_
Twist jac_i_
Definition: chainjnttojacdotsolver.hpp:178
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:637
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::ChainJntToJacDotSolver::ChainJntToJacDotSolver
ChainJntToJacDotSolver(const Chain &chain)
Definition: chainjnttojacdotsolver.cpp:34
KDL::ChainJntToJacDotSolver::~ChainJntToJacDotSolver
virtual ~ChainJntToJacDotSolver()
Definition: chainjnttojacdotsolver.cpp:249
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::Jacobian::changeRefPoint
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:89
KDL::ChainJntToJacDotSolver::strError
virtual const char * strError(const int error) const
Definition: chainjnttojacdotsolver.cpp:241
KDL::ChainJntToJacDotSolver::nr_of_unlocked_joints_
unsigned int nr_of_unlocked_joints_
Definition: chainjnttojacdotsolver.hpp:170
KDL::Jacobian::resize
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:54
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
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::ChainJntToJacDotSolver::locked_joints_
std::vector< bool > locked_joints_
Definition: chainjnttojacdotsolver.hpp:169
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::ChainJntToJacDotSolver::jac_solver_
ChainJntToJacSolver jac_solver_
Definition: chainjnttojacdotsolver.hpp:171
KDL::ChainJntToJacDotSolver::INERTIAL
static const int INERTIAL
Definition: chainjnttojacdotsolver.hpp:60
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Definition: chainfksolverpos_recursive.cpp:34
KDL::ChainJntToJacDotSolver::jac_dot_
Jacobian jac_dot_
Definition: chainjnttojacdotsolver.hpp:173
KDL::ChainJntToJacDotSolver::JntToJacDot
virtual int JntToJacDot(const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1)
Computes .
Definition: chainjnttojacdotsolver.cpp:55
KDL::ChainJntToJacDotSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainjnttojacdotsolver.cpp:46
KDL::ChainJntToJacDotSolver::chain
const Chain & chain
Definition: chainjnttojacdotsolver.hpp:168
KDL::ChainJntToJacDotSolver::HYBRID
static const int HYBRID
Definition: chainjnttojacdotsolver.hpp:56
KDL::ChainJntToJacSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainjnttojacsolver.cpp:31
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::JntArrayVel
Definition: jntarrayvel.hpp:45
KDL::ChainJntToJacDotSolver::fk_solver_
ChainFkSolverPos_recursive fk_solver_
Definition: chainjnttojacdotsolver.hpp:175
KDL::ChainJntToJacDotSolver::jac_dot_k_
Twist jac_dot_k_
Definition: chainjnttojacdotsolver.hpp:177
KDL::ChainJntToJacDotSolver::getPartialDerivative
const Twist & getPartialDerivative(const Jacobian &J, const unsigned int &joint_idx, const unsigned int &column_idx, const int &representation)
Computes .
Definition: chainjnttojacdotsolver.cpp:125
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
KDL::JntArrayVel::qdot
JntArray qdot
Definition: jntarrayvel.hpp:49
KDL::ChainJntToJacDotSolver::jac_
Jacobian jac_
Definition: chainjnttojacdotsolver.hpp:172
chainjnttojacdotsolver.hpp
KDL::ChainFkSolverPos_recursive::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainfksolverpos_recursive.hpp:45
KDL::ChainJntToJacDotSolver::getPartialDerivativeInertial
const Twist & getPartialDerivativeInertial(const Jacobian &bs_J_bs, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:197
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159
KDL::ChainJntToJacDotSolver::getPartialDerivativeBodyFixed
const Twist & getPartialDerivativeBodyFixed(const Jacobian &ee_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:175
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::JntArrayVel::q
JntArray q
Definition: jntarrayvel.hpp:48
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::MultiplyJacobian
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Definition: jntarray.cpp:100
KDL::Jacobian::getColumn
Twist getColumn(unsigned int i) const
Definition: jacobian.cpp:144
KDL::ChainJntToJacDotSolver::t_djdq_
Twist t_djdq_
Definition: chainjnttojacdotsolver.hpp:179
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::ChainJntToJacDotSolver::getPartialDerivativeHybrid
const Twist & getPartialDerivativeHybrid(const Jacobian &bs_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:144
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
KDL::ChainJntToJacDotSolver::E_JAC_DOT_FAILED
static const int E_JAC_DOT_FAILED
Definition: chainjnttojacdotsolver.hpp:51
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::ChainJntToJacDotSolver::setRepresentation
void setRepresentation(const int &representation)
Sets the internal variable for the representation (with a check on the value)
Definition: chainjnttojacdotsolver.cpp:218
KDL::Jacobian
Definition: jacobian.hpp:36