treeinvdyntest.cpp
Go to the documentation of this file.
1 #include "treeinvdyntest.hpp"
2 #include <kinfam_io.hpp>
3 #include <frames_io.hpp>
4 #include <chainidsolver_recursive_newton_euler.hpp>
5 #include <treeidsolver_recursive_newton_euler.hpp>
6 #include <time.h>
7 #include <cmath>
8 
9 
10 CPPUNIT_TEST_SUITE_REGISTRATION( TreeInvDynTest );
11 
12 using namespace KDL;
13 using std::cout;
14 using std::endl;
15 
16 void TreeInvDynTest::setUp()
17 {
18  srand( (unsigned)time( NULL ));
19 
20  //spatial inertia (just to test dynamics)
21  RigidBodyInertia inertia(0.3, Vector(0.0, 0.1, 0.0), RotationalInertia(0.005, 0.001, 0.001));
22 
23  //create chain #1
24  chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
25  Frame(Vector(0.0,0.0,0.0)), inertia));
26  chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
27  Frame(Vector(0.0,0.0,0.9)), inertia));
28  chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::None),
29  Frame(Vector(-0.4,0.0,0.0))));
30  chain1.addSegment(Segment("Segment 14", Joint("Joint 14", Joint::RotX),
31  Frame(Vector(0.0,0.0,1.2)), inertia));
32  chain1.addSegment(Segment("Segment 15", Joint("Joint 15", Joint::None),
33  Frame(Vector(0.4,0.0,0.0)), inertia));
34  chain1.addSegment(Segment("Segment 16", Joint("Joint 16", Joint::RotZ),
35  Frame(Vector(0.0,0.0,1.4)), inertia));
36  chain1.addSegment(Segment("Segment 17", Joint("Joint 17", Joint::RotX),
37  Frame(Vector(0.0,0.0,0.0)), inertia));
38  chain1.addSegment(Segment("Segment 18", Joint("Joint 18", Joint::RotZ),
39  Frame(Vector(0.0,0.0,0.4)), inertia));
40  chain1.addSegment(Segment("Segment 19", Joint("Joint 19", Joint::None),
41  Frame(Vector(0.0,0.0,0.0)), inertia));
42 
43  //create chain #2
44  chain2.addSegment(Segment("Segment 21", Joint("Joint 21", Joint::RotZ),
45  Frame(Vector(0.0,0.0,0.5)), inertia));
46  chain2.addSegment(Segment("Segment 22", Joint("Joint 22", Joint::RotX),
47  Frame(Vector(0.0,0.0,0.4)), inertia));
48  chain2.addSegment(Segment("Segment 23", Joint("Joint 23", Joint::RotX),
49  Frame(Vector(0.0,0.0,0.3)), inertia));
50  chain2.addSegment(Segment("Segment 24", Joint("Joint 24", Joint::RotX),
51  Frame(Vector(0.0,0.0,0.2)), inertia));
52  chain2.addSegment(Segment("Segment 25", Joint("Joint 25", Joint::RotZ),
53  Frame(Vector(0.0,0.0,0.1)), inertia));
54 
55  //create tree as two chains attached to the root
56  tree = Tree();
57  tree.addChain(chain1, tree.getRootSegment()->first);
58  tree.addChain(chain2, tree.getRootSegment()->first);
59 
60  //create a simple "y-shaped" tree
61  m=1.0, Iz=0.05, L=0.5; //geometric and dynamic parameters
62  Segment s1("S1", Joint("q1", Joint::RotZ), Frame(), RigidBodyInertia(m, Vector(L,0,0), RotationalInertia(0,0,Iz)));
63  Segment s2("S2", Joint("q2", Vector(L,0,0), Vector(0,0,1), Joint::RotAxis), Frame(Vector(L,0,0)), RigidBodyInertia(m, Vector(L/2,0,0), RotationalInertia(0,0,Iz)));
64  Segment s3("S3", Joint("q3", Vector(2*L,0,0), Vector(0,0,1), Joint::RotAxis), Frame(Vector(2*L,0,0)), RigidBodyInertia(m, Vector(L/2,0,0), RotationalInertia(0,0,Iz)));
65  ytree = Tree("root");
66  ytree.addSegment(s1, "root");
67  ytree.addSegment(s2, "S1");
68  ytree.addSegment(s3, "S1");
69 }
70 
71 
72 void TreeInvDynTest::tearDown() { }
73 
74 
75 void TreeInvDynTest::UpdateTreeTest() {
76  // std::cout << "Tree: " << endl << tree2str(tree) << endl; //NOTE: tree2str is not available as I implemented it in another "parallel" commit
77  std::cout << "\nTree: " << endl << tree << endl;
78 
79  Tree temp_tree(tree);
80  TreeIdSolver_RNE solver(temp_tree, Vector::Zero());
81  JntArray q, qd, qdd, tau;
82 
83  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
84  q.resize(temp_tree.getNrOfJoints());
85  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
86  qd.resize(temp_tree.getNrOfJoints());
87  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
88  qdd.resize(temp_tree.getNrOfJoints());
89  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
90  tau.resize(temp_tree.getNrOfJoints());
91  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
92 
93  temp_tree.addSegment(Segment("extra"), temp_tree.getRootSegment()->first);
94  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
95  solver.updateInternalDataStructures();
96  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
97 }
98 
99 
100 void TreeInvDynTest::TwoChainsTest() {
101  Vector gravity(0,0,-9.8);
102  TreeIdSolver_RNE solver(tree, gravity);
103  ChainIdSolver_RNE solver1(chain1, gravity);
104  ChainIdSolver_RNE solver2(chain2, gravity);
105 
106  unsigned int nt = tree.getNrOfJoints();
107  unsigned int n1 = chain1.getNrOfJoints();
108  unsigned int n2 = chain2.getNrOfJoints();
109 
110  //Check that sizes are consistent -- otherwise following code does not make sense!
111  CPPUNIT_ASSERT_EQUAL(nt, n1+n2);
112 
113  JntArray q(nt), qd(nt), qdd(nt), tau(nt); //data related to tree
114  JntArray q1(n1), qd1(n1), qdd1(n1), tau1(n1); //data related to chain1
115  JntArray q2(n2), qd2(n2), qdd2(n2), tau2(n2); //data related to chain2
116 
117  unsigned int iterations = 100;
118  while(iterations-- > 0) {
119  //Randomize joint vectors
120  for(unsigned int i=0; i<nt; i++) random(q(i)), random(qd(i)), random(qdd(i));
121  for(unsigned int i=0; i<n1; i++) q1(i)=q(i), qd1(i)=qd(i), qdd1(i)=qdd(i);
122  for(unsigned int i=0; i<n2; i++) q2(i)=q(i+n1), qd2(i)=qd(i+n1), qdd2(i)=qdd(i+n1);
123 
124  //Evaluate efforts
125  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
126  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver1.CartToJnt(q1, qd1, qdd1, Wrenches(chain1.getNrOfSegments()), tau1));
127  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver2.CartToJnt(q2, qd2, qdd2, Wrenches(chain2.getNrOfSegments()), tau2));
128 
129  //compare efforts
130  JntArray tau12(tau.rows());
131  for(unsigned int i=0; i<n1; i++) tau12(i) = tau1(i);
132  for(unsigned int i=0; i<n2; i++) tau12(i+n1) = tau2(i);
133  CPPUNIT_ASSERT_EQUAL(tau, tau12);
134  }
135 }
136 
137 
138 void TreeInvDynTest::YTreeTest() {
139  std::cout << "\nY-shaped tree: " << endl << ytree << endl;
140 
141  double g = 9.8;
142  TreeIdSolver_RNE solver(ytree, Vector(0,-g,0));
143 
144  //Following is just a check in case the model is modified. In this case the
145  //analytic model derived using Euler-Lagrange equations would not be valid.
146  unsigned int dof = ytree.getNrOfJoints();
147  CPPUNIT_ASSERT_EQUAL(dof, (unsigned int)3); // The analytic euler-lagrange model was derived for a specific 3-dof model
148 
149  JntArray q(dof), qd(dof), qdd(dof), tau(dof), eff(dof);
150 
151  double mLL = m*L*L;
152  double Ie = Iz + 0.25*mLL;
153 
154  unsigned int iterations = 100;
155  while(iterations-- > 0) {
156  //Randomize joint vectors
157  for(unsigned int i=0; i<dof; i++)
158  random(q(i)), random(qd(i)), random(qdd(i));
159 
160  //Evaluate efforts using the numerical solver
161  CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOERROR, solver.CartToJnt(q, qd, qdd, WrenchMap(), tau));
162 
163  //Evaluate efforts using the analytic model derived using Euler-Lagrange equations
164  double s2 = std::sin(q(1)), c2 = std::cos(q(1));
165  double s3 = std::sin(q(2)), c3 = std::cos(q(2));
166  double c1 = std::cos(q(0)), c12 = std::cos(q(0)+q(1)), c13 = std::cos(q(0)+q(2));
167  eff(0) = (3*Iz+6.5*mLL+mLL*c2+2*mLL*c3) * qdd(0) + (Ie+0.5*mLL*c2) * qdd(1)
168  + (Ie+mLL*c3) * qdd(2) - 0.5*mLL*s2*qd(1)*qd(1) - mLL*s3*qd(2)*qd(2)
169  - mLL*s2*qd(0)*qd(1) - 2*mLL*s3*qd(0)*qd(2) + m*g*(4*L*c1+0.5*L*c12+0.5*L*c13);
170  eff(1) = (Ie+0.5*mLL*c2) * qdd(0) + Ie * qdd(1) + 0.5*mLL*s2*qd(0)*qd(0) + 0.5*m*g*L*c12;
171  eff(2) = (Ie+mLL*c3) * qdd(0) + Ie * qdd(2) + mLL*s3*qd(0)*qd(0) + 0.5*m*g*L*c13;
172 
173  //compare efforts
174  CPPUNIT_ASSERT_EQUAL(tau, eff);
175  }
176 
177 }
KDL::Tree::getRootSegment
SegmentMap::const_iterator getRootSegment() const
Definition: tree.hpp:186
KDL::Vector::Zero
static Vector Zero()
Definition: frames.hpp:139
KDL::TreeIdSolver_RNE::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques)
Definition: treeidsolver_recursive_newton_euler.cpp:52
KDL::Segment
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
Definition: segment.hpp:46
KDL::SolverI::E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::Tree::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: tree.hpp:159
KDL::JntArray
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
KDL::SolverI::E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
frames_io.hpp
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::WrenchMap
std::map< std::string, Wrench > WrenchMap
Definition: treeidsolver.hpp:33
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
chainidsolver_recursive_newton_euler.hpp
KDL::ChainIdSolver_RNE
Recursive newton euler inverse dynamics solver.
Definition: chainidsolver_recursive_newton_euler.hpp:40
KDL::ChainIdSolver_RNE::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
Definition: chainidsolver_recursive_newton_euler.cpp:44
kinfam_io.hpp
KDL::JntArray::resize
void resize(unsigned int newSize)
Definition: jntarray.cpp:53
KDL::Wrenches
std::vector< Wrench > Wrenches
Definition: chainfdsolver.hpp:34
KDL::TreeIdSolver_RNE::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: treeidsolver_recursive_newton_euler.cpp:35
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
treeidsolver_recursive_newton_euler.hpp
KDL::Joint
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition: joint.hpp:45
KDL::Tree::addSegment
bool addSegment(const Segment &segment, const std::string &hook_name)
Definition: tree.cpp:54
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:321
KDL::random
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition: frames.hpp:1215
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
Definition: tree.hpp:99
KDL::TreeIdSolver_RNE
Recursive newton euler inverse dynamics solver for kinematic trees.
Definition: treeidsolver_recursive_newton_euler.hpp:41
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:313
CPPUNIT_TEST_SUITE_REGISTRATION
CPPUNIT_TEST_SUITE_REGISTRATION(TreeInvDynTest)

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