chainjnttojacdotsolver.cpp
Go to the documentation of this file.
1 /*
2  Computes the Jacobian time derivative
3  Copyright (C) 2020 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 
47  locked_joints_.resize(chain.getNrOfJoints(),false);
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 {
67  if(locked_joints_.size() != chain.getNrOfJoints())
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 
227 int ChainJntToJacDotSolver::setLockedJoints(const std::vector<bool>& locked_joints)
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
void setRepresentation(const int &representation)
Sets the internal variable for the representation (with a check on the value)
int setLockedJoints(const std::vector< bool > &locked_joints)
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
unsigned int columns() const
Definition: jacobian.cpp:76
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:56
const Twist & getPartialDerivativeHybrid(const Jacobian &bs_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
Vector vel
The velocity of that point.
Definition: frames.hpp:722
const Twist & getPartialDerivativeBodyFixed(const Jacobian &ee_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
Chain size changed.
Definition: solveri.hpp:97
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Definition: jntarray.cpp:102
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:68
Input size does not match internal state.
Definition: solveri.hpp:99
ChainFkSolverPos_recursive fk_solver_
unsigned int rows() const
Definition: jntarray.cpp:72
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
const Joint & getJoint() const
Definition: segment.hpp:118
represents both translational and rotational velocities.
Definition: frames.hpp:720
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1069
virtual void updateInternalDataStructures()
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
virtual const char * strError(const int error) const
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
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:105
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:91
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.hpp:638
const JointType & getType() const
Definition: joint.hpp:159
ChainJntToJacDotSolver(const Chain &chain)
virtual int JntToJacDot(const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1)
Computes .
virtual const char * strError(const int error) const
Definition: solveri.hpp:125
Twist getColumn(unsigned int i) const
Definition: jacobian.cpp:146
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
const Twist & getPartialDerivative(const Jacobian &J, const unsigned int &joint_idx, const unsigned int &column_idx, const int &representation)
Computes .
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
const Twist & getPartialDerivativeInertial(const Jacobian &bs_J_bs, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .


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