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