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;
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);
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);