Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/kinematics.cpp	Lines:	153	153	100.0 %
Date:	2024-04-26 13:14:21	Branches:	698	1384	50.4 %


//
// Copyright (c) 2018-2019 CNRS INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);

  forwardKinematics(model,data,Model::ConfigVectorType::Ones(model.nq));
}

BOOST_AUTO_TEST_CASE(test_kinematics_zero)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);

  forwardKinematics(model,data_ref,q);
  crba(model,data,q);
  updateGlobalPlacements(model,data);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]);
    BOOST_CHECK(data.liMi[i] == data_ref.liMi[i]);
  }
}

BOOST_AUTO_TEST_CASE(test_kinematics_first)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Zero(model.nv));

  forwardKinematics(model,data,q,v);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.v[i] == Motion::Zero());
  }
}

BOOST_AUTO_TEST_CASE(test_kinematics_second)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Zero(model.nv));
  VectorXd a(VectorXd::Zero(model.nv));

  forwardKinematics(model,data,q,v,a);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.v[i] == Motion::Zero());
    BOOST_CHECK(data.a[i] == Motion::Zero());
  }
}

BOOST_AUTO_TEST_CASE(test_get_velocity)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));

  forwardKinematics(model,data,q,v);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i)));
    BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i,LOCAL)));
    BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model,data,i,WORLD)));
    BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.v[i]).isApprox(getVelocity(model,data,i,LOCAL_WORLD_ALIGNED)));
  }
}

BOOST_AUTO_TEST_CASE(test_get_acceleration)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  forwardKinematics(model,data,q,v,a);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i)));
    BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i,LOCAL)));
    BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model,data,i,WORLD)));
    BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.a[i]).isApprox(getAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
  }
}

BOOST_AUTO_TEST_CASE(test_get_classical_acceleration)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  forwardKinematics(model,data,q,v,a);

  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
  {
    SE3 T = data.oMi[i];
    Motion vel = data.v[i];
    Motion acc = data.a[i];
    Vector3d linear;

    Motion acc_classical_local = acc;
    linear = acc.linear() + vel.angular().cross(vel.linear());
    acc_classical_local.linear() = linear;

    BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i)));
    BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i,LOCAL)));

    Motion vel_world = T.act(vel);
    Motion acc_classical_world = T.act(acc);
    linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear());
    acc_classical_world.linear() = linear;

    BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model,data,i,WORLD)));

    Motion vel_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(vel);
    Motion acc_classical_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(acc);
    linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear());
    acc_classical_aligned.linear() = linear;

    BOOST_CHECK(acc_classical_aligned.isApprox(getClassicalAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
  }
}

