18 srand( (
unsigned)time( NULL ));
57 tree.addChain(chain1, tree.getRootSegment()->first);
58 tree.addChain(chain2, tree.getRootSegment()->first);
61 m=1.0, Iz=0.05, L=0.5;
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)));
66 ytree.addSegment(s1,
"root");
67 ytree.addSegment(s2,
"S1");
68 ytree.addSegment(s3,
"S1");
77 std::cout <<
"\nTree: " << endl << tree << endl;
85 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, solver.
CartToJnt(q, qd, qdd,
WrenchMap(), tau));
87 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, solver.
CartToJnt(q, qd, qdd,
WrenchMap(), tau));
89 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_SIZE_MISMATCH, solver.
CartToJnt(q, qd, qdd,
WrenchMap(), tau));
96 CPPUNIT_ASSERT_EQUAL((
int)SolverI::E_NOERROR, solver.
CartToJnt(q, qd, qdd,
WrenchMap(), tau));
106 unsigned int nt = tree.getNrOfJoints();
107 unsigned int n1 = chain1.getNrOfJoints();
108 unsigned int n2 = chain2.getNrOfJoints();
111 CPPUNIT_ASSERT_EQUAL(nt, n1+n2);
113 JntArray q(nt), qd(nt), qdd(nt), tau(nt);
114 JntArray q1(n1), qd1(n1), qdd1(n1), tau1(n1);
115 JntArray q2(n2), qd2(n2), qdd2(n2), tau2(n2);
117 unsigned int iterations = 100;
118 while(iterations-- > 0) {
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);
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));
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);
139 std::cout <<
"\nY-shaped tree: " << endl << ytree << endl;
146 unsigned int dof = ytree.getNrOfJoints();
147 CPPUNIT_ASSERT_EQUAL(dof, (
unsigned int)3);
149 JntArray q(dof), qd(dof), qdd(dof), tau(dof), eff(dof);
152 double Ie = Iz + 0.25*mLL;
154 unsigned int iterations = 100;
155 while(iterations-- > 0) {
157 for(
unsigned int i=0; i<dof; i++)
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;
174 CPPUNIT_ASSERT_EQUAL(tau, eff);
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques)
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
unsigned int rows() const
This class represents an fixed size array containing joint values of a KDL::Chain.
6D Inertia of a rigid body
Input size does not match internal state.
std::map< std::string, Wrench > WrenchMap
A concrete implementation of a 3 dimensional vector class.
Recursive newton euler inverse dynamics solver.
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
void resize(unsigned int newSize)
std::vector< Wrench > Wrenches
SegmentMap::const_iterator getRootSegment() const
virtual void updateInternalDataStructures()
represents a frame transformation in 3D space (rotation + translation)
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
bool addSegment(const Segment &segment, const std::string &hook_name)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
unsigned int getNrOfJoints() const
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
Recursive newton euler inverse dynamics solver for kinematic trees.
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
CPPUNIT_TEST_SUITE_REGISTRATION(TreeInvDynTest)