GCC Code Coverage Report

Directory: ./
File: unittest/contact-inverse-dynamics.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 37 66 56.1%
Branches: 77 242 31.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2023 INRIA
3 //
4
5 #include "pinocchio/algorithm/aba.hpp"
6 #include "pinocchio/algorithm/rnea.hpp"
7 #include "pinocchio/algorithm/frames.hpp"
8 #include "pinocchio/algorithm/jacobian.hpp"
9 #include "pinocchio/algorithm/centroidal.hpp"
10 #include "pinocchio/algorithm/kinematics.hpp"
11 #include "pinocchio/algorithm/contact-info.hpp"
12 #include "pinocchio/algorithm/compute-all-terms.hpp"
13 #include "pinocchio/algorithm/constrained-dynamics.hpp"
14 #include "pinocchio/algorithm/contact-dynamics.hpp"
15 #include "pinocchio/algorithm/contact-inverse-dynamics.hpp"
16 #include "pinocchio/algorithm/joint-configuration.hpp"
17 #include "pinocchio/multibody/sample-models.hpp"
18 #include "pinocchio/utils/timer.hpp"
19 #include "pinocchio/spatial/classic-acceleration.hpp"
20
21 #include <iostream>
22
23 #include <boost/test/unit_test.hpp>
24 #include <boost/utility/binary.hpp>
25 #include <optional>
26
27 #define KP 10
28 #define KD 10
29
30 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
31
32 /// \brief Computes motions in the world frame
33 ✗ pinocchio::Motion computeAcceleration(
34 const pinocchio::Model & model,
35 pinocchio::Data & data,
36 const pinocchio::JointIndex & joint_id,
37 pinocchio::ReferenceFrame reference_frame,
38 const pinocchio::ContactType type,
39 const pinocchio::SE3 & placement = pinocchio::SE3::Identity())
40 {
41 PINOCCHIO_UNUSED_VARIABLE(model);
42 using namespace pinocchio;
43 ✗ Motion res(Motion::Zero());
44
45 ✗ const Data::SE3 & oMi = data.oMi[joint_id];
46 ✗ const Data::SE3 & iMc = placement;
47 ✗ const Data::SE3 oMc = oMi * iMc;
48
49 ✗ const Motion ov = oMi.act(data.v[joint_id]);
50 ✗ const Motion oa = oMi.act(data.a[joint_id]);
51
52 ✗ switch (reference_frame)
53 {
54 ✗ case WORLD:
55 ✗ if (type == CONTACT_3D)
56 ✗ classicAcceleration(ov, oa, res.linear());
57 else
58 ✗ res.linear() = oa.linear();
59 ✗ res.angular() = oa.angular();
60 ✗ break;
61 ✗ case LOCAL_WORLD_ALIGNED:
62 ✗ if (type == CONTACT_3D)
63 ✗ res.linear() = oMc.rotation() * classicAcceleration(data.v[joint_id], data.a[joint_id], iMc);
64 else
65 ✗ res.linear() = oMc.rotation() * (iMc.actInv(data.a[joint_id])).linear();
66 ✗ res.angular() = oMi.rotation() * data.a[joint_id].angular();
67 ✗ break;
68 ✗ case LOCAL:
69 ✗ if (type == CONTACT_3D)
70 ✗ classicAcceleration(data.v[joint_id], data.a[joint_id], iMc, res.linear());
71 else
72 ✗ res.linear() = (iMc.actInv(data.a[joint_id])).linear();
73 ✗ res.angular() = iMc.rotation().transpose() * data.a[joint_id].angular();
74 ✗ break;
75 ✗ default:
76 ✗ break;
77 }
78
79 ✗ return res;
80 }
81
82
33/66
✓ 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 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_contact_inverse_dynamics_3D)
83 {
84 using namespace Eigen;
85 using namespace pinocchio;
86
87
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true);
89
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(model), data_ref(model);
90
91
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 model.lowerPositionLimit.head<3>().fill(-1.);
92
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 model.upperPositionLimit.head<3>().fill(1.);
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
94
95
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd v = VectorXd::Random(model.nv);
96
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd tau = VectorXd::Random(model.nv);
97
98
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const std::string RF = "rleg6_joint";
99 // const Model::JointIndex RF_id = model.getJointId(RF);
100
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const std::string LF = "lleg6_joint";
101 // const Model::JointIndex LF_id = model.getJointId(LF);
102
103 // Contact models and data
104 typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)
105 RigidConstraintModelVector;
106 typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) RigidConstraintDataVector;
107 typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(CoulombFrictionCone) CoulombFrictionConeVector;
108
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RigidConstraintModelVector contact_models;
110
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RigidConstraintDataVector contact_datas;
111
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 CoulombFrictionConeVector cones;
112
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 RigidConstraintModel ci_RF(CONTACT_3D, model, model.getJointId(RF), LOCAL);
113
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contact_models.push_back(ci_RF);
114
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 contact_datas.push_back(RigidConstraintData(ci_RF));
115
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 cones.push_back(CoulombFrictionCone(0.4));
116
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 RigidConstraintModel ci_LF(CONTACT_3D, model, model.getJointId(LF), LOCAL);
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contact_models.push_back(ci_LF);
118
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 contact_datas.push_back(RigidConstraintData(ci_LF));
119
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 cones.push_back(CoulombFrictionCone(0.4));
120
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 RigidConstraintDataVector contact_datas_ref(contact_datas);
122
123 2 Eigen::DenseIndex constraint_dim = 0;
124
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
 6 for (size_t k = 0; k < contact_models.size(); ++k)
