Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/joint-spherical.cpp	Lines:	154	157	98.1 %
Date:	2024-04-26 13:14:21	Branches:	675	1350	50.0 %


//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

#include <boost/test/unit_test.hpp>
#include <iostream>

using namespace pinocchio;

template<typename D>
void addJointAndBody(Model & model,
                     const JointModelBase<D> & jmodel,
                     const Model::JointIndex parent_id,
                     const SE3 & joint_placement,
                     const std::string & joint_name,
                     const Inertia & Y)
{
  Model::JointIndex idx;

  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
  model.appendBodyToJoint(idx,Y);
}

BOOST_AUTO_TEST_SUITE(JointSpherical)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionSpherical mp(MotionSpherical::Vector3(1.,2.,3.));
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef SE3::Matrix3 Matrix3;

  Model modelSpherical, modelFreeflyer;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelSpherical,JointModelSpherical(),0,pos,"spherical",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);

  Data dataSpherical(modelSpherical);
  Data dataFreeFlyer(modelFreeflyer);

  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelSpherical.nq);q.normalize();
  VectorFF qff; qff << 0, 0, 0, q[0], q[1], q[2], q[3];
  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelSpherical.nv);
  Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
  Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones(modelSpherical.nv);
  Eigen::VectorXd tauff; tauff.resize(7); tauff << 0,0,0,1,1,1,1;
  Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones(modelSpherical.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelSpherical, dataSpherical, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelSpherical, dataSpherical, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSpherical.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSpherical.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSpherical.Ycrb[1].matrix()));
  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataSpherical.f[1].toVector()));

  Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[3],
                                                         dataFreeFlyer.nle[4],
                                                         dataFreeFlyer.nle[5]
                                                         ;
  BOOST_CHECK(nle_expected_ff.isApprox(dataSpherical.nle));
  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSpherical.com[0]));

  // InverseDynamics == rnea
  tauSpherical = rnea(modelSpherical, dataSpherical, q, v, aSpherical);
  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);

  Vector3 tau_expected; tau_expected << tauff(3), tauff(4), tauff(5);
  BOOST_CHECK(tauSpherical.isApprox(tau_expected));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaSpherical = aba(modelSpherical,dataSpherical, q, v, tauSpherical);
  Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
  Vector3 a_expected; a_expected << aAbaFreeFlyer[3],
                                    aAbaFreeFlyer[4],
                                    aAbaFreeFlyer[5]
                                    ;
  BOOST_CHECK(aAbaSpherical.isApprox(a_expected));

  // crba
  crba(modelSpherical, dataSpherical,q);
  crba(modelFreeflyer, dataFreeFlyer, qff);

  Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.bottomRightCorner<3,3>());

  BOOST_CHECK(dataSpherical.M.isApprox(M_expected));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
  computeJointJacobians(modelSpherical, dataSpherical, q);
  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
  getJointJacobian(modelSpherical, dataSpherical, 1, LOCAL, jacobian_planar);
  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);


  Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(3),
                                                                      jacobian_ff.col(4),
                                                                      jacobian_ff.col(5)
                                                                      ;

  BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));

}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(JointSphericalZYX)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionSpherical mp(MotionSpherical::Vector3(1.,2.,3.));
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
  // WARNIG : Dynamic algorithm's results cannot be compared to FreeFlyer's ones because
  // of the representation of the rotation and the ConstraintSubspace difference.
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef SE3::Matrix3 Matrix3;

  Model modelSphericalZYX, modelFreeflyer;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelSphericalZYX,JointModelSphericalZYX(),0,pos,"spherical-zyx",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);

  Data dataSphericalZYX(modelSphericalZYX);
  Data dataFreeFlyer(modelFreeflyer);

  Eigen::AngleAxisd rollAngle(1, Eigen::Vector3d::UnitZ());
  Eigen::AngleAxisd yawAngle(1, Eigen::Vector3d::UnitY());
  Eigen::AngleAxisd pitchAngle(1, Eigen::Vector3d::UnitX());
  Eigen::Quaterniond q_sph = rollAngle * yawAngle * pitchAngle;

  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelSphericalZYX.nq);
  VectorFF qff; qff << 0, 0, 0, q_sph.x(), q_sph.y(), q_sph.z(), q_sph.w();
  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
  Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
  Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
  Eigen::VectorXd tauff; tauff.resize(6); tauff << 0,0,0,1,1,1;
  Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelSphericalZYX, dataSphericalZYX, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelSphericalZYX, dataSphericalZYX, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSphericalZYX.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix()));

  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSphericalZYX.com[0]));
}