BOOST_AUTO_TEST_CASE(test_kinematic_getters)
{
  using namespace Eigen;
  using namespace pinocchio;

  // Build a simple 2R planar model
  Model model;
  JointIndex jointId = 0;
  jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
  jointId = model.addJoint(jointId, JointModelRZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");

  Data data(model);

  // Predetermined configuration values
  VectorXd q(model.nq);
  q << M_PI/2.0, 0.0;

  VectorXd v(model.nv);
  v << 1.0, 0.0;

  VectorXd a(model.nv);
  a << 0.0, 0.0;

  // Expected velocity
  Motion v_local;
  v_local.linear() = Vector3d(0.0, 1.0, 0.0);
  v_local.angular() = Vector3d(0.0, 0.0, 1.0);

  Motion v_world;
  v_world.linear() = Vector3d::Zero();
  v_world.angular() = Vector3d(0.0, 0.0, 1.0);

  Motion v_align;
  v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
  v_align.angular() = Vector3d(0.0, 0.0, 1.0);

  // Expected classical acceleration
  Motion ac_local;
  ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
  ac_local.angular() = Vector3d::Zero();

  Motion ac_world = Motion::Zero();

  Motion ac_align;
  ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
  ac_align.angular() = Vector3d::Zero();

  // Perform kinematics
  forwardKinematics(model,data,q,v,a);

  // Check output velocity
  BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId)));
  BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId,LOCAL)));
  BOOST_CHECK(v_world.isApprox(getVelocity(model,data,jointId,WORLD)));
  BOOST_CHECK(v_align.isApprox(getVelocity(model,data,jointId,LOCAL_WORLD_ALIGNED)));

  // Check output acceleration (all zero)
  BOOST_CHECK(getAcceleration(model,data,jointId).isZero());
  BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL).isZero());
  BOOST_CHECK(getAcceleration(model,data,jointId,WORLD).isZero());
  BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED).isZero());

  // Check output classical
  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId)));
  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL)));
  BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model,data,jointId,WORLD)));
  BOOST_CHECK(ac_align.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED)));
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2018-2019 CNRS INRIA
3			//
4
5			#include "pinocchio/multibody/model.hpp"
6			#include "pinocchio/multibody/data.hpp"
7			#include "pinocchio/algorithm/kinematics.hpp"
8			#include "pinocchio/algorithm/crba.hpp"
9			#include "pinocchio/algorithm/joint-configuration.hpp"
10			#include "pinocchio/parsers/sample-models.hpp"
11
12			#include <iostream>
13
14			#include <boost/test/unit_test.hpp>
15			#include <boost/utility/binary.hpp>
16
17			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
18
19	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
20			{
21			using namespace Eigen;
22			using namespace pinocchio;
23
24	✓✗	4	Model model;
25	✓✗	2	buildModels::humanoidRandom(model);
26
27	✓✗	4	Data data(model);
28
29	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
30	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
31	✓✗	2	VectorXd q = randomConfiguration(model);
32
33	✓✗✓✗	2	forwardKinematics(model,data,Model::ConfigVectorType::Ones(model.nq));
34		2	}
35
36	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_kinematics_zero)
37			{
38			using namespace Eigen;
39			using namespace pinocchio;
40
41	✓✗	4	Model model;
42	✓✗	2	buildModels::humanoidRandom(model);
43
44	✓✗✓✗	4	Data data(model), data_ref(model);
45
46	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
47	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
48	✓✗	4	VectorXd q = randomConfiguration(model);
49
50	✓✗	2	forwardKinematics(model,data_ref,q);
51	✓✗	2	crba(model,data,q);
52	✓✗	2	updateGlobalPlacements(model,data);
53
54	✓✓	56	for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
55			{
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]);
57	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(data.liMi[i] == data_ref.liMi[i]);
58			}
59		2	}
60
61	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_kinematics_first)
62			{
63			using namespace Eigen;
64			using namespace pinocchio;
65
66	✓✗	4	Model model;
67	✓✗	2	buildModels::humanoidRandom(model);
68
69	✓✗	4	Data data(model);
70
71	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
72	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
73	✓✗	4	VectorXd q = randomConfiguration(model);
74	✓✗✓✗	4	VectorXd v(VectorXd::Zero(model.nv));
75
76	✓✗	2	forwardKinematics(model,data,q,v);
77
78	✓✓	56	for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
79			{
80	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(data.v[i] == Motion::Zero());
81			}
82		2	}
83
84	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_kinematics_second)
85			{
86			using namespace Eigen;
87			using namespace pinocchio;
88
89	✓✗	4	Model model;
90	✓✗	2	buildModels::humanoidRandom(model);
91
92	✓✗	4	Data data(model);
93
94	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
95	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
96	✓✗	4	VectorXd q = randomConfiguration(model);
97	✓✗✓✗	4	VectorXd v(VectorXd::Zero(model.nv));
98	✓✗✓✗	4	VectorXd a(VectorXd::Zero(model.nv));
99
100	✓✗	2	forwardKinematics(model,data,q,v,a);
101
102	✓✓	56	for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
103			{
104	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(data.v[i] == Motion::Zero());
105	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(data.a[i] == Motion::Zero());
106			}
107		2	}
108
109	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_get_velocity)
110			{
111			using namespace Eigen;
112			using namespace pinocchio;
113
114	✓✗	4	Model model;
115	✓✗	2	buildModels::humanoidRandom(model);
116
117	✓✗	4	Data data(model);
118
119	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
120	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
121	✓✗	4	VectorXd q = randomConfiguration(model);
122	✓✗✓✗	4	VectorXd v(VectorXd::Random(model.nv));
123
124	✓✗	2	forwardKinematics(model,data,q,v);
125
126	✓✓	56	for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
127			{
128	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i)));
129	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i,LOCAL)));
130	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model,data,i,WORLD)));
131	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.v[i]).isApprox(getVelocity(model,data,i,LOCAL_WORLD_ALIGNED)));
132			}
133		2	}
134
135	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_get_acceleration)
136			{
137			using namespace Eigen;
138			using namespace pinocchio;
139
140	✓✗	4	Model model;
141	✓✗	2	buildModels::humanoidRandom(model);
142
143	✓✗	4	Data data(model);
144
145	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
146	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
147	✓✗	4	VectorXd q = randomConfiguration(model);
148	✓✗✓✗	4	VectorXd v(VectorXd::Random(model.nv));
149	✓✗✓✗	4	VectorXd a(VectorXd::Random(model.nv));
150
151	✓✗	2	forwardKinematics(model,data,q,v,a);
152
153	✓✓	56	for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
154			{
155	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i)));
156	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i,LOCAL)));
157	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model,data,i,WORLD)));
158	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.a[i]).isApprox(getAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
159			}
160		2	}
161
162	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_get_classical_acceleration)
163			{
164			using namespace Eigen;
165			using namespace pinocchio;
166
167	✓✗	4	Model model;
168	✓✗	2	buildModels::humanoidRandom(model);
169
170	✓✗	4	Data data(model);
171
172	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
173	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1.);
174	✓✗	4	VectorXd q = randomConfiguration(model);
175	✓✗✓✗	4	VectorXd v(VectorXd::Random(model.nv));
176	✓✗✓✗	4	VectorXd a(VectorXd::Random(model.nv));
177
178	✓✗	2	forwardKinematics(model,data,q,v,a);
179
180	✓✓	56	for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
181			{
182	✓✗	54	SE3 T = data.oMi[i];
183	✓✗	54	Motion vel = data.v[i];
184	✓✗	54	Motion acc = data.a[i];
185	✓✗	54	Vector3d linear;
186
187	✓✗	54	Motion acc_classical_local = acc;
188	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	54	linear = acc.linear() + vel.angular().cross(vel.linear());
189	✓✗✓✗	54	acc_classical_local.linear() = linear;
190
191	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i)));
192	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i,LOCAL)));
193
194	✓✗	54	Motion vel_world = T.act(vel);
195	✓✗	54	Motion acc_classical_world = T.act(acc);
196	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	54	linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear());
197	✓✗✓✗	54	acc_classical_world.linear() = linear;
198
199	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model,data,i,WORLD)));
200
201	✓✗✓✗ ✓✗✓✗	54	Motion vel_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(vel);
202	✓✗✓✗ ✓✗✓✗	54	Motion acc_classical_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(acc);
203	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	54	linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear());
204	✓✗✓✗	54	acc_classical_aligned.linear() = linear;
205
206	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(acc_classical_aligned.isApprox(getClassicalAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
207			}
208		2	}
209
210	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_kinematic_getters)
211			{
212			using namespace Eigen;
213			using namespace pinocchio;
214
215			// Build a simple 2R planar model
216	✓✗	4	Model model;
217		2	JointIndex jointId = 0;
218	✓✗✓✗ ✓✗✓✗ ✓✗	2	jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
219	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	jointId = model.addJoint(jointId, JointModelRZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");
220
221	✓✗	4	Data data(model);
222
223			// Predetermined configuration values
224	✓✗	4	VectorXd q(model.nq);
225	✓✗✓✗	2	q << M_PI/2.0, 0.0;
226
227	✓✗	4	VectorXd v(model.nv);
228	✓✗✓✗	2	v << 1.0, 0.0;
229
230	✓✗	4	VectorXd a(model.nv);
231	✓✗✓✗	2	a << 0.0, 0.0;
232
233			// Expected velocity
234	✓✗	2	Motion v_local;
235	✓✗✓✗ ✓✗	2	v_local.linear() = Vector3d(0.0, 1.0, 0.0);
236	✓✗✓✗ ✓✗	2	v_local.angular() = Vector3d(0.0, 0.0, 1.0);
237
238	✓✗	2	Motion v_world;
239	✓✗✓✗ ✓✗	2	v_world.linear() = Vector3d::Zero();
240	✓✗✓✗ ✓✗	2	v_world.angular() = Vector3d(0.0, 0.0, 1.0);
241
242	✓✗	2	Motion v_align;
243	✓✗✓✗ ✓✗	2	v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
244	✓✗✓✗ ✓✗	2	v_align.angular() = Vector3d(0.0, 0.0, 1.0);
245
246			// Expected classical acceleration
247	✓✗	2	Motion ac_local;
248	✓✗✓✗ ✓✗	2	ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
249	✓✗✓✗ ✓✗	2	ac_local.angular() = Vector3d::Zero();
250
251	✓✗	2	Motion ac_world = Motion::Zero();
252
253	✓✗	2	Motion ac_align;
254	✓✗✓✗ ✓✗	2	ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
255	✓✗✓✗ ✓✗	2	ac_align.angular() = Vector3d::Zero();
256
257			// Perform kinematics
258	✓✗	2	forwardKinematics(model,data,q,v,a);
259
260			// Check output velocity
261	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId)));
262	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId,LOCAL)));
263	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(v_world.isApprox(getVelocity(model,data,jointId,WORLD)));
264	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(v_align.isApprox(getVelocity(model,data,jointId,LOCAL_WORLD_ALIGNED)));
265
266			// Check output acceleration (all zero)
267	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(getAcceleration(model,data,jointId).isZero());
268	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL).isZero());
269	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(getAcceleration(model,data,jointId,WORLD).isZero());
270	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED).isZero());
271
272			// Check output classical
273	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId)));
274	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL)));
275	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model,data,jointId,WORLD)));
276	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(ac_align.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED)));
277		2	}
278
279			BOOST_AUTO_TEST_SUITE_END()