GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/joint-translation.cpp Lines: 74 74 100.0 %
Date: 2024-04-26 13:14:21 Branches: 310 620 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(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()