GCC Code Coverage Report

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

	Exec	Total	Coverage
Lines:	117	117	100.0%
Branches:	407	814	50.0%

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2023 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			#include <iostream>
15
16			using namespace pinocchio;
17			using namespace Eigen;
18
19			template<typename D>
20		8	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 4 times. ✗ Branch 3 not taken.	8	idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
31	1/2 ✓ Branch 2 taken 4 times. ✗ Branch 3 not taken.	8	model.appendBodyToJoint(idx, Y);
32		8	}
33
34			BOOST_AUTO_TEST_SUITE(JointUniversal)
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(vsRXRY)
37			{
38			typedef SE3::Vector3 Vector3;
39			typedef SE3::Matrix3 Matrix3;
40
41	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Vector3 axis1;
42	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	axis1 << 1.0, 0.0, 0.0;
43	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Vector3 axis2;
44	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	axis2 << 0.0, 1.0, 0.0;
45
46	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Model modelUniversal, modelRXRY;
47	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());
48
49	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	JointModelUniversal joint_model_U(axis1, axis2);
50	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(modelUniversal, joint_model_U, 0, SE3::Identity(), "universal", inertia);
51
52	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	JointModelComposite joint_model_RXRY;
53	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	joint_model_RXRY.addJoint(JointModelRX());
54	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	joint_model_RXRY.addJoint(JointModelRY());
55	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(modelRXRY, joint_model_RXRY, 0, SE3::Identity(), "rxry", inertia);
56
57	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataUniversal(modelUniversal);
58	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataRXRY(modelRXRY);
59
60	6/12 ✓ 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 22 not taken. ✓ Branch 23 taken 1 times.	2	BOOST_CHECK(modelUniversal.nv == modelRXRY.nv);
61	6/12 ✓ 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 22 not taken. ✓ Branch 23 taken 1 times.	2	BOOST_CHECK(modelUniversal.nq == modelRXRY.nq);
62
63	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(modelRXRY.nq);
64
65	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelRXRY, dataRXRY, q);
66	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelUniversal, dataUniversal, q);
67
68	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(dataUniversal.oMi.back().isApprox(dataRXRY.oMi.back()));
69	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(dataUniversal.liMi.back().isApprox(dataRXRY.liMi.back()));
70	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(dataUniversal.Ycrb.back().matrix().isApprox(dataRXRY.Ycrb.back().matrix()));
71
72	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(modelRXRY.nv);
73	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelRXRY, dataRXRY, q, v);
74	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelUniversal, dataUniversal, q, v);
75
76	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(dataUniversal.oMi.back().isApprox(dataRXRY.oMi.back()));
77	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(dataUniversal.liMi.back().isApprox(dataRXRY.liMi.back()));
78	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(dataUniversal.Ycrb.back().matrix().isApprox(dataRXRY.Ycrb.back().matrix()));
79
80	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelRXRY, dataRXRY, q, v);
81	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelUniversal, dataUniversal, q, v);
82
83	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(dataUniversal.com.back().isApprox(dataRXRY.com.back()));
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 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(dataUniversal.nle.isApprox(dataRXRY.nle));
85	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(dataUniversal.f.back().toVector().isApprox(dataRXRY.f.back().toVector()));
86
87			// InverseDynamics == rnea
88	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd a = Eigen::VectorXd::Ones(modelRXRY.nv);
89
90	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauRXRY = rnea(modelRXRY, dataRXRY, q, v, a);
91	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauUniversal = rnea(modelUniversal, dataUniversal, q, v, a);
92
93	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(tauUniversal.isApprox(tauRXRY));
94
95			// ForwardDynamics == aba
96	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd aAbaRXRY = aba(modelRXRY, dataRXRY, q, v, tauRXRY, Convention::WORLD);
97			Eigen::VectorXd aAbaUniversal =
98	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aba(modelUniversal, dataUniversal, q, v, tauUniversal, Convention::WORLD);
99
100	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(aAbaUniversal.isApprox(aAbaRXRY));
101
102			// CRBA
103	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelRXRY, dataRXRY, q, Convention::WORLD);
104	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelUniversal, dataUniversal, q, Convention::WORLD);
105
106	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(dataUniversal.M.isApprox(dataRXRY.M));
107
108			// Jacobian
109	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRXRY;
110	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianRXRY.resize(6, 2);
111	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianRXRY.setZero();
112	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianUniversal;
113	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianUniversal.resize(6, 2);
114	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianUniversal.setZero();
115
116	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelRXRY, dataRXRY, q);
117	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelUniversal, dataUniversal, q);
118	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getJointJacobian(modelRXRY, dataRXRY, 1, LOCAL, jacobianRXRY);
119	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getJointJacobian(modelUniversal, dataUniversal, 1, LOCAL, jacobianUniversal);
120
121	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(jacobianUniversal.isApprox(jacobianRXRY));
122		2	}
123
124	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(vsRandomAxis)
125			{
126			typedef SE3::Vector3 Vector3;
127			typedef SE3::Matrix3 Matrix3;
128
129	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Vector3 axis1;
130	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	axis1 << 0., 0., 1.;
131	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Vector3 axis2;
132	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	axis2 << -1., 0., 0.;
133
134	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Model modelUniversal, modelRandomAxis;
135	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Inertia inertia = Inertia::Random();
136
137	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	JointModelUniversal joint_model_U(axis1, axis2);
138	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(modelUniversal, joint_model_U, 0, SE3::Identity(), "universal", inertia);
139
140	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	JointModelComposite joint_model_RandomAxis;
141	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	joint_model_RandomAxis.addJoint(JointModelRevoluteUnaligned(axis1));
142	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	joint_model_RandomAxis.addJoint(JointModelRevoluteUnaligned(axis2));
143	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	6	addJointAndBody(
144	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	4	modelRandomAxis, joint_model_RandomAxis, 0, SE3::Identity(), "random_axis", inertia);
145
146	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataUniversal(modelUniversal);
147	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data dataRandomAxis(modelRandomAxis);
148
149	6/12 ✓ 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 22 not taken. ✓ Branch 23 taken 1 times.	2	BOOST_CHECK(modelUniversal.nv == modelRandomAxis.nv);
150	6/12 ✓ 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 22 not taken. ✓ Branch 23 taken 1 times.	2	BOOST_CHECK(modelUniversal.nq == modelRandomAxis.nq);
151
152	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(modelRandomAxis.nq);
153
154	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelRandomAxis, dataRandomAxis, q);
155	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelUniversal, dataUniversal, q);
156
157	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(dataUniversal.oMi.back().isApprox(dataRandomAxis.oMi.back()));
158	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(dataUniversal.liMi.back().isApprox(dataRandomAxis.liMi.back()));
159	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(dataUniversal.Ycrb.back().matrix().isApprox(dataRandomAxis.Ycrb.back().matrix()));
160
161	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(modelRandomAxis.nv);
162	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelRandomAxis, dataRandomAxis, q, v);
163	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(modelUniversal, dataUniversal, q, v);
164
165	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(dataUniversal.oMi.back().isApprox(dataRandomAxis.oMi.back()));
166	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(dataUniversal.liMi.back().isApprox(dataRandomAxis.liMi.back()));
167	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(dataUniversal.Ycrb.back().matrix().isApprox(dataRandomAxis.Ycrb.back().matrix()));
168
169	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelRandomAxis, dataRandomAxis, q, v);
170	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(modelUniversal, dataUniversal, q, v);
171
172	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(dataUniversal.com.back().isApprox(dataRandomAxis.com.back()));
173	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(dataUniversal.nle.isApprox(dataRandomAxis.nle));
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 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(dataUniversal.f.back().toVector().isApprox(dataRandomAxis.f.back().toVector()));
175
176			// InverseDynamics == rnea
177	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd a = Eigen::VectorXd::Ones(modelRandomAxis.nv);
178
179	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauRandomAxis = rnea(modelRandomAxis, dataRandomAxis, q, v, a);
180	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd tauUniversal = rnea(modelUniversal, dataUniversal, q, v, a);
181
182	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(tauUniversal.isApprox(tauRandomAxis));
183
184			// ForwardDynamics == aba
185			Eigen::VectorXd aAbaRandomAxis =
186	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aba(modelRandomAxis, dataRandomAxis, q, v, tauRandomAxis, Convention::WORLD);
187			Eigen::VectorXd aAbaUniversal =
188	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	aba(modelUniversal, dataUniversal, q, v, tauUniversal, Convention::WORLD);
189
190	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(aAbaUniversal.isApprox(aAbaRandomAxis));
191
192			// CRBA
193	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelRandomAxis, dataRandomAxis, q, Convention::WORLD);
194	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(modelUniversal, dataUniversal, q, Convention::WORLD);
195
196	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(dataUniversal.M.isApprox(dataRandomAxis.M));
197
198			// Jacobian
199	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRandomAxis;
200	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianRandomAxis.resize(6, 2);
201	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianRandomAxis.setZero();
202	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianUniversal;
203	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianUniversal.resize(6, 2);
204	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobianUniversal.setZero();
205
206	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelRandomAxis, dataRandomAxis, q);
207	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeJointJacobians(modelUniversal, dataUniversal, q);
208	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getJointJacobian(modelRandomAxis, dataRandomAxis, 1, LOCAL, jacobianRandomAxis);
209	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getJointJacobian(modelUniversal, dataUniversal, 1, LOCAL, jacobianUniversal);
210
211	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(jacobianUniversal.isApprox(jacobianRandomAxis));
212		2	}
213
214			BOOST_AUTO_TEST_SUITE_END()
215