Skip to content

Commit e566376

Browse files
author
ipuch
committed
useless test
1 parent 8e91cff commit e566376

File tree

1 file changed

+0
-72
lines changed

1 file changed

+0
-72
lines changed

unittest/joint-ellipsoid.cpp

Lines changed: 0 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -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

14270
BOOST_AUTO_TEST_SUITE_END()
14371

0 commit comments

Comments
 (0)