Directory: | ./ |
---|---|
File: | unittest/contact-inverse-dynamics.cpp |
Date: | 2024-08-27 18:20:05 |
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 |