@@ -66,78 +66,6 @@ BOOST_AUTO_TEST_CASE(vsFreeFlyer)
6666
6767 // Calculer jdata.S().transpose() * data.f[i]
6868}
69- BOOST_AUTO_TEST_CASE (vsRandomForce)
70- {
71- using namespace pinocchio ;
72- typedef SE3::Vector3 Vector3;
73- typedef Eigen::Matrix<double , 6 , 1 > Vector6;
74- typedef Eigen::Matrix<double , 7 , 1 > VectorFF;
75- typedef SE3::Matrix3 Matrix3;
76-
77- JointModelEllipsoid jmodel (1 , 2 , 3 );
78- JointDataEllipsoid data = jmodel.createData ();
79-
80- Eigen::Vector3d qEllipsoid = Eigen::Vector3d::Ones ();
81- jmodel.calc (data, qEllipsoid);
82-
83- Force frandom = Force::Random ();
84- std::cout << " Random force:\n " << frandom.toVector () << std::endl;
85- std::cout << " jdata.S():\n " << data.S .matrix () << std::endl;
86- std::cout << " jdata.S().transpose():\n " << data.S .matrix ().transpose () << std::endl;
87-
88- // Direct matrix multiplication works
89- Eigen::Vector3d tau1 = data.S .matrix ().transpose () * frandom.toVector ();
90- std::cout << " tau1 (S^T * f via matrix): \n " << tau1.transpose () << std::endl;
91-
92- // Now data.S.transpose() * frandom works too (bug fixed!)
93- Eigen::Vector3d tau2 = data.S .transpose () * frandom;
94- std::cout << " tau2 (S^T * f via transpose): \n " << tau2.transpose () << std::endl;
95-
96- // Verify both methods give the same result
97- BOOST_CHECK (tau1.isApprox (tau2));
98-
99- Model modelComposite;
100-
101- Inertia inertia (1 ., Vector3 (0.5 , 0 ., 0.0 ), Matrix3::Identity ());
102- SE3 pos (1 );
103- pos.translation () = SE3::LinearType (1 ., 0 ., 0 .);
104-
105- JointModelComposite jmodel_composite ((JointModelRZ ()));
106- jmodel_composite.addJoint (JointModelRY ());
107- jmodel_composite.addJoint (JointModelRX ());
108-
109-
110- addJointAndBody (modelComposite, jmodel_composite, 0 , pos, " composite" , inertia);
111- Data dataComposite (modelComposite);
112-
113-
114- Eigen::VectorXd q = Eigen::VectorXd::Ones (modelComposite.nq );
115- Eigen::VectorXd v = Eigen::VectorXd::Ones (modelComposite.nv );
116-
117- forwardKinematics (modelComposite, dataComposite, q, v);
118-
119- Eigen::VectorXd tau = Eigen::VectorXd::Ones (modelComposite.nv );
120-
121- // ABA
122- aba (modelComposite,dataComposite,q,v,tau);
123-
124- // for later
125- // addJointAndBody(modelEllipsoid, JointModelEllipsoid(1, 2,3), 0, pos, "ellipsoid", inertia);
126- // addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, pos, "free-flyer", inertia);
127-
128- // Data dataEllipsoid(modelEllipsoid);
129- // forwardKinematics(modelEllipsoid, dataEllipsoid, q, v);
130-
131- // Eigen::VectorXd tauEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv);
132-
133- // Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv);
134-
135- // ForwardDynamics == aba
136- // Eigen::VectorXd aAbaEllipsoid =
137- // aba(modelEllipsoid, dataEllipsoid, q, v, tauEllipsoid, Convention::WORLD);
138-
139- // Calculer jdata.S().transpose() * data.f[i]
140- }
14169
14270BOOST_AUTO_TEST_SUITE_END ()
14371
0 commit comments