GCC Code Coverage Report

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

	Exec	Total	Coverage
Lines:	81	81	100.0%
Branches:	311	622	50.0%

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		4	void addJointAndBody(
21			Model & model,
22			const JointModelBase<D> & jmodel,
23			const Model::JointIndex parent_id,
24			const SE3 & joint_placement,
25			const std::string & joint_name,
26			const Inertia & Y)
27			{
28			Model::JointIndex idx;
29
30	1/2 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken.	4	idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
31	1/2 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken.	4	model.appendBodyToJoint(idx, Y);
32		4	}
33
34			BOOST_AUTO_TEST_SUITE(JointTranslation)
35
36	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)
37			{
38			typedef TransformTranslationTpl<double, 0> TransformTranslation;
39			typedef SE3::Vector3 Vector3;
40
41	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const Vector3 displacement(Vector3::Random());
42	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());
43
44	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TransformTranslation Mtrans(displacement);
45	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Mplain = Mtrans;
46	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(Mplain.translation().isApprox(displacement));
47	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 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 29 not taken. ✓ Branch 30 taken 1 times.	2	BOOST_CHECK(Mplain.rotation().isIdentity());
48	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 * Mtrans));
49
50	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	SE3 M(SE3::Random());
51	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Motion v(Motion::Random());
52
53	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	MotionTranslation mp(MotionTranslation::Vector3(1., 2., 3.));
54	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Motion mp_dense(mp);
55
56	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(mp).isApprox(M.act(mp_dense)));
57	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(mp).isApprox(M.actInv(mp_dense)));
58
59	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(mp).isApprox(v.cross(mp_dense)));
60		2	}
61
62	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(vsFreeFlyer)
63			{
64			using namespace pinocchio;
65			typedef SE3::Vector3 Vector3;
66			typedef Eigen::Matrix<double, 6, 1> Vector6;
67			typedef Eigen::Matrix<double, 7, 1> VectorFF;
68			typedef SE3::Matrix3 Matrix3;
69
70	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Model modelTranslation, modelFreeflyer;
71
72	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.5, 0., 0.0), Matrix3::Identity());
73	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	SE3 pos(1);
74	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.);
75
76	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	6	addJointAndBody(
77	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	4	modelTranslation, JointModelTranslation(), 0, SE3::Identity(), "translation", inertia);
78	4/8 ✓ 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. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken.	2	addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, SE3::Identity(), "free-flyer", inertia);
79
80	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataTranslation(modelTranslation);
81	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataFreeFlyer(modelFreeflyer);
82
83	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd q = Eigen::VectorXd::Ones(modelTranslation.nq);
84	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorFF qff;
85	7/14 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken.	2	qff << 1, 1, 1, 0, 0, 0, 1;
86	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd v = Eigen::VectorXd::Ones(modelTranslation.nv);
87	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Vector6 vff;
88	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	vff << 1, 1, 1, 0, 0, 0;
89	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauTranslation = Eigen::VectorXd::Ones(modelTranslation.nv);
90	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::VectorXd tauff(6);
91	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	tauff << 1, 1, 1, 0, 0, 0;
92	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd aTranslation = Eigen::VectorXd::Ones(modelTranslation.nv);
93	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::VectorXd aff(vff);
94
95	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelTranslation, dataTranslation, q, v);
96	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
97
98	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelTranslation, dataTranslation, q, v);
99	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
100
101	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(dataFreeFlyer.oMi[1].isApprox(dataTranslation.oMi[1]));
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 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(dataFreeFlyer.liMi[1].isApprox(dataTranslation.liMi[1]));
103	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(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataTranslation.Ycrb[1].matrix()));
104	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(dataFreeFlyer.f[1].toVector().isApprox(dataTranslation.f[1].toVector()));
105
106	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::VectorXd nle_expected_ff(3);
107	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	nle_expected_ff << dataFreeFlyer.nle[0], dataFreeFlyer.nle[1], dataFreeFlyer.nle[2];
108	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(nle_expected_ff.isApprox(dataTranslation.nle));
109	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(dataFreeFlyer.com[0].isApprox(dataTranslation.com[0]));
110
111			// InverseDynamics == rnea
112	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauTranslation = rnea(modelTranslation, dataTranslation, q, v, aTranslation);
113	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
114
115	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Vector3 tau_expected;
116	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	tau_expected << tauff(0), tauff(1), tauff(2);
117	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(tauTranslation.isApprox(tau_expected));
118
119			// ForwardDynamics == aba
120			Eigen::VectorXd aAbaTranslation =
121	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aba(modelTranslation, dataTranslation, q, v, tauTranslation, Convention::WORLD);
122			Eigen::VectorXd aAbaFreeFlyer =
123	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aba(modelFreeflyer, dataFreeFlyer, qff, vff, tauff, Convention::WORLD);
124	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Vector3 a_expected;
125	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	a_expected << aAbaFreeFlyer[0], aAbaFreeFlyer[1], aAbaFreeFlyer[2];
126	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(aAbaTranslation.isApprox(a_expected));
127
128			// crba
129	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelTranslation, dataTranslation, q, Convention::WORLD);
130	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelFreeflyer, dataFreeFlyer, qff, Convention::WORLD);
131
132	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.topLeftCorner<3, 3>());
133
134	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(dataTranslation.M.isApprox(M_expected));
135
136			// Jacobian
137	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_translation;
138	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobian_translation.resize(6, 3);
139	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobian_translation.setZero();
140	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;
141	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobian_ff.resize(6, 6);
142	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobian_ff.setZero();
143	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelTranslation, dataTranslation, q);
144	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
145	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getJointJacobian(modelTranslation, dataTranslation, 1, LOCAL, jacobian_translation);
146	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);
147
148	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, 3> jacobian_expected;
149	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	jacobian_expected << jacobian_ff.col(0), jacobian_ff.col(1), jacobian_ff.col(2);
150	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(jacobian_translation.isApprox(jacobian_expected));
151		2	}
152
153			BOOST_AUTO_TEST_SUITE_END()
154