GCC Code Coverage Report

Directory:	./
File:	examples/casadi/casadi-rnea.cpp
Date:	2025-02-12 21:03:38

	Exec	Total	Coverage
Lines:	39	39	100.0%
Branches:	84	166	50.6%

Line	Branch	Exec	Source
1			#include "pinocchio/autodiff/casadi.hpp"
2
3			#include "pinocchio/multibody/sample-models.hpp"
4
5			#include "pinocchio/algorithm/rnea.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	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
32	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	1	Eigen::VectorXd a(Eigen::VectorXd::Random(model.nv));
33
34			// Create CasADi model and data from model
35			typedef ADModel::ConfigVectorType ConfigVectorAD;
36			typedef ADModel::TangentVectorType TangentVectorAD;
37	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	ADModel ad_model = model.cast<ADScalar>();
38	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	ADData ad_data(ad_model);
39
40			// Create symbolic CasADi vectors
41	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);
42	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	ConfigVectorAD q_ad(model.nq);
43	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);
44
45	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	2	::casadi::SX cs_v = ::casadi::SX::sym("v", model.nv);
46	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	TangentVectorAD v_ad(model.nv);
47	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	v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
48
49	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	2	::casadi::SX cs_a = ::casadi::SX::sym("a", model.nv);
50	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	TangentVectorAD a_ad(model.nv);
51	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	a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
52
53			// Build CasADi function
54	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
55	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	::casadi::SX tau_ad(model.nv, 1);
56
57	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	33	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
58	3/6 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 32 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 32 times. ✗ Branch 8 not taken.	32	tau_ad(k) = ad_data.tau[k];
59
60			::casadi::Function eval_rnea(
61	8/16 ✓ 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 12 taken 1 times. ✗ Branch 13 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken.	8	"eval_rnea", ::casadi::SXVector{cs_q, cs_v, cs_a}, ::casadi::SXVector{tau_ad});
62
63			// Evaluate CasADi expression with real value
64	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	std::vector<double> q_vec((size_t)model.nq);
65	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;
66
67	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	std::vector<double> v_vec((size_t)model.nv);
68	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>(v_vec.data(), model.nv, 1) = v;
69
70	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	std::vector<double> a_vec((size_t)model.nv);
71	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>(a_vec.data(), model.nv, 1) = a;
72
73	6/12 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken.	5	::casadi::DM tau_casadi_res = eval_rnea(::casadi::DMVector{q_vec, v_vec, a_vec})[0];
74	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	3	Data::TangentVectorType tau_casadi_vec = Eigen::Map<Data::TangentVectorType>(
75	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	static_cast<std::vector<double>>(tau_casadi_res).data(), model.nv, 1);
76
77			// Eval RNEA using classic Pinocchio model
78	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	pinocchio::rnea(model, data, q, v, a);
79
80			// Print both results
81	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 double:\n" << "\ttau = " << data.tau.transpose() << std::endl;
82	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" << "\ttau = " << tau_casadi_vec.transpose() << std::endl;
83		1	}
84

1

#include "pinocchio/autodiff/casadi.hpp"

2

3

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

4

5

#include "pinocchio/algorithm/rnea.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

2/4

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

1

Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));

32

2/4

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

1

Eigen::VectorXd a(Eigen::VectorXd::Random(model.nv));

33

34

// Create CasADi model and data from model

35

typedef ADModel::ConfigVectorType ConfigVectorAD;

36

typedef ADModel::TangentVectorType TangentVectorAD;

37

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

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

38

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

ADData ad_data(ad_model);

39

40

// Create symbolic CasADi vectors

41

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);

42

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

ConfigVectorAD q_ad(model.nq);

43

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);

44

45

2/4

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 1 times.

✗ Branch 6 not taken.

2

::casadi::SX cs_v = ::casadi::SX::sym("v", model.nv);

46

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

TangentVectorAD v_ad(model.nv);

47

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

v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);

48

49

2/4

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

✓ Branch 5 taken 1 times.

✗ Branch 6 not taken.

2

::casadi::SX cs_a = ::casadi::SX::sym("a", model.nv);

50

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

TangentVectorAD a_ad(model.nv);

51

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

a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);

52

53

// Build CasADi function

54

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

rnea(ad_model, ad_data, q_ad, v_ad, a_ad);

55

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

::casadi::SX tau_ad(model.nv, 1);

56

57

2/2

✓ Branch 0 taken 32 times.

✓ Branch 1 taken 1 times.

33

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

58

3/6

✓ Branch 1 taken 32 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 32 times.

✗ Branch 5 not taken.

✓ Branch 7 taken 32 times.

✗ Branch 8 not taken.

32

tau_ad(k) = ad_data.tau[k];

59

60

::casadi::Function eval_rnea(

61

8/16

✓ 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 12 taken 1 times.

✗ Branch 13 not taken.

✓ Branch 15 taken 1 times.

✗ Branch 16 not taken.

✓ Branch 19 taken 1 times.

✗ Branch 20 not taken.

✓ Branch 23 taken 1 times.

✗ Branch 24 not taken.

✓ Branch 26 taken 1 times.

✗ Branch 27 not taken.

8

"eval_rnea", ::casadi::SXVector{cs_q, cs_v, cs_a}, ::casadi::SXVector{tau_ad});

62

63

// Evaluate CasADi expression with real value

64

1/2

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

1

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

65

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;

66

67

1/2

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

1

std::vector<double> v_vec((size_t)model.nv);

68

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>(v_vec.data(), model.nv, 1) = v;

69

70

1/2

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

1

std::vector<double> a_vec((size_t)model.nv);

71

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>(a_vec.data(), model.nv, 1) = a;

72

73

6/12

✓ 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 11 taken 1 times.

✗ Branch 12 not taken.

✓ Branch 14 taken 1 times.

✗ Branch 15 not taken.

✓ Branch 18 taken 1 times.

✗ Branch 19 not taken.

5

::casadi::DM tau_casadi_res = eval_rnea(::casadi::DMVector{q_vec, v_vec, a_vec})[0];

74

2/4

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 1 times.

✗ Branch 6 not taken.

3

Data::TangentVectorType tau_casadi_vec = Eigen::Map<Data::TangentVectorType>(

75

2/4

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 4 taken 1 times.

✗ Branch 5 not taken.

2

static_cast<std::vector<double>>(tau_casadi_res).data(), model.nv, 1);

76

77

// Eval RNEA using classic Pinocchio model

78

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

pinocchio::rnea(model, data, q, v, a);

79

80

// Print both results

81

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 double:\n" << "\ttau = " << data.tau.transpose() << std::endl;

82

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" << "\ttau = " << tau_casadi_vec.transpose() << std::endl;

83

1

}

84