BOOST_AUTO_TEST_CASE(test_rnea)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Model model;
  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());

  addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data(model);

  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);

  rnea(model, data, q, v, a);
  Vector3 tau_expected(0., -4.905, 0.);

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));

  q = Eigen::VectorXd::Ones(model.nq);
  v = Eigen::VectorXd::Ones(model.nv);
  a = Eigen::VectorXd::Ones(model.nv);

  rnea(model, data, q, v, a);
  tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));

  q << 3, 2, 1;
  v = Eigen::VectorXd::Ones(model.nv);
  a = Eigen::VectorXd::Ones(model.nv);

  rnea(model, data, q, v, a);
  tau_expected << 0.73934458094049,  2.7804530848031, 0.50684940972146;

  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
}

BOOST_AUTO_TEST_CASE(test_crba)
{
  using namespace pinocchio;
  using namespace std;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;

  Model model;
  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());

  addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);

  Data data(model);

  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
  Eigen::MatrixXd M_expected(model.nv,model.nv);

  crba(model, data, q);
  M_expected <<
  1.25,    0,    0,
  0, 1.25,    0,
  0,    0,    1;

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));

  q = Eigen::VectorXd::Ones(model.nq);

  crba(model, data, q);
  M_expected <<
  1.0729816454316, -5.5511151231258e-17,     -0.8414709848079,
  -5.5511151231258e-17,                 1.25,                    0,
  -0.8414709848079,                    0,                    1;

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));

  q << 3, 2, 1;

  crba(model, data, q);
  M_expected <<
  1.043294547392, 2.7755575615629e-17,   -0.90929742682568,
  0,                1.25,                   0,
  -0.90929742682568,                   0,                  1;

  BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4			//
