GCC Code Coverage Report

Directory:	./
File:	examples/casadi/casadi-crba.cpp
Date:	2025-04-30 16:14:33

	Exec	Total	Coverage
Lines:	31	31	100.0%
Branches:	63	122	51.6%

Line	Branch	Exec	Source
1			#include "pinocchio/autodiff/casadi.hpp"
2
3			#include "pinocchio/multibody/sample-models.hpp"
4
5			#include "pinocchio/algorithm/crba.hpp"
6			#include "pinocchio/algorithm/joint-configuration.hpp"
7
8		1	int main(int /argc/, char ** /argv/)
9			{
10			using namespace pinocchio;
11
12			typedef double Scalar;
13			typedef ::casadi::SX ADScalar;
14
15			typedef ModelTpl<Scalar> Model;
16			typedef Model::Data Data;
17
18			typedef ModelTpl<ADScalar> ADModel;
19			typedef ADModel::Data ADData;
20
21			// Create a random humanoid model
22	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Model model;
23	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	buildModels::humanoidRandom(model);
24	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	model.lowerPositionLimit.head<3>().fill(-1.);
25	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	model.upperPositionLimit.head<3>().fill(1.);
26	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Data data(model);
27
28			// Pick up random configuration, velocity and acceleration vectors.
29	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	Eigen::VectorXd q(model.nq);
30	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	q = randomConfiguration(model);
31
32			// Create CasADi model and data from model
33			typedef ADModel::ConfigVectorType ConfigVectorAD;
34	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	ADModel ad_model = model.cast<ADScalar>();
35	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	ADData ad_data(ad_model);
36
37			// Create symbolic CasADi vectors
38	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	2	::casadi::SX cs_q = ::casadi::SX::sym("q", model.nq);
39	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	ConfigVectorAD q_ad(model.nq);
40	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken.	1	q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
41
42			// Build CasADi function
43	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	crba(ad_model, ad_data, q_ad, pinocchio::Convention::WORLD);
44	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	ad_data.M.triangularView<Eigen::StrictlyLower>() =
45	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
46	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	::casadi::SX M_ad(model.nv, model.nv);
47	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	33	for (Eigen::DenseIndex j = 0; j < model.nv; ++j)
48			{
49	2/2 ✓ Branch 0 taken 1024 times. ✓ Branch 1 taken 32 times.	1056	for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
50			{
51	3/6 ✓ Branch 1 taken 1024 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1024 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1024 times. ✗ Branch 8 not taken.	1024	M_ad(i, j) = ad_data.M(i, j);
52			}
53			}
54
55	6/12 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken.	6	::casadi::Function eval_crba("eval_crba", ::casadi::SXVector{cs_q}, ::casadi::SXVector{M_ad});
56
57			// Evaluate CasADi expression with real value
58	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	std::vector<double> q_vec((size_t)model.nq);
59	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	1	Eigen::Map<Eigen::VectorXd>(q_vec.data(), model.nq, 1) = q;
60
61	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken.	3	::casadi::DM M_res = eval_crba(::casadi::DMVector{q_vec})[0];
62			Data::MatrixXs M_mat =
63	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken.	1	Eigen::Map<Data::MatrixXs>(static_cast<std::vector<double>>(M_res).data(), model.nv, model.nv);
64
65			// Eval CRBA using classic Pinocchio model
66	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	pinocchio::crba(model, data, q);
67	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	data.M.triangularView<Eigen::StrictlyLower>() =
68	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	data.M.transpose().triangularView<Eigen::StrictlyLower>();
69
70			// Print both results
71	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	1	std::cout << "pinocchio double:\n" << "\tM =\n" << data.M << std::endl;
72	5/10 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken.	1	std::cout << "pinocchio CasADi:\n" << "\tM =\n" << M_mat.transpose() << std::endl;
73		1	}
74

1

#include "pinocchio/autodiff/casadi.hpp"

2

3

#include "pinocchio/multibody/sample-models.hpp"

4

5

#include "pinocchio/algorithm/crba.hpp"

6

#include "pinocchio/algorithm/joint-configuration.hpp"

7

8

1

int main(int /*argc*/, char ** /*argv*/)

9

