chainidsolver_recursive_newton_euler.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 
23 #include "frames_io.hpp"
24 
25 namespace KDL{
26 
28  chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
29  X(ns),S(ns),v(ns),a(ns),f(ns)
30  {
31  ag=-Twist(grav,Vector::Zero());
32  }
33 
37  X.resize(ns);
38  S.resize(ns);
39  v.resize(ns);
40  a.resize(ns);
41  f.resize(ns);
42  }
43 
44  int ChainIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)
45  {
46  if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments())
47  return (error = E_NOT_UP_TO_DATE);
48 
49  //Check sizes when in debug mode
50  if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
51  return (error = E_SIZE_MISMATCH);
52  unsigned int j=0;
53 
54  //Sweep from root to leaf
55  for(unsigned int i=0;i<ns;i++){
56  double q_,qdot_,qdotdot_;
58  q_=q(j);
59  qdot_=q_dot(j);
60  qdotdot_=q_dotdot(j);
61  j++;
62  }else
63  q_=qdot_=qdotdot_=0.0;
64 
65  //Calculate segment properties: X,S,vj,cj
66  X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the
67  //frame for transformations from
68  //the parent to the current coord frame
69  //Transform velocity and unit velocity to segment frame
70  Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
71  S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
72  //We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
73  //calculate velocity and acceleration of the segment (in segment coordinates)
74  if(i==0){
75  v[i]=vj;
76  a[i]=X[i].Inverse(ag)+S[i]*qdotdot_+v[i]*vj;
77  }else{
78  v[i]=X[i].Inverse(v[i-1])+vj;
79  a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;
80  }
81  //Calculate the force for the joint
82  //Collect RigidBodyInertia and external forces
84  f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
85  //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
86  }
87  //Sweep from leaf to root
88  j=nj-1;
89  for(int i=ns-1;i>=0;i--){
91  torques(j)=dot(S[i],f[i]);
92  torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia
93  --j;
94  }
95  if(i!=0)
96  f[i-1]=f[i-1]+X[i]*f[i];
97  }
98  return (error = E_NOERROR);
99  }
100 }//namespace
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
ChainIdSolver_RNE(const Chain &chain, Vector grav)
const RigidBodyInertia & getInertia() const
Definition: segment.hpp:128
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
6D Inertia of a rigid body
Input size does not match internal state.
Definition: solveri.hpp:99
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.hpp:138
unsigned int rows() const
Definition: jntarray.cpp:72
const Joint & getJoint() const
Definition: segment.hpp:118
represents both translational and rotational velocities.
Definition: frames.hpp:720
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
Frame pose(const double &q) const
Definition: segment.cpp:57
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
const JointType & getType() const
Definition: joint.hpp:159
std::vector< Wrench > Wrenches
const double & getInertia() const
Definition: joint.hpp:200
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
static Vector Zero()
Definition: frames.hpp:139


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