chainfdsolver_recursive_newton_euler.cpp
Go to the documentation of this file.
1 // Copyright (C) 2018 Craig Carignan <craigc at ssl dot umd dot edu>
2 
3 // Version: 1.0
4 // Author: Craig Carignan <craigc at ssl dot umd dot edu>
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot be>
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 
24 #include "frames_io.hpp"
25 #include "kinfam_io.hpp"
26 
27 namespace KDL{
28 
30  chain(_chain),
31  DynSolver(chain, _grav),
32  IdSolver(chain, _grav),
33  nj(chain.getNrOfJoints()),
34  ns(chain.getNrOfSegments()),
35  H(nj),
36  Tzeroacc(nj),
37  H_eig(nj,nj),
38  Tzeroacc_eig(nj),
39  L_eig(nj,nj),
40  D_eig(nj),
41  r_eig(nj),
42  acc_eig(nj)
43  {
44  }
45 
49  }
50 
51  int ChainFdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot)
52  {
53  if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments())
54  return (error = E_NOT_UP_TO_DATE);
55 
56  //Check sizes of function parameters
57  if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
58  return (error = E_SIZE_MISMATCH);
59 
60  // Inverse Dynamics:
61  // T = H * qdd + Tcor + Tgrav - J^T * Fext
62  // Forward Dynamics:
63  // 1. Call ChainDynParam->JntToMass to get H
64  // 2. Call ChainIdSolver_RNE->CartToJnt with qdd=0 to get Tcor+Tgrav-J^T*Fext
65  // 3. Calculate qdd = H^-1 * T where T are applied joint torques minus non-inertial internal torques
66 
67  // Calculate Joint Space Inertia Matrix
68  error = DynSolver.JntToMass(q, H);
69  if (error < 0)
70  return (error);
71 
72  // Calculate non-inertial internal torques by inputting zero joint acceleration to ID
73  for(unsigned int i=0;i<nj;i++){
74  q_dotdot(i) = 0.;
75  }
76  error = IdSolver.CartToJnt(q, q_dot, q_dotdot, f_ext, Tzeroacc);
77  if (error < 0)
78  return (error);
79 
80  // Calculate acceleration using inverse symmetric matrix times vector
81  for(unsigned int i=0;i<nj;i++){
82  Tzeroacc_eig(i) = (torques(i)-Tzeroacc(i));
83  for(unsigned int j=0;j<nj;j++){
84  H_eig(i,j) = H(i,j);
85  }
86  }
88  for(unsigned int i=0;i<nj;i++){
89  q_dotdot(i) = acc_eig(i);
90  }
91 
92  return (error = E_NOERROR);
93  }
94 
95  void ChainFdSolver_RNE::RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
96  KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver,
97  KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot,
98  KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp)
99  {
100  fdsolver.CartToJnt(q, q_dot, torques, f_ext, q_dotdot);
101  for (unsigned int i=0; i<nj; ++i)
102  {
103  q_temp(i) = q(i) + q_dot(i)*dt/2.0;
104  q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
105  dq(i) = q_dot(i);
106  dq_dot(i) = q_dotdot(i);
107  }
108  fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
109  for (unsigned int i=0; i<nj; ++i)
110  {
111  q_temp(i) = q(i) + q_dot_temp(i)*dt/2.0;
112  q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
113  dq(i) += 2.0*q_dot_temp(i);
114  dq_dot(i) += 2.0*q_dotdot(i);
115  }
116  fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
117  for (unsigned int i=0; i<nj; ++i)
118  {
119  q_temp(i) = q(i) + q_dot_temp(i)*dt;
120  q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt;
121  dq(i) += 2.0*q_dot_temp(i);
122  dq_dot(i) += 2.0*q_dotdot(i);
123  }
124  fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
125  for (unsigned int i=0; i<nj; ++i)
126  {
127  dq(i) = (dq(i)+q_dot_temp(i))*dt/6.0;
128  dq_dot(i) = (dq_dot(i)+q_dotdot(i))*dt/6.0;
129  }
130  for (unsigned int i=0; i<nj; ++i)
131  {
132  q(i) += dq(i);
133  q_dot(i) += dq_dot(i);
134  }
135  return;
136  }
137 
138 }//namespace
void RK4Integrator(unsigned int &nj, const double &t, double &dt, KDL::JntArray &q, KDL::JntArray &q_dot, KDL::JntArray &torques, KDL::Wrenches &f_ext, KDL::ChainFdSolver_RNE &fdsolver, KDL::JntArray &q_dotdot, KDL::JntArray &dq, KDL::JntArray &dq_dot, KDL::JntArray &q_temp, KDL::JntArray &q_dot_temp)
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
int ldl_solver_eigen(const Eigen::MatrixXd &A, const Eigen::VectorXd &v, Eigen::MatrixXd &L, Eigen::VectorXd &D, Eigen::VectorXd &vtmp, Eigen::VectorXd &q)
Solves the system of equations Aq = v for q via LDL decomposition, where A is a square positive defin...
Input size does not match internal state.
Definition: solveri.hpp:99
Recursive newton euler forward dynamics solver.
unsigned int rows() const
Definition: jntarray.cpp:70
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches &f_ext, JntArray &q_dotdot)
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
ChainFdSolver_RNE(const Chain &chain, Vector grav)
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
std::vector< Wrench > Wrenches
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix &H)
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14