GCC Code Coverage Report

Directory: ./
File: unittest/joint-planar.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 74 0.0%
Branches: 0 576 0.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 ✗ 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 ✗ idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
31 ✗ model.appendBodyToJoint(idx, Y);
32 }
33
34 BOOST_AUTO_TEST_SUITE(JointPlanar)
35
36 ✗ BOOST_AUTO_TEST_CASE(spatial)
37 {
38 ✗ SE3 M(SE3::Random());
39 ✗ Motion v(Motion::Random());
40
41 ✗ MotionPlanar mp(1., 2., 3.);
42 ✗ Motion mp_dense(mp);
43
44 ✗ BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
45 ✗ BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
46
47 ✗ BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
48 }
49
50 ✗ BOOST_AUTO_TEST_CASE(vsFreeFlyer)
51 {
52 using namespace pinocchio;
53 typedef SE3::Vector3 Vector3;
54 typedef Eigen::Matrix<double, 6, 1> Vector6;
55 typedef Eigen::Matrix<double, 4, 1> VectorPl;
56 typedef Eigen::Matrix<double, 7, 1> VectorFF;
57 typedef SE3::Matrix3 Matrix3;
58
59 ✗ Model modelPlanar, modelFreeflyer;
60
61 ✗ Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
62 ✗ SE3 pos(1);
63 ✗ pos.translation() = SE3::LinearType(1., 0., 0.);
64
65 ✗ addJointAndBody(modelPlanar, JointModelPlanar(), 0, SE3::Identity(), "planar", inertia);
66 ✗ addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, SE3::Identity(), "free-flyer", inertia);
67
68 ✗ Data dataPlanar(modelPlanar);
69 ✗ Data dataFreeFlyer(modelFreeflyer);
70
71 ✗ VectorPl q;
72 ✗ q << 1, 1, 0, 1; // Angle is PI /2;
73 ✗ VectorFF qff;
74 ✗ qff << 1, 1, 0, 0, 0, sqrt(2) / 2, sqrt(2) / 2;
75 ✗ Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPlanar.nv);
76 ✗ Vector6 vff;
77 ✗ vff << 1, 1, 0, 0, 0, 1;
78 ✗ Eigen::VectorXd tauPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
79 ✗ Eigen::VectorXd tauff = Eigen::VectorXd::Ones(modelFreeflyer.nv);
80 ✗ Eigen::VectorXd aPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
81 ✗ Eigen::VectorXd aff(vff);
82
83 ✗ forwardKinematics(modelPlanar, dataPlanar, q, v);
84 ✗ forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
85
86 ✗ computeAllTerms(modelPlanar, dataPlanar, q, v);
87 ✗ computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
88
89 ✗ BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataPlanar.oMi[1]));
90 ✗ BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataPlanar.liMi[1]));
91 ✗ BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataPlanar.Ycrb[1].matrix()));
92 ✗ BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataPlanar.f[1].toVector()));
93
94 ✗ Eigen::VectorXd nle_expected_ff(3);
95 ✗ nle_expected_ff << dataFreeFlyer.nle[0], dataFreeFlyer.nle[1], dataFreeFlyer.nle[5];
96 ✗ BOOST_CHECK(nle_expected_ff.isApprox(dataPlanar.nle));
97 ✗ BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataPlanar.com[0]));
98
99 // InverseDynamics == rnea
100 ✗ tauPlanar = rnea(modelPlanar, dataPlanar, q, v, aPlanar);
101 ✗ tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
102
103 ✗ Vector3 tau_expected;
104 ✗ tau_expected << tauff(0), tauff(1), tauff(5);
105 ✗ BOOST_CHECK(tauPlanar.isApprox(tau_expected));
106
107 // ForwardDynamics == aba
108 ✗ Eigen::VectorXd aAbaPlanar = aba(modelPlanar, dataPlanar, q, v, tauPlanar, Convention::WORLD);
109 Eigen::VectorXd aAbaFreeFlyer =
110 ✗ aba(modelFreeflyer, dataFreeFlyer, qff, vff, tauff, Convention::WORLD);
111 ✗ Vector3 a_expected;
112 ✗ a_expected << aAbaFreeFlyer[0], aAbaFreeFlyer[1], aAbaFreeFlyer[5];
113 ✗ BOOST_CHECK(aAbaPlanar.isApprox(a_expected));
114
115 // crba
116 ✗ crba(modelPlanar, dataPlanar, q, Convention::WORLD);
117 ✗ crba(modelFreeflyer, dataFreeFlyer, qff, Convention::WORLD);
118
119 ✗ Eigen::Matrix<double, 3, 3> M_expected;
120 ✗ M_expected.block<2, 2>(0, 0) = dataFreeFlyer.M.block<2, 2>(0, 0);
121 ✗ M_expected.block<1, 2>(2, 0) = dataFreeFlyer.M.block<1, 2>(5, 0);
122 ✗ M_expected.block<2, 1>(0, 2) = dataFreeFlyer.M.col(5).head<2>();
123 ✗ M_expected.block<1, 1>(2, 2) = dataFreeFlyer.M.col(5).tail<1>();
124
125 ✗ BOOST_CHECK(dataPlanar.M.isApprox(M_expected));
126
127 // Jacobian
128 ✗ Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;
129 ✗ jacobian_planar.resize(6, 3);
130 ✗ jacobian_planar.setZero();
131 ✗ Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;
132 ✗ jacobian_ff.resize(6, 6);
133 ✗ jacobian_ff.setZero();
134 ✗ computeJointJacobians(modelPlanar, dataPlanar, q);
135 ✗ computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
136 ✗ getJointJacobian(modelPlanar, dataPlanar, 1, LOCAL, jacobian_planar);
137 ✗ getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);
138
139 ✗ Eigen::Matrix<double, 6, 3> jacobian_expected;
140 ✗ jacobian_expected << jacobian_ff.col(0), jacobian_ff.col(1), jacobian_ff.col(5);
141
142 ✗ BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
143 }
144
145 BOOST_AUTO_TEST_SUITE_END()
146