GCC Code Coverage Report

Directory:	./
File:	unittest/joint-helical.cpp
Date:	2025-02-12 21:03:38

	Exec	Total	Coverage
Lines:	154	154	100.0%
Branches:	640	1280	50.0%

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2022 CNRS INRIA
3			//
4
5			#include "pinocchio/math/fwd.hpp"
6			#include "pinocchio/multibody/joint/joints.hpp"
7			#include "pinocchio/algorithm/rnea.hpp"
8			#include "pinocchio/algorithm/aba.hpp"
9			#include "pinocchio/algorithm/crba.hpp"
10			#include "pinocchio/algorithm/jacobian.hpp"
11			#include "pinocchio/algorithm/compute-all-terms.hpp"
12
13			#include <boost/test/unit_test.hpp>
14
15			using namespace pinocchio;
16
17			template<typename D>
18		10	void addJointAndBody(
19			Model & model,
20			const JointModelBase<D> & jmodel,
21			const Model::JointIndex parent_id,
22			const SE3 & joint_placement,
23			const std::string & joint_name,
24			const Inertia & Y)
25			{
26			Model::JointIndex idx;
27
28	1/2 ✓ Branch 2 taken 5 times. ✗ Branch 3 not taken.	10	idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
29	1/2 ✓ Branch 2 taken 5 times. ✗ Branch 3 not taken.	10	model.appendBodyToJoint(idx, Y);
30		10	}
31
32			BOOST_AUTO_TEST_SUITE(JointHelical)
33
34	33/66 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 41 taken 1 times. ✗ Branch 42 not taken. ✓ Branch 45 taken 1 times. ✗ Branch 46 not taken. ✓ Branch 48 taken 1 times. ✗ Branch 49 not taken. ✓ Branch 52 taken 1 times. ✗ Branch 53 not taken. ✓ Branch 55 taken 1 times. ✗ Branch 56 not taken. ✓ Branch 58 taken 1 times. ✗ Branch 59 not taken. ✓ Branch 61 taken 1 times. ✗ Branch 62 not taken. ✓ Branch 66 taken 1 times. ✗ Branch 67 not taken. ✓ Branch 70 taken 1 times. ✗ Branch 71 not taken. ✓ Branch 73 taken 1 times. ✗ Branch 74 not taken. ✓ Branch 77 taken 1 times. ✗ Branch 78 not taken. ✓ Branch 80 taken 1 times. ✗ Branch 81 not taken. ✓ Branch 83 taken 1 times. ✗ Branch 84 not taken. ✓ Branch 86 taken 1 times. ✗ Branch 87 not taken. ✓ Branch 91 taken 1 times. ✗ Branch 92 not taken. ✓ Branch 95 taken 1 times. ✗ Branch 96 not taken. ✓ Branch 98 taken 1 times. ✗ Branch 99 not taken. ✓ Branch 102 taken 1 times. ✗ Branch 103 not taken. ✓ Branch 105 taken 1 times. ✗ Branch 106 not taken. ✓ Branch 108 taken 1 times. ✗ Branch 109 not taken. ✓ Branch 111 taken 1 times. ✗ Branch 112 not taken. ✓ Branch 116 taken 1 times. ✗ Branch 117 not taken.	4	BOOST_AUTO_TEST_CASE(vsPXRX)
35			{
36			typedef SE3::Vector3 Vector3;
37			typedef SE3::Matrix3 Matrix3;
38
39	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Model modelHX, modelPXRX;
40
41	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	2	Inertia inertia(1., Vector3(0., 0., 0.0), Matrix3::Identity());
42			// Important to have the same mass for both systems, otherwise COM position not the same
43	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	2	Inertia inertia_zero_mass(0., Vector3(0.0, 0.0, 0.0), Matrix3::Identity());
44		2	const double h = 0.4;
45
46	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	JointModelHX joint_model_HX(h);
47	3/6 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	addJointAndBody(modelHX, joint_model_HX, 0, SE3::Identity(), "helical x", inertia);
48
49	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	JointModelPX joint_model_PX;
50	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	JointModelRX joint_model_RX;
51	3/6 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	addJointAndBody(modelPXRX, joint_model_PX, 0, SE3::Identity(), "prismatic x", inertia);
52	3/6 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	addJointAndBody(modelPXRX, joint_model_RX, 1, SE3::Identity(), "revolute x", inertia_zero_mass);
53
54	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataHX(modelHX);
55	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataPXRX(modelPXRX);
56
57			// Set the prismatic joint to corresponding displacement, velocit and acceleration
58	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd q_hx = Eigen::VectorXd::Ones(modelHX.nq); // dim 1
59	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd q_PXRX = Eigen::VectorXd::Ones(modelPXRX.nq); // dim 2
60	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	q_PXRX(0) = q_hx(0) * h;
61
62	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd v_hx = Eigen::VectorXd::Ones(modelHX.nv);
63	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd v_PXRX = Eigen::VectorXd::Ones(modelPXRX.nv);
64	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	v_PXRX(0) = v_hx(0) * h;
65
66	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauHX = Eigen::VectorXd::Ones(modelHX.nv);
67	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauPXRX = Eigen::VectorXd::Ones(modelPXRX.nv);
68	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd aHX = Eigen::VectorXd::Ones(modelHX.nv);
69	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd aPXRX = Eigen::VectorXd::Ones(modelPXRX.nv);
70	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aPXRX(0) = aHX(0) * h * h;
71
72	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelHX, dataHX, q_hx, v_hx);
73	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelPXRX, dataPXRX, q_PXRX, v_PXRX);
74
75	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelHX, dataHX, q_hx, v_hx);
76	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelPXRX, dataPXRX, q_PXRX, v_PXRX);
77
78	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	2	BOOST_CHECK(dataPXRX.oMi[2].isApprox(dataHX.oMi[1]));
79	8/16 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK((dataPXRX.liMi[2] * dataPXRX.liMi[1]).isApprox(dataHX.liMi[1]));
80	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✗ Branch 34 not taken. ✓ Branch 35 taken 1 times.	2	BOOST_CHECK(dataPXRX.Ycrb[2].matrix().isApprox(dataHX.Ycrb[1].matrix()));
81	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✗ Branch 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK((dataPXRX.liMi[2].actInv(dataPXRX.f[1])).toVector().isApprox(dataHX.f[1].toVector()));
82	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK(
83			(Eigen::Matrix<double, 1, 1>(dataPXRX.nle.dot(Eigen::VectorXd::Ones(2)))).isApprox(dataHX.nle));
84	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	2	BOOST_CHECK(dataPXRX.com[0].isApprox(dataHX.com[0]));
85
86			// InverseDynamics == rnea
87	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauHX = rnea(modelHX, dataHX, q_hx, v_hx, aHX);
88	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauPXRX = rnea(modelPXRX, dataPXRX, q_PXRX, v_PXRX, aPXRX);
89	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK(tauHX.isApprox(Eigen::Matrix<double, 1, 1>(tauPXRX.dot(Eigen::VectorXd::Ones(2)))));
90
91			// ForwardDynamics == aba
92	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd aAbaHX = aba(modelHX, dataHX, q_hx, v_hx, tauHX, Convention::WORLD);
93	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd aAbaPXRX = aba(modelPXRX, dataPXRX, q_PXRX, v_PXRX, tauPXRX, Convention::WORLD);
94
95	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(aAbaHX.isApprox(aHX));
96	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(aAbaPXRX.isApprox(aPXRX));
97	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK(aAbaPXRX.isApprox(Eigen::Matrix<double, 2, 1>(aHX(0) * h * h, aHX(0))));
98
99	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aAbaHX = aba(modelHX, dataHX, q_hx, v_hx, tauHX, Convention::LOCAL);
100	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aAbaPXRX = aba(modelPXRX, dataPXRX, q_PXRX, v_PXRX, tauPXRX, Convention::LOCAL);
101
102	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(aAbaHX.isApprox(aHX));
103	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(aAbaPXRX.isApprox(aPXRX));
104	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK(aAbaPXRX.isApprox(Eigen::Matrix<double, 2, 1>(aHX(0) * h * h, aHX(0))));
105
106			// crba
107	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelHX, dataHX, q_hx, Convention::WORLD);
108	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelPXRX, dataPXRX, q_PXRX, Convention::WORLD);
109
110	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauHX = dataHX.M * aHX;
111	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauPXRX = dataPXRX.M * aPXRX;
112
113	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK(tauHX.isApprox(Eigen::Matrix<double, 1, 1>(tauPXRX.dot(Eigen::VectorXd::Ones(2)))));
114
115	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelHX, dataHX, q_hx, Convention::LOCAL);
116	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelPXRX, dataPXRX, q_PXRX, Convention::LOCAL);
117
118	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauHX = dataHX.M * aHX;
119	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauPXRX = dataPXRX.M * aPXRX;
120
121	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK(tauHX.isApprox(Eigen::Matrix<double, 1, 1>(tauPXRX.dot(Eigen::VectorXd::Ones(2)))));
122
123			// Jacobian
124	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelHX, dataHX, q_hx);
125	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelPXRX, dataPXRX, q_PXRX);
126	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd v_body_hx = dataHX.J * v_hx;
127	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd v_body_PXRX = dataPXRX.J * v_PXRX;
128	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(v_body_hx.isApprox(v_body_PXRX));
129		2	}
130
131	33/66 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 41 taken 1 times. ✗ Branch 42 not taken. ✓ Branch 45 taken 1 times. ✗ Branch 46 not taken. ✓ Branch 48 taken 1 times. ✗ Branch 49 not taken. ✓ Branch 52 taken 1 times. ✗ Branch 53 not taken. ✓ Branch 55 taken 1 times. ✗ Branch 56 not taken. ✓ Branch 58 taken 1 times. ✗ Branch 59 not taken. ✓ Branch 61 taken 1 times. ✗ Branch 62 not taken. ✓ Branch 66 taken 1 times. ✗ Branch 67 not taken. ✓ Branch 70 taken 1 times. ✗ Branch 71 not taken. ✓ Branch 73 taken 1 times. ✗ Branch 74 not taken. ✓ Branch 77 taken 1 times. ✗ Branch 78 not taken. ✓ Branch 80 taken 1 times. ✗ Branch 81 not taken. ✓ Branch 83 taken 1 times. ✗ Branch 84 not taken. ✓ Branch 86 taken 1 times. ✗ Branch 87 not taken. ✓ Branch 91 taken 1 times. ✗ Branch 92 not taken. ✓ Branch 95 taken 1 times. ✗ Branch 96 not taken. ✓ Branch 98 taken 1 times. ✗ Branch 99 not taken. ✓ Branch 102 taken 1 times. ✗ Branch 103 not taken. ✓ Branch 105 taken 1 times. ✗ Branch 106 not taken. ✓ Branch 108 taken 1 times. ✗ Branch 109 not taken. ✓ Branch 111 taken 1 times. ✗ Branch 112 not taken. ✓ Branch 116 taken 1 times. ✗ Branch 117 not taken.	4	BOOST_AUTO_TEST_CASE(spatial)
132			{
133			typedef TransformHelicalTpl<double, 0, 0> TransformX;
134			typedef TransformHelicalTpl<double, 0, 1> TransformY;
135			typedef TransformHelicalTpl<double, 0, 2> TransformZ;
136
137			typedef SE3::Vector3 Vector3;
138
139		2	const double alpha = 0.2, h = 0.1;
140			double sin_alpha, cos_alpha;
141		2	SINCOS(alpha, &sin_alpha, &cos_alpha);
142	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	SE3 Mplain, Mrand(SE3::Random());
143
144		2	TransformX Mx(sin_alpha, cos_alpha, alpha * h);
145	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Mplain = Mx;
146	11/22 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✗ Branch 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK(Mplain.translation().isApprox(Vector3::UnitX() * alpha * h));
147	11/22 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✗ Branch 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK(
148			Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitX()).toRotationMatrix()));
149	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mx));
150
151		2	TransformY My(sin_alpha, cos_alpha, alpha * h);
152	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Mplain = My;
153	11/22 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✗ Branch 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK(Mplain.translation().isApprox(Vector3::UnitY() * alpha * h));
154	11/22 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✗ Branch 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK(
155			Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitY()).toRotationMatrix()));
156	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * My));
157
158		2	TransformZ Mz(sin_alpha, cos_alpha, alpha * h);
159	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Mplain = Mz;
160	11/22 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✗ Branch 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK(Mplain.translation().isApprox(Vector3::UnitZ() * alpha * h));
161	11/22 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✗ Branch 38 not taken. ✓ Branch 39 taken 1 times.	2	BOOST_CHECK(
162			Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitZ()).toRotationMatrix()));
163	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 35 not taken. ✓ Branch 36 taken 1 times.	2	BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mz));
164
165	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	SE3 M(SE3::Random());
166	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Motion v(Motion::Random());
167
168		2	MotionHelicalTpl<double, 0, 0> mh_x(2., h);
169	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Motion mh_dense_x(mh_x);
170
171	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(M.act(mh_x).isApprox(M.act(mh_dense_x)));
172	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(M.actInv(mh_x).isApprox(M.actInv(mh_dense_x)));
173
174	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(v.cross(mh_x).isApprox(v.cross(mh_dense_x)));
175
176		2	MotionHelicalTpl<double, 0, 1> mh_y(2., h);
177	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Motion mh_dense_y(mh_y);
178
179	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(M.act(mh_y).isApprox(M.act(mh_dense_y)));
180	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(M.actInv(mh_y).isApprox(M.actInv(mh_dense_y)));
181
182	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(v.cross(mh_y).isApprox(v.cross(mh_dense_y)));
183
184		2	MotionHelicalTpl<double, 0, 2> mh_z(2., h);
185	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Motion mh_dense_z(mh_z);
186
187	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(M.act(mh_z).isApprox(M.act(mh_dense_z)));
188	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(M.actInv(mh_z).isApprox(M.actInv(mh_dense_z)));
189
190	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 24 taken 1 times. ✗ Branch 25 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✗ Branch 32 not taken. ✓ Branch 33 taken 1 times.	2	BOOST_CHECK(v.cross(mh_z).isApprox(v.cross(mh_dense_z)));
191		2	}
192			BOOST_AUTO_TEST_SUITE_END()
193
194			BOOST_AUTO_TEST_SUITE(JointHelicalUnaligned)
195
196	33/66 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 41 taken 1 times. ✗ Branch 42 not taken. ✓ Branch 45 taken 1 times. ✗ Branch 46 not taken. ✓ Branch 48 taken 1 times. ✗ Branch 49 not taken. ✓ Branch 52 taken 1 times. ✗ Branch 53 not taken. ✓ Branch 55 taken 1 times. ✗ Branch 56 not taken. ✓ Branch 58 taken 1 times. ✗ Branch 59 not taken. ✓ Branch 61 taken 1 times. ✗ Branch 62 not taken. ✓ Branch 66 taken 1 times. ✗ Branch 67 not taken. ✓ Branch 70 taken 1 times. ✗ Branch 71 not taken. ✓ Branch 73 taken 1 times. ✗ Branch 74 not taken. ✓ Branch 77 taken 1 times. ✗ Branch 78 not taken. ✓ Branch 80 taken 1 times. ✗ Branch 81 not taken. ✓ Branch 83 taken 1 times. ✗ Branch 84 not taken. ✓ Branch 86 taken 1 times. ✗ Branch 87 not taken. ✓ Branch 91 taken 1 times. ✗ Branch 92 not taken. ✓ Branch 95 taken 1 times. ✗ Branch 96 not taken. ✓ Branch 98 taken 1 times. ✗ Branch 99 not taken. ✓ Branch 102 taken 1 times. ✗ Branch 103 not taken. ✓ Branch 105 taken 1 times. ✗ Branch 106 not taken. ✓ Branch 108 taken 1 times. ✗ Branch 109 not taken. ✓ Branch 111 taken 1 times. ✗ Branch 112 not taken. ✓ Branch 116 taken 1 times. ✗ Branch 117 not taken.	4	BOOST_AUTO_TEST_CASE(vsHX)
197			{
198			using namespace pinocchio;
199			typedef SE3::Vector3 Vector3;
200			typedef SE3::Matrix3 Matrix3;
201
202	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Vector3 axis;
203	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	axis << 1.0, 0.0, 0.0;
204		2	const double h = 0.2;
205
206	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Model modelHX, modelHelicalUnaligned;
207
208	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	2	Inertia inertia(1., Vector3(0.0, 0., 0.0), Matrix3::Identity());
209	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	SE3 pos(1);
210	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	pos.translation() = SE3::LinearType(1., 0., 0.);
211
212	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	JointModelHelicalUnaligned joint_model_HU(axis, h);
213
214	3/6 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	addJointAndBody(modelHX, JointModelHX(h), 0, pos, "HX", inertia);
215	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	2	addJointAndBody(modelHelicalUnaligned, joint_model_HU, 0, pos, "Helical-unaligned", inertia);
216
217	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataHX(modelHX);
218	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataHelicalUnaligned(modelHelicalUnaligned);
219
220	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	Eigen::VectorXd q = 3 * Eigen::VectorXd::Ones(modelHX.nq);
221	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	Eigen::VectorXd v = 30 * Eigen::VectorXd::Ones(modelHX.nv);
222	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauHX = Eigen::VectorXd::Ones(modelHX.nv);
223	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauHelicalUnaligned = Eigen::VectorXd::Ones(modelHelicalUnaligned.nv);
224	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	Eigen::VectorXd aHX = 5 * Eigen::VectorXd::Ones(modelHX.nv);
225	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::VectorXd aHelicalUnaligned(aHX);
226
227	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelHX, dataHX, q, v);
228	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelHelicalUnaligned, dataHelicalUnaligned, q, v);
229
230	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelHX, dataHX, q, v);
231	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelHelicalUnaligned, dataHelicalUnaligned, q, v);
232
233	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	2	BOOST_CHECK(dataHelicalUnaligned.oMi[1].isApprox(dataHX.oMi[1]));
234	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	2	BOOST_CHECK(dataHelicalUnaligned.liMi[1].isApprox(dataHX.liMi[1]));
235	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✗ Branch 34 not taken. ✓ Branch 35 taken 1 times.	2	BOOST_CHECK(dataHelicalUnaligned.Ycrb[1].matrix().isApprox(dataHX.Ycrb[1].matrix()));
236	9/18 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✗ Branch 34 not taken. ✓ Branch 35 taken 1 times.	2	BOOST_CHECK(dataHelicalUnaligned.f[1].toVector().isApprox(dataHX.f[1].toVector()));
237
238	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(dataHelicalUnaligned.nle.isApprox(dataHX.nle));
239	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 28 not taken. ✓ Branch 29 taken 1 times.	2	BOOST_CHECK(dataHelicalUnaligned.com[0].isApprox(dataHX.com[0]));
240
241			// InverseDynamics == rnea
242	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauHX = rnea(modelHX, dataHX, q, v, aHX);
243	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauHelicalUnaligned = rnea(modelHelicalUnaligned, dataHelicalUnaligned, q, v, aHelicalUnaligned);
244
245	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(tauHX.isApprox(tauHelicalUnaligned));
246
247			// ForwardDynamics == aba
248	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd aAbaHX = aba(modelHX, dataHX, q, v, tauHX, Convention::WORLD);
249			Eigen::VectorXd aAbaHelicalUnaligned =
250	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aba(modelHelicalUnaligned, dataHelicalUnaligned, q, v, tauHelicalUnaligned, Convention::WORLD);
251
252	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(aAbaHX.isApprox(aAbaHelicalUnaligned));
253
254			// crba
255	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelHX, dataHX, q, Convention::WORLD);
256	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelHelicalUnaligned, dataHelicalUnaligned, q, Convention::WORLD);
257
258	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(dataHX.M.isApprox(dataHelicalUnaligned.M));
259
260			// Jacobian
261	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;
262	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianPX.resize(6, 1);
263	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianPX.setZero();
264	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;
265	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianPrismaticUnaligned.resize(6, 1);
266	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianPrismaticUnaligned.setZero();
267	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelHX, dataHX, q);
268	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelHelicalUnaligned, dataHelicalUnaligned, q);
269	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getJointJacobian(modelHX, dataHX, 1, LOCAL, jacobianPX);
270	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getJointJacobian(
271			modelHelicalUnaligned, dataHelicalUnaligned, 1, LOCAL, jacobianPrismaticUnaligned);
272
273	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
274		2	}
275
276			BOOST_AUTO_TEST_SUITE_END()
277