GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/energy.cpp Lines: 25 25 100.0 %
Date: 2024-04-26 13:14:21 Branches: 109 218 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2016-2020 CNRS INRIA
3
//
4
5
#include "pinocchio/algorithm/energy.hpp"
6
#include "pinocchio/algorithm/crba.hpp"
7
#include "pinocchio/algorithm/joint-configuration.hpp"
8
#include "pinocchio/algorithm/center-of-mass.hpp"
9
10
#include "pinocchio/parsers/sample-models.hpp"
11
12
#include <boost/test/unit_test.hpp>
13
#include <boost/utility/binary.hpp>
14
#include <boost/test/tools/floating_point_comparison.hpp>
15
16
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
17
18
















4
BOOST_AUTO_TEST_CASE(test_kinetic_energy)
19
{
20
  using namespace Eigen;
21
  using namespace pinocchio;
22
23
4
  pinocchio::Model model;
24
2
  pinocchio::buildModels::humanoidRandom(model);
25
4
  pinocchio::Data data(model);
26
27

4
  const VectorXd qmax = VectorXd::Ones(model.nq);
28

4
  VectorXd q = randomConfiguration(model,-qmax,qmax);
29

4
  VectorXd v = VectorXd::Ones(model.nv);
30
31

2
  data.M.fill(0);  crba(model,data,q);
32
2
  data.M.triangularView<Eigen::StrictlyLower>()
33

4
  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
34
35


2
  double kinetic_energy_ref = 0.5 * v.transpose() * data.M * v;
36
2
  double kinetic_energy = computeKineticEnergy(model, data, q, v);
37
38


2
  BOOST_CHECK_SMALL(kinetic_energy_ref - kinetic_energy, 1e-12);
39
2
}
40
41
















4
BOOST_AUTO_TEST_CASE(test_potential_energy)
42
{
43
  using namespace Eigen;
44
  using namespace pinocchio;
45
46
4
  pinocchio::Model model;
47
2
  pinocchio::buildModels::humanoidRandom(model);
48

4
  pinocchio::Data data(model), data_ref(model);
49
50

4
  const VectorXd qmax = VectorXd::Ones(model.nq);
51

4
  VectorXd q = randomConfiguration(model,-qmax,qmax);
52
53
2
  double potential_energy = computePotentialEnergy(model, data, q);
54
2
  centerOfMass(model,data_ref,q);
55
56

2
  double potential_energy_ref = -data_ref.mass[0] * (data_ref.com[0].dot(model.gravity.linear()));
57
58


2
  BOOST_CHECK_SMALL(potential_energy_ref - potential_energy, 1e-12);
59
2
}
60
61
BOOST_AUTO_TEST_SUITE_END()