GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/crba.cpp Lines: 41 41 100.0 %
Date: 2023-08-09 08:43:58 Branches: 153 302 50.7 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
//
4
5
/*
6
 * Test the CRBA algorithm. The code validates both the computation times and
7
 * the value by comparing the results of the CRBA with the reconstruction of
8
 * the mass matrix using the RNEA.
9
 * For a strong timing benchmark, see benchmark/timings.
10
 *
11
 */
12
13
#include "pinocchio/multibody/model.hpp"
14
#include "pinocchio/multibody/data.hpp"
15
#include "pinocchio/algorithm/crba.hpp"
16
#include "pinocchio/algorithm/centroidal.hpp"
17
#include "pinocchio/algorithm/rnea.hpp"
18
#include "pinocchio/algorithm/jacobian.hpp"
19
#include "pinocchio/algorithm/center-of-mass.hpp"
20
#include "pinocchio/algorithm/joint-configuration.hpp"
21
#include "pinocchio/parsers/sample-models.hpp"
22
#include "pinocchio/utils/timer.hpp"
23
24
#include <iostream>
25
26
#include <boost/test/unit_test.hpp>
27
#include <boost/utility/binary.hpp>
28
29
template<typename JointModel>
30
static void addJointAndBody(pinocchio::Model & model,
31
                            const pinocchio::JointModelBase<JointModel> & joint,
32
                            const std::string & parent_name,
33
                            const std::string & name,
34
                            const pinocchio::SE3 placement = pinocchio::SE3::Random(),
35
                            bool setRandomLimits = true)
36
{
37
  using namespace pinocchio;
38
  typedef typename JointModel::ConfigVector_t CV;
39
  typedef typename JointModel::TangentVector_t TV;
40
41
  Model::JointIndex idx;
42
43
  if (setRandomLimits)
44
    idx = model.addJoint(model.getJointId(parent_name),joint,
45
                         SE3::Random(),
46
                         name + "_joint",
47
                         TV::Random() + TV::Constant(1),
48
                         TV::Random() + TV::Constant(1),
49
                         CV::Random() - CV::Constant(1),
50
                         CV::Random() + CV::Constant(1)
51
                         );
52
  else
53
    idx = model.addJoint(model.getJointId(parent_name),joint,
54
                         placement, name + "_joint");
55
56
  model.addJointFrame(idx);
57
58
  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
59
  model.addBodyFrame(name + "_body", idx);
60
}
61
62
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
63
64
















4
BOOST_AUTO_TEST_CASE ( test_crba )
65
{
66
4
  pinocchio::Model model;
67
2
  pinocchio::buildModels::humanoidRandom(model);
68
4
  pinocchio::Data data(model);
69
70
  #ifdef NDEBUG
71
    #ifdef _INTENSE_TESTING_
72
      const size_t NBT = 1000*1000;
73
    #else
74
      const size_t NBT = 10;
75
    #endif
76
77
    Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
78
79
    PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
80
    SMOOTH(NBT)
81
      {
82
        crba(model,data,q);
83
      }
84
    timer.toc(std::cout,NBT);
85
86
  #else
87
2
    const size_t NBT = 1;
88
    using namespace Eigen;
89
    using namespace pinocchio;
90
91
4
    Eigen::MatrixXd M(model.nv,model.nv);
92

4
    Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
93

2
    q.segment <4> (3).normalize();
94

4
    Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
95

4
    Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
96

2
    data.M.fill(0);  crba(model,data,q);
97


2
    data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
98
99
    /* Joint inertia from iterative crba. */
100

4
    const Eigen::VectorXd bias = rnea(model,data,q,v,a);
101
66
    for(int i=0;i<model.nv;++i)
102
    {
103


64
      M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
104
    }
105
106
    // std::cout << "Mcrb = [ " << data.M << "  ];" << std::endl;
107
    // std::cout << "Mrne = [  " << M << " ]; " << std::endl;
108



2
    BOOST_CHECK(M.isApprox(data.M,1e-12));
109
110
2
    std::cout << "(the time score in debug mode is not relevant)  " ;
111
112

2
    q = Eigen::VectorXd::Zero(model.nq);
113
114

4
    PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
115
4
    SMOOTH(NBT)
116
    {
117
2
      crba(model,data,q);
118
    }
119
2
    timer.toc(std::cout,NBT);
120
121
  #endif // ifndef NDEBUG
122
123
2
}
124
125
















4
BOOST_AUTO_TEST_CASE(test_minimal_crba)
126
{
127
4
  pinocchio::Model model;
128
2
  pinocchio::buildModels::humanoidRandom(model);
129

4
  pinocchio::Data data(model), data_ref(model);
130
131

2
  model.lowerPositionLimit.head<7>().fill(-1.);
132

2
  model.upperPositionLimit.head<7>().fill(1.);
133
134
4
  Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
135

4
  Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
136
137
2
  crba(model,data_ref,q);
138


2
  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
139
140
2
  crbaMinimal(model,data,q);
141


2
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
142
143



2
  BOOST_CHECK(data.M.isApprox(data_ref.M));
144
145
2
  ccrba(model,data_ref,q,v);
146
2
  computeJointJacobians(model,data_ref,q);
147



2
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
148



2
  BOOST_CHECK(data.J.isApprox(data_ref.J));
149
150
2
}
151
152
BOOST_AUTO_TEST_SUITE_END ()
153