125 4 constraint_dim += contact_models[k].size();
126
127
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::MatrixXd J_ref(constraint_dim, model.nv);
128
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref.setZero();
129
130 2 double dt = 1e-3;
131
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd R = Eigen::VectorXd::Zero(constraint_dim);
132
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd constraint_correction = Eigen::VectorXd::Zero(constraint_dim);
133 2 boost::optional<Eigen::VectorXd> lambda_guess = boost::optional<Eigen::VectorXd>(boost::none);
134
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
135
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ProximalSettings prox_settings(1e-12, 1e-6, 1);
136
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 initConstraintDynamics(model, data_ref, contact_models);
137
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contactInverseDynamics(
138 model, data_ref, q, v, a, dt, contact_models, contact_datas, cones, R, constraint_correction,
139 prox_settings, lambda_guess);
140 // constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas_ref,
141 // prox_settings_cd); forwardKinematics(model, data_ref, q, v, v*0);
142
143 // Data::Matrix6x Jtmp = Data::Matrix6x::Zero(6,model.nv);
144 // getJointJacobian(model,data_ref,ci_RF.joint1_id,ci_RF.reference_frame,Jtmp);
145 // J_ref.middleRows<3>(0) = Jtmp.middleRows<3>(Motion::LINEAR);
146 // Jtmp.setZero(); getJointJacobian(model,data_ref,ci_LF.joint1_id,ci_LF.reference_frame,Jtmp);
147 // J_ref.middleRows<3>(3) = Jtmp.middleRows<3>(Motion::LINEAR);
148
149 // Eigen::VectorXd gamma(constraint_dim);
150
151 // gamma.segment<3>(0) =
152 // computeAcceleration(model,data_ref,ci_RF.joint1_id,ci_RF.reference_frame,ci_RF.type).linear();
153 // gamma.segment<3>(3) =
154 // computeAcceleration(model,data_ref,ci_LF.joint1_id,ci_LF.reference_frame,ci_LF.type).linear();
155
156 // BOOST_CHECK((J_ref*data_ref.ddq + gamma).isZero());
157
158 // Data data_constrained_dyn(model);
159
160 // PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
161 // PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
162 // forwardDynamics(model,data_constrained_dyn,q,v,tau,J_ref,gamma,0.);
163 // PINOCCHIO_COMPILER_DIAGNOSTIC_POP
164
165 // BOOST_CHECK((J_ref*data_constrained_dyn.ddq + gamma).isZero());
166
167 // ProximalSettings prox_settings;
168 // prox_settings.max_iter = 10;
169 // prox_settings.mu = 1e8;
170 // contactABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
171
172 // BOOST_CHECK((J_ref*data.ddq + gamma).isZero());
173
174 // // Call the algorithm a second time
175 // Data data2(model);
176 // ProximalSettings prox_settings2;
177 // contactABA(model, data2, q, v, tau, contact_models, contact_datas, prox_settings2);
178
179 // BOOST_CHECK(prox_settings2.iter == 0);
180 2 }
181
182 BOOST_AUTO_TEST_SUITE_END()
183