GCC Code Coverage Report

Directory:	./
File:	examples/casadi/casadi-aba.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/aba.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 tau(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_tau = ::casadi::SX::sym("tau", model.nv);
50	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	TangentVectorAD tau_ad(model.nv);
51			tau_ad =
52	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<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
53
54			// Build CasADi function
55	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	aba(ad_model, ad_data, q_ad, v_ad, tau_ad);
56	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	::casadi::SX a_ad(model.nv, 1);
57
58	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	33	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
59	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	a_ad(k) = ad_data.ddq[k];
60
61			::casadi::Function eval_aba(
62	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_aba", ::casadi::SXVector{cs_q, cs_v, cs_tau}, ::casadi::SXVector{a_ad});
63
64			// Evaluate CasADi expression with real value
65	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	std::vector<double> q_vec((size_t)model.nq);
66	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;
67
68	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	std::vector<double> v_vec((size_t)model.nv);
69	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;
70
71	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	1	std::vector<double> tau_vec((size_t)model.nv);
72	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>(tau_vec.data(), model.nv, 1) = tau;
73
74	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 a_casadi_res = eval_aba(::casadi::DMVector{q_vec, v_vec, tau_vec})[0];
75	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	3	Data::TangentVectorType a_casadi_vec = Eigen::Map<Data::TangentVectorType>(
76	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>>(a_casadi_res).data(), model.nv, 1);
77
78			// Eval ABA using classic Pinocchio model
79	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	1	pinocchio::aba(model, data, q, v, tau);
80
81			// Print both results
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 double:\n" << "\ta = " << data.ddq.transpose() << std::endl;
83	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" << "\ta = " << a_casadi_vec.transpose() << std::endl;
84		1	}
85

1

#include "pinocchio/autodiff/casadi.hpp"

2

3

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

4

5

#include "pinocchio/algorithm/aba.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 tau(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_tau = ::casadi::SX::sym("tau", model.nv);

50

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

TangentVectorAD tau_ad(model.nv);

51

tau_ad =

52

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<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);

53

54

// Build CasADi function

55

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

aba(ad_model, ad_data, q_ad, v_ad, tau_ad);

56

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

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

57

58

2/2

✓ Branch 0 taken 32 times.

✓ Branch 1 taken 1 times.

33

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

59

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

a_ad(k) = ad_data.ddq[k];

60

61

::casadi::Function eval_aba(

62

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_aba", ::casadi::SXVector{cs_q, cs_v, cs_tau}, ::casadi::SXVector{a_ad});

63

64

// Evaluate CasADi expression with real value

65

1/2

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

1

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

66

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;

67

68

1/2

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

1

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

69

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;

70

71

1/2

✓ Branch 2 taken 1 times.

✗ Branch 3 not taken.

1

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

72

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

73

74

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 a_casadi_res = eval_aba(::casadi::DMVector{q_vec, v_vec, tau_vec})[0];

75

2/4

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

✓ Branch 5 taken 1 times.

✗ Branch 6 not taken.

3

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

76

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

77

78

// Eval ABA using classic Pinocchio model

79

1/2

✓ Branch 1 taken 1 times.

✗ Branch 2 not taken.

1

pinocchio::aba(model, data, q, v, tau);

80

81

// Print both results

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

83

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

84

1

}

85