{

10

using namespace pinocchio;

11

12

typedef double Scalar;

13

typedef ::casadi::SX ADScalar;

14

15

typedef ModelTpl<Scalar> Model;

16

typedef Model::Data Data;

17

18

typedef ModelTpl<ADScalar> ADModel;

19

typedef ADModel::Data ADData;

20

21

// Create a random humanoid model

22

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

Model model;

23

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

buildModels::humanoidRandom(model);

24

2/4

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

1

model.lowerPositionLimit.head<3>().fill(-1.);

25

2/4

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

1

model.upperPositionLimit.head<3>().fill(1.);

26

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

Data data(model);

27

28

// Pick up random configuration, velocity and acceleration vectors.

29

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

Eigen::VectorXd q(model.nq);

30

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

q = randomConfiguration(model);

31

32

// Create CasADi model and data from model

33

typedef ADModel::ConfigVectorType ConfigVectorAD;

34

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

ADModel ad_model = model.cast<ADScalar>();

35

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

ADData ad_data(ad_model);

36

37

// Create symbolic CasADi vectors

38

2/4

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 1 times.

✗ Branch 6 not taken.

2

::casadi::SX cs_q = ::casadi::SX::sym("q", model.nq);

39

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

ConfigVectorAD q_ad(model.nq);

40

4/8

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

✓ Branch 8 taken 1 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 1 times.

✗ Branch 12 not taken.

1

q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);

41

42

// Build CasADi function

43

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

crba(ad_model, ad_data, q_ad, pinocchio::Convention::WORLD);

44

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

ad_data.M.triangularView<Eigen::StrictlyLower>() =

45

3/6

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 1 times.

✗ Branch 8 not taken.

2

ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();

46

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

::casadi::SX M_ad(model.nv, model.nv);

47

2/2

✓ Branch 0 taken 32 times.

✓ Branch 1 taken 1 times.

33

for (Eigen::DenseIndex j = 0; j < model.nv; ++j)

48

{

49

2/2

✓ Branch 0 taken 1024 times.

✓ Branch 1 taken 32 times.

1056

for (Eigen::DenseIndex i = 0; i < model.nv; ++i)

50

{

51

3/6

✓ Branch 1 taken 1024 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1024 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 1024 times.

✗ Branch 8 not taken.

1024

M_ad(i, j) = ad_data.M(i, j);

}

}

6/12

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

✓ Branch 6 taken 1 times.

✗ Branch 7 not taken.

✓ Branch 9 taken 1 times.

✗ Branch 10 not taken.

✓ Branch 13 taken 1 times.

✗ Branch 14 not taken.

✓ Branch 17 taken 1 times.

✗ Branch 18 not taken.

✓ Branch 20 taken 1 times.

✗ Branch 21 not taken.

6

::casadi::Function eval_crba("eval_crba", ::casadi::SXVector{cs_q}, ::casadi::SXVector{M_ad});

56

57

// Evaluate CasADi expression with real value

58

1/2

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

1

std::vector<double> q_vec((size_t)model.nq);

59

3/6

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 1 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 1 times.

✗ Branch 9 not taken.

1

Eigen::Map<Eigen::VectorXd>(q_vec.data(), model.nq, 1) = q;

60

61

4/8

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 1 times.

✗ Branch 6 not taken.

✓ Branch 8 taken 1 times.

✗ Branch 9 not taken.

✓ Branch 12 taken 1 times.

✗ Branch 13 not taken.

3

::casadi::DM M_res = eval_crba(::casadi::DMVector{q_vec})[0];

62

Data::MatrixXs M_mat =

63

4/8

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

✓ Branch 8 taken 1 times.

✗ Branch 9 not taken.

✓ Branch 11 taken 1 times.

✗ Branch 12 not taken.

1

Eigen::Map<Data::MatrixXs>(static_cast<std::vector<double>>(M_res).data(), model.nv, model.nv);

64

65

// Eval CRBA using classic Pinocchio model

66

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

pinocchio::crba(model, data, q);

67

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

data.M.triangularView<Eigen::StrictlyLower>() =

68

3/6

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 1 times.

✗ Branch 8 not taken.

2

data.M.transpose().triangularView<Eigen::StrictlyLower>();

69

70

// Print both results

71

4/8

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 1 times.

✗ Branch 8 not taken.

✓ Branch 10 taken 1 times.

✗ Branch 11 not taken.

1

std::cout << "pinocchio double:\n" << "\tM =\n" << data.M << std::endl;

72

5/10

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 1 times.

✗ Branch 8 not taken.

✓ Branch 10 taken 1 times.

✗ Branch 11 not taken.

✓ Branch 13 taken 1 times.

✗ Branch 14 not taken.

1

std::cout << "pinocchio CasADi:\n" << "\tM =\n" << M_mat.transpose() << std::endl;

73

1

}

74