5
6			#include "pinocchio/math/fwd.hpp"
7			#include "pinocchio/multibody/joint/joints.hpp"
8			#include "pinocchio/algorithm/rnea.hpp"
9			#include "pinocchio/algorithm/aba.hpp"
10			#include "pinocchio/algorithm/crba.hpp"
11			#include "pinocchio/algorithm/jacobian.hpp"
12			#include "pinocchio/algorithm/compute-all-terms.hpp"
13
14			#include <boost/test/unit_test.hpp>
15			#include <iostream>
16
17			using namespace pinocchio;
18
19			template<typename D>
20		12	void addJointAndBody(Model & model,
21			const JointModelBase<D> & jmodel,
22			const Model::JointIndex parent_id,
23			const SE3 & joint_placement,
24			const std::string & joint_name,
25			const Inertia & Y)
26			{
27			Model::JointIndex idx;
28
29	✓✗	12	idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
30	✓✗	12	model.appendBodyToJoint(idx,Y);
31		12	}
32
33			BOOST_AUTO_TEST_SUITE(JointSpherical)
34
35	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(spatial)
36			{
37	✓✗	2	SE3 M(SE3::Random());
38	✓✗	2	Motion v(Motion::Random());
39
40	✓✗✓✗	2	MotionSpherical mp(MotionSpherical::Vector3(1.,2.,3.));
41	✓✗	2	Motion mp_dense(mp);
42
43	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
44	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
45
46	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
47		2	}
48
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(vsFreeFlyer)
50			{
51			using namespace pinocchio;
52			typedef SE3::Vector3 Vector3;
53			typedef Eigen::Matrix <double, 6, 1> Vector6;
54			typedef Eigen::Matrix <double, 7, 1> VectorFF;
55			typedef SE3::Matrix3 Matrix3;
56
57	✓✗✓✗	4	Model modelSpherical, modelFreeflyer;
58
59	✓✗✓✗ ✓✗✓✗	2	Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
60	✓✗✓✗ ✓✗	2	SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
61
62	✓✗✓✗ ✓✗	2	addJointAndBody(modelSpherical,JointModelSpherical(),0,pos,"spherical",inertia);
63	✓✗✓✗ ✓✗	2	addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);
64
65	✓✗	4	Data dataSpherical(modelSpherical);
66	✓✗	4	Data dataFreeFlyer(modelFreeflyer);
67
68	✓✗✓✗ ✓✗	4	Eigen::VectorXd q = Eigen::VectorXd::Ones(modelSpherical.nq);q.normalize();
69	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	VectorFF qff; qff << 0, 0, 0, q[0], q[1], q[2], q[3];
70	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Ones(modelSpherical.nv);
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
72	✓✗✓✗	4	Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones(modelSpherical.nv);
73	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	Eigen::VectorXd tauff; tauff.resize(7); tauff << 0,0,0,1,1,1,1;
74	✓✗✓✗	4	Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones(modelSpherical.nv);
75	✓✗	4	Eigen::VectorXd aff(vff);
76
77	✓✗	2	forwardKinematics(modelSpherical, dataSpherical, q, v);
78	✓✗	2	forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
79
80	✓✗	2	computeAllTerms(modelSpherical, dataSpherical, q, v);
81	✓✗	2	computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
82
83	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSpherical.oMi[1]));
84	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSpherical.liMi[1]));
85	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSpherical.Ycrb[1].matrix()));
86	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataSpherical.f[1].toVector()));
87
88	✓✗✓✗ ✓✗	6	Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[3],
89	✓✗✓✗	2	dataFreeFlyer.nle[4],
90	✓✗✓✗	2	dataFreeFlyer.nle[5]
91			;
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(nle_expected_ff.isApprox(dataSpherical.nle));
93	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSpherical.com[0]));
94
95			// InverseDynamics == rnea
96	✓✗✓✗	2	tauSpherical = rnea(modelSpherical, dataSpherical, q, v, aSpherical);
97	✓✗✓✗	2	tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
98
99	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	Vector3 tau_expected; tau_expected << tauff(3), tauff(4), tauff(5);
100	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tauSpherical.isApprox(tau_expected));
101
102			// ForwardDynamics == aba
103	✓✗✓✗	4	Eigen::VectorXd aAbaSpherical = aba(modelSpherical,dataSpherical, q, v, tauSpherical);
104	✓✗✓✗	4	Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
105	✓✗✓✗ ✓✗	4	Vector3 a_expected; a_expected << aAbaFreeFlyer[3],
106	✓✗✓✗	2	aAbaFreeFlyer[4],
107	✓✗✓✗	2	aAbaFreeFlyer[5]
108			;
109	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(aAbaSpherical.isApprox(a_expected));
110
111			// crba
112	✓✗	2	crba(modelSpherical, dataSpherical,q);
113	✓✗	2	crba(modelFreeflyer, dataFreeFlyer, qff);
114
115	✓✗✓✗	2	Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.bottomRightCorner<3,3>());
116
117	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataSpherical.M.isApprox(M_expected));
118
119			// Jacobian
120	✓✗✓✗ ✓✗	4	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
121	✓✗✓✗ ✓✗	4	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
122	✓✗	2	computeJointJacobians(modelSpherical, dataSpherical, q);
123	✓✗	2	computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
124	✓✗	2	getJointJacobian(modelSpherical, dataSpherical, 1, LOCAL, jacobian_planar);
125	✓✗	2	getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);
126
127
128	✓✗✓✗ ✓✗	4	Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(3),
129	✓✗✓✗	2	jacobian_ff.col(4),
130	✓✗✓✗	2	jacobian_ff.col(5)
131			;
132
133	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
134
135		2	}
136			BOOST_AUTO_TEST_SUITE_END()
137
138			BOOST_AUTO_TEST_SUITE(JointSphericalZYX)
139
140	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(spatial)
141			{
142	✓✗	2	SE3 M(SE3::Random());
143	✓✗	2	Motion v(Motion::Random());
144
145	✓✗✓✗	2	MotionSpherical mp(MotionSpherical::Vector3(1.,2.,3.));
146	✓✗	2	Motion mp_dense(mp);
147
148	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
149	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
150
151	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
152		2	}
153
154	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(vsFreeFlyer)
155			{
156			// WARNIG : Dynamic algorithm's results cannot be compared to FreeFlyer's ones because
157			// of the representation of the rotation and the ConstraintSubspace difference.
158			using namespace pinocchio;
159			typedef SE3::Vector3 Vector3;
160			typedef Eigen::Matrix <double, 6, 1> Vector6;
161			typedef Eigen::Matrix <double, 7, 1> VectorFF;
162			typedef SE3::Matrix3 Matrix3;
163
164	✓✗✓✗	4	Model modelSphericalZYX, modelFreeflyer;
165
166	✓✗✓✗ ✓✗✓✗	2	Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
167	✓✗✓✗ ✓✗	2	SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
168
169	✓✗✓✗ ✓✗	2	addJointAndBody(modelSphericalZYX,JointModelSphericalZYX(),0,pos,"spherical-zyx",inertia);
170	✓✗✓✗ ✓✗	2	addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);
171
172	✓✗	4	Data dataSphericalZYX(modelSphericalZYX);
173	✓✗	4	Data dataFreeFlyer(modelFreeflyer);
174
175	✓✗✓✗	2	Eigen::AngleAxisd rollAngle(1, Eigen::Vector3d::UnitZ());
176	✓✗✓✗	2	Eigen::AngleAxisd yawAngle(1, Eigen::Vector3d::UnitY());
177	✓✗✓✗	2	Eigen::AngleAxisd pitchAngle(1, Eigen::Vector3d::UnitX());
178	✓✗✓✗	2	Eigen::Quaterniond q_sph = rollAngle * yawAngle * pitchAngle;
179
180	✓✗✓✗	4	Eigen::VectorXd q = Eigen::VectorXd::Ones(modelSphericalZYX.nq);
181	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	VectorFF qff; qff << 0, 0, 0, q_sph.x(), q_sph.y(), q_sph.z(), q_sph.w();
182	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
183	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
184	✓✗✓✗	4	Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
185	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	4	Eigen::VectorXd tauff; tauff.resize(6); tauff << 0,0,0,1,1,1;
186	✓✗✓✗	4	Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
187	✓✗	4	Eigen::VectorXd aff(vff);
188
189	✓✗	2	forwardKinematics(modelSphericalZYX, dataSphericalZYX, q, v);
190	✓✗	2	forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
191
192	✓✗	2	computeAllTerms(modelSphericalZYX, dataSphericalZYX, q, v);
193	✓✗	2	computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
194
195	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
196	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSphericalZYX.liMi[1]));
197	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix()));
198
199	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSphericalZYX.com[0]));
200		2	}
201
202	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_rnea)
203			{
204			using namespace pinocchio;
205			typedef SE3::Vector3 Vector3;
206			typedef SE3::Matrix3 Matrix3;
207
208	✓✗	4	Model model;
209	✓✗✓✗ ✓✗✓✗	2	Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
210
211	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
212
213	✓✗	4	Data data(model);
214
215	✓✗✓✗	4	Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
216	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
217	✓✗✓✗	4	Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
218
219	✓✗	2	rnea(model, data, q, v, a);
220	✓✗	2	Vector3 tau_expected(0., -4.905, 0.);
221
222	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));
223
224	✓✗✓✗	2	q = Eigen::VectorXd::Ones(model.nq);
225	✓✗✓✗	2	v = Eigen::VectorXd::Ones(model.nv);
226	✓✗✓✗	2	a = Eigen::VectorXd::Ones(model.nv);
227
228	✓✗	2	rnea(model, data, q, v, a);
229	✓✗✓✗ ✓✗	2	tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;
230
231	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
232
233	✓✗✓✗ ✓✗	2	q << 3, 2, 1;
234	✓✗✓✗	2	v = Eigen::VectorXd::Ones(model.nv);
235	✓✗✓✗	2	a = Eigen::VectorXd::Ones(model.nv);
236
237	✓✗	2	rnea(model, data, q, v, a);
238	✓✗✓✗ ✓✗	2	tau_expected << 0.73934458094049, 2.7804530848031, 0.50684940972146;
239
240	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
241		2	}
242
243	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_crba)
244			{
245			using namespace pinocchio;
246			using namespace std;
247			typedef SE3::Vector3 Vector3;
248			typedef SE3::Matrix3 Matrix3;
249
250	✓✗	4	Model model;
251	✓✗✓✗ ✓✗✓✗	2	Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
252
253	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
254
255	✓✗	4	Data data(model);
256
257	✓✗✓✗	4	Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
258	✓✗	4	Eigen::MatrixXd M_expected(model.nv,model.nv);
259
260	✓✗	2	crba(model, data, q);
261			M_expected <<
262	✓✗✓✗ ✓✗	4	1.25, 0, 0,
263	✓✗✓✗ ✓✗	2	0, 1.25, 0,
264	✓✗✓✗ ✓✗	2	0, 0, 1;
265
266	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));
267
268	✓✗✓✗	2	q = Eigen::VectorXd::Ones(model.nq);
269
270	✓✗	2	crba(model, data, q);
271			M_expected <<
272	✓✗✓✗ ✓✗	4	1.0729816454316, -5.5511151231258e-17, -0.8414709848079,
273	✓✗✓✗ ✓✗	2	-5.5511151231258e-17, 1.25, 0,
274	✓✗✓✗ ✓✗	2	-0.8414709848079, 0, 1;
275
276	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));
277
278	✓✗✓✗ ✓✗	2	q << 3, 2, 1;
279
280	✓✗	2	crba(model, data, q);
281			M_expected <<
282	✓✗✓✗ ✓✗	4	1.043294547392, 2.7755575615629e-17, -0.90929742682568,
283	✓✗✓✗ ✓✗	2	0, 1.25, 0,
284	✓✗✓✗ ✓✗	2	-0.90929742682568, 0, 1;
285
286	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
287		2	}
288
289			BOOST_AUTO_TEST_SUITE_END()