| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2017-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/multibody/model.hpp" |
||
6 |
#include "pinocchio/multibody/data.hpp" |
||
7 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
8 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
9 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
10 |
#include "pinocchio/algorithm/kinematics-derivatives.hpp" |
||
11 |
#include "pinocchio/parsers/sample-models.hpp" |
||
12 |
|||
13 |
#include <iostream> |
||
14 |
|||
15 |
#include <boost/test/unit_test.hpp> |
||
16 |
#include <boost/utility/binary.hpp> |
||
17 |
|||
18 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
19 |
|||
20 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_all) |
21 |
{ |
||
22 |
using namespace Eigen; |
||
23 |
using namespace pinocchio; |
||
24 |
|||
25 |
✓✗ | 4 |
Model model; |
26 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
27 |
|||
28 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
29 |
|||
30 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
31 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
32 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
33 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
34 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
35 |
|||
36 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v,a); |
37 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data,q,v,a); |
38 |
|||
39 |
✓✓ | 56 |
for(size_t i = 1; i < (size_t)model.njoints; ++i) |
40 |
{ |
||
41 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[i].isApprox(data_ref.oMi[i])); |
42 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.v[i].isApprox(data_ref.v[i])); |
43 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(data.ov[i].isApprox(data_ref.oMi[i].act(data_ref.v[i]))); |
44 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.a[i].isApprox(data_ref.a[i])); |
45 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(data.oa[i].isApprox(data_ref.oMi[i].act(data_ref.a[i]))); |
46 |
} |
||
47 |
|||
48 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
49 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
50 |
|||
51 |
✓✗ | 2 |
computeJointJacobiansTimeVariation(model, data_ref, q, v); |
52 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
53 |
2 |
} |
|
54 |
|||
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_velocity) |
56 |
{ |
||
57 |
using namespace Eigen; |
||
58 |
using namespace pinocchio; |
||
59 |
|||
60 |
✓✗ | 4 |
Model model; |
61 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
62 |
|||
63 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
64 |
|||
65 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
66 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
67 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
68 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
69 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
70 |
|||
71 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data,q,v,a); |
72 |
|||
73 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
74 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq(6,model.nv); partial_dq.setZero(); |
75 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_local_world_aligned(6,model.nv); partial_dq_local_world_aligned.setZero(); |
76 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_local(6,model.nv); partial_dq_local.setZero(); |
77 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv(6,model.nv); partial_dv.setZero(); |
78 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_local_world_aligned(6,model.nv); partial_dv_local_world_aligned.setZero(); |
79 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_local(6,model.nv); partial_dv_local.setZero(); |
80 |
|||
81 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data,jointId,WORLD, |
82 |
partial_dq,partial_dv); |
||
83 |
|||
84 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data,jointId,LOCAL_WORLD_ALIGNED, |
85 |
partial_dq_local_world_aligned,partial_dv_local_world_aligned); |
||
86 |
|||
87 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data,jointId,LOCAL, |
88 |
partial_dq_local,partial_dv_local); |
||
89 |
|||
90 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref(6,model.nv); J_ref.setZero(); |
91 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero(); |
92 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero(); |
93 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
94 |
✓✗ | 2 |
getJointJacobian(model,data_ref,jointId,WORLD,J_ref); |
95 |
✓✗ | 2 |
getJointJacobian(model,data_ref,jointId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned); |
96 |
✓✗ | 2 |
getJointJacobian(model,data_ref,jointId,LOCAL,J_ref_local); |
97 |
|||
98 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv.isApprox(J_ref)); |
99 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned)); |
100 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv_local.isApprox(J_ref_local)); |
101 |
|||
102 |
// Check against finite differences |
||
103 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_fd(6,model.nv); partial_dq_fd.setZero(); |
104 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_fd_local_world_aligned(6,model.nv); partial_dq_fd_local_world_aligned.setZero(); |
105 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_fd_local(6,model.nv); partial_dq_fd_local.setZero(); |
106 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_fd(6,model.nv); partial_dv_fd.setZero(); |
107 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_fd_local_world_aligned(6,model.nv); partial_dv_fd_local_world_aligned.setZero(); |
108 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_fd_local(6,model.nv); partial_dv_fd_local.setZero(); |
109 |
2 |
const double alpha = 1e-8; |
|
110 |
|||
111 |
// dvel/dv |
||
112 |
✓✗ | 4 |
Eigen::VectorXd v_plus(v); |
113 |
✓✗ | 4 |
Data data_plus(model); |
114 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v); |
115 |
✓✗ | 2 |
SE3 oMi_rot(SE3::Identity()); |
116 |
✓✗✓✗ ✓✗ |
2 |
oMi_rot.rotation() = data_ref.oMi[jointId].rotation(); |
117 |
✓✗ | 2 |
Motion v0(data_ref.oMi[jointId].act(data_ref.v[jointId])); |
118 |
✓✗ | 2 |
Motion v0_local_world_aligned(oMi_rot.act(data_ref.v[jointId])); |
119 |
✓✗ | 2 |
Motion v0_local(data_ref.v[jointId]); |
120 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
121 |
{ |
||
122 |
✓✗ | 64 |
v_plus[k] += alpha; |
123 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q,v_plus); |
124 |
|||
125 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
partial_dv_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha; |
126 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
partial_dv_fd_local_world_aligned.col(k) = (oMi_rot.act(data_plus.v[jointId]) - v0_local_world_aligned).toVector()/alpha; |
127 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
partial_dv_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha; |
128 |
✓✗ | 64 |
v_plus[k] -= alpha; |
129 |
} |
||
130 |
|||
131 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,sqrt(alpha))); |
132 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned,sqrt(alpha))); |
133 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local,sqrt(alpha))); |
134 |
|||
135 |
// dvel/dq |
||
136 |
✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv)); |
137 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v); |
138 |
✓✗ | 2 |
v0 = data_ref.oMi[jointId].act(data_ref.v[jointId]); |
139 |
|||
140 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
141 |
{ |
||
142 |
✓✗ | 64 |
v_eps[k] += alpha; |
143 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
144 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q_plus,v); |
145 |
|||
146 |
✓✗ | 64 |
SE3 oMi_plus_rot = data_plus.oMi[jointId]; |
147 |
✓✗✓✗ |
64 |
oMi_plus_rot.translation().setZero(); |
148 |
|||
149 |
✓✗ | 64 |
Motion v_plus_local_world_aligned = oMi_plus_rot.act(data_plus.v[jointId]); |
150 |
✓✗✓✗ ✓✗✓✗ |
64 |
SE3::Vector3 trans = data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation(); |
151 |
✓✗✓✗ ✓✗✓✗ |
64 |
v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans); |
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha; |
153 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
partial_dq_fd_local_world_aligned.col(k) = (v_plus_local_world_aligned - v0_local_world_aligned).toVector()/alpha; |
154 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
partial_dq_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha; |
155 |
✓✗ | 64 |
v_eps[k] -= alpha; |
156 |
} |
||
157 |
|||
158 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha))); |
159 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned,sqrt(alpha))); |
160 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha))); |
161 |
|||
162 |
2 |
} |
|
163 |
|||
164 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration) |
165 |
{ |
||
166 |
using namespace Eigen; |
||
167 |
using namespace pinocchio; |
||
168 |
|||
169 |
✓✗ | 4 |
Model model; |
170 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
171 |
|||
172 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
173 |
|||
174 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
175 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
176 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
177 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
178 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
179 |
|||
180 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data,q,v,a); |
181 |
|||
182 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
183 |
|||
184 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero(); |
185 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero(); |
186 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_local_world_aligned(6,model.nv); v_partial_dq_local_world_aligned.setZero(); |
187 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero(); |
188 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_local_world_aligned(6,model.nv); a_partial_dq_local_world_aligned.setZero(); |
189 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero(); |
190 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero(); |
191 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_local_world_aligned(6,model.nv); a_partial_dv_local_world_aligned.setZero(); |
192 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero(); |
193 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero(); |
194 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_local_world_aligned(6,model.nv); a_partial_da_local_world_aligned.setZero(); |
195 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero(); |
196 |
|||
197 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data,jointId,WORLD, |
198 |
v_partial_dq, |
||
199 |
a_partial_dq,a_partial_dv,a_partial_da); |
||
200 |
|||
201 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data,jointId,LOCAL_WORLD_ALIGNED, |
202 |
v_partial_dq_local_world_aligned, |
||
203 |
a_partial_dq_local_world_aligned, |
||
204 |
a_partial_dv_local_world_aligned, |
||
205 |
a_partial_da_local_world_aligned); |
||
206 |
|||
207 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data,jointId,LOCAL, |
208 |
v_partial_dq_local, |
||
209 |
a_partial_dq_local,a_partial_dv_local,a_partial_da_local); |
||
210 |
|||
211 |
// Check v_partial_dq against getJointVelocityDerivatives |
||
212 |
{ |
||
213 |
✓✗ | 4 |
Data data_v(model); |
214 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data_v,q,v,a); |
215 |
|||
216 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero(); |
217 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_ref_local_world_aligned(6,model.nv); v_partial_dq_ref_local_world_aligned.setZero(); |
218 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero(); |
219 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero(); |
220 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_ref_local_world_aligned(6,model.nv); v_partial_dv_ref_local_world_aligned.setZero(); |
221 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_ref_local(6,model.nv); v_partial_dv_ref_local.setZero(); |
222 |
|||
223 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data_v,jointId,WORLD, |
224 |
v_partial_dq_ref,v_partial_dv_ref); |
||
225 |
|||
226 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref)); |
227 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref)); |
228 |
|||
229 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data_v,jointId,LOCAL_WORLD_ALIGNED, |
230 |
v_partial_dq_ref_local_world_aligned,v_partial_dv_ref_local_world_aligned); |
||
231 |
|||
232 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned)); |
233 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned)); |
234 |
|||
235 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data_v,jointId,LOCAL, |
236 |
v_partial_dq_ref_local,v_partial_dv_ref_local); |
||
237 |
|||
238 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local)); |
239 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local)); |
240 |
} |
||
241 |
|||
242 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref(6,model.nv); J_ref.setZero(); |
243 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero(); |
244 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero(); |
245 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
246 |
✓✗ | 2 |
getJointJacobian(model,data_ref,jointId,WORLD,J_ref); |
247 |
✓✗ | 2 |
getJointJacobian(model,data_ref,jointId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned); |
248 |
✓✗ | 2 |
getJointJacobian(model,data_ref,jointId,LOCAL,J_ref_local); |
249 |
|||
250 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da.isApprox(J_ref)); |
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned)); |
252 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local)); |
253 |
|||
254 |
// Check against finite differences |
||
255 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_fd(6,model.nv); a_partial_da_fd.setZero(); |
256 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_fd_local_world_aligned(6,model.nv); a_partial_da_fd_local_world_aligned.setZero(); |
257 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_fd_local(6,model.nv); a_partial_da_fd_local.setZero(); |
258 |
2 |
const double alpha = 1e-8; |
|
259 |
|||
260 |
✓✗✓✗ |
4 |
Eigen::VectorXd v_plus(v), a_plus(a); |
261 |
✓✗ | 4 |
Data data_plus(model); |
262 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v,a); |
263 |
✓✗ | 2 |
SE3 oMi_rot(SE3::Identity()); |
264 |
✓✗✓✗ ✓✗ |
2 |
oMi_rot.rotation() = data_ref.oMi[jointId].rotation(); |
265 |
|||
266 |
// dacc/da |
||
267 |
✓✗ | 2 |
Motion a0(data_ref.oMi[jointId].act(data_ref.a[jointId])); |
268 |
✓✗ | 2 |
Motion a0_local_world_aligned(oMi_rot.act(data_ref.a[jointId])); |
269 |
✓✗ | 2 |
Motion a0_local(data_ref.a[jointId]); |
270 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
271 |
{ |
||
272 |
✓✗ | 64 |
a_plus[k] += alpha; |
273 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q,v,a_plus); |
274 |
|||
275 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_da_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha; |
276 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_da_fd_local_world_aligned.col(k) = (oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector()/alpha; |
277 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
a_partial_da_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha; |
278 |
✓✗ | 64 |
a_plus[k] -= alpha; |
279 |
} |
||
280 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd,sqrt(alpha))); |
281 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned,sqrt(alpha))); |
282 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha))); |
283 |
✓✗✓✗ |
2 |
motionSet::se3Action(data_ref.oMi[jointId].inverse(),a_partial_da,a_partial_da_local); |
284 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha))); |
285 |
|||
286 |
// dacc/dv |
||
287 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_fd(6,model.nv); a_partial_dv_fd.setZero(); |
288 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_fd_local_world_aligned(6,model.nv); a_partial_dv_fd_local_world_aligned.setZero(); |
289 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_fd_local(6,model.nv); a_partial_dv_fd_local.setZero(); |
290 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
291 |
{ |
||
292 |
✓✗ | 64 |
v_plus[k] += alpha; |
293 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q,v_plus,a); |
294 |
|||
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_dv_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha; a_partial_dv_fd_local_world_aligned.col(k) = (oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector()/alpha; |
296 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
a_partial_dv_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha; |
297 |
✓✗ | 64 |
v_plus[k] -= alpha; |
298 |
} |
||
299 |
|||
300 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd,sqrt(alpha))); |
301 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned,sqrt(alpha))); |
302 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha))); |
303 |
✓✗✓✗ |
2 |
motionSet::se3Action(data_ref.oMi[jointId].inverse(),a_partial_dv,a_partial_dv_local); |
304 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha))); |
305 |
|||
306 |
// dacc/dq |
||
307 |
✓✗ | 2 |
a_partial_dq.setZero(); |
308 |
✓✗ | 2 |
a_partial_dv.setZero(); |
309 |
✓✗ | 2 |
a_partial_da.setZero(); |
310 |
|||
311 |
✓✗ | 2 |
a_partial_dq_local_world_aligned.setZero(); |
312 |
✓✗ | 2 |
a_partial_dv_local_world_aligned.setZero(); |
313 |
✓✗ | 2 |
a_partial_da_local_world_aligned.setZero(); |
314 |
|||
315 |
✓✗ | 2 |
a_partial_dq_local.setZero(); |
316 |
✓✗ | 2 |
a_partial_dv_local.setZero(); |
317 |
✓✗ | 2 |
a_partial_da_local.setZero(); |
318 |
|||
319 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero(); |
320 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_fd_local_world_aligned(6,model.nv); a_partial_dq_fd_local_world_aligned.setZero(); |
321 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_fd_local(6,model.nv); a_partial_dq_fd_local.setZero(); |
322 |
|||
323 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data,q,v,a); |
324 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data,jointId,WORLD, |
325 |
v_partial_dq, |
||
326 |
a_partial_dq,a_partial_dv,a_partial_da); |
||
327 |
|||
328 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data,jointId,LOCAL_WORLD_ALIGNED, |
329 |
v_partial_dq_local_world_aligned, |
||
330 |
a_partial_dq_local_world_aligned, |
||
331 |
a_partial_dv_local_world_aligned, |
||
332 |
a_partial_da_local_world_aligned); |
||
333 |
|||
334 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data,jointId,LOCAL, |
335 |
v_partial_dq_local, |
||
336 |
a_partial_dq_local,a_partial_dv_local,a_partial_da_local); |
||
337 |
|||
338 |
✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv)); |
339 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v,a); |
340 |
✓✗ | 2 |
a0 = data_ref.oMi[jointId].act(data_ref.a[jointId]); |
341 |
✓✗✓✗ ✓✗ |
2 |
oMi_rot.rotation() = data_ref.oMi[jointId].rotation(); |
342 |
✓✗ | 2 |
a0_local_world_aligned = oMi_rot.act(data_ref.a[jointId]); |
343 |
✓✗ | 2 |
a0_local = data_ref.a[jointId]; |
344 |
|||
345 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
346 |
{ |
||
347 |
✓✗ | 64 |
v_eps[k] += alpha; |
348 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
349 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q_plus,v,a); |
350 |
|||
351 |
✓✗ | 64 |
SE3 oMi_plus_rot = data_plus.oMi[jointId]; |
352 |
✓✗✓✗ |
64 |
oMi_plus_rot.translation().setZero(); |
353 |
|||
354 |
✓✗ | 64 |
Motion a_plus_local_world_aligned = oMi_plus_rot.act(data_plus.a[jointId]); |
355 |
✓✗✓✗ ✓✗✓✗ |
64 |
const SE3::Vector3 trans = data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation(); |
356 |
✓✗✓✗ ✓✗✓✗ |
64 |
a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans); |
357 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha; |
358 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
a_partial_dq_fd_local_world_aligned.col(k) = (a_plus_local_world_aligned - a0_local_world_aligned).toVector()/alpha; |
359 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
a_partial_dq_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha; |
360 |
✓✗ | 64 |
v_eps[k] -= alpha; |
361 |
} |
||
362 |
|||
363 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd,sqrt(alpha))); |
364 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned,sqrt(alpha))); |
365 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha))); |
366 |
|||
367 |
2 |
} |
|
368 |
|||
369 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_against_classic_formula) |
370 |
{ |
||
371 |
using namespace Eigen; |
||
372 |
using namespace pinocchio; |
||
373 |
|||
374 |
✓✗ | 4 |
Model model; |
375 |
✓✗ | 2 |
buildModels::humanoidRandom(model,true); |
376 |
|||
377 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
378 |
|||
379 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
380 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
381 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
382 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
383 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
384 |
|||
385 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
const Model::JointIndex jointId = model.existJointName("rarm4_joint")?model.getJointId("rarm4_joint"):(Model::Index)(model.njoints-1); |
386 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero(); |
387 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero(); |
388 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero(); |
389 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero(); |
390 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero(); |
391 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero(); |
392 |
|||
393 |
// LOCAL: da/dv == dJ/dt + dv/dq |
||
394 |
// { |
||
395 |
// Data::Matrix6x rhs(6,model.nv); rhs.setZero(); |
||
396 |
// |
||
397 |
// v_partial_dq.setZero(); |
||
398 |
// a_partial_dq.setZero(); |
||
399 |
// a_partial_dv.setZero(); |
||
400 |
// a_partial_da.setZero(); |
||
401 |
// |
||
402 |
// computeForwardKinematicsDerivatives(model,data_ref,q,v,a); |
||
403 |
// computeForwardKinematicsDerivatives(model,data,q,v,a); |
||
404 |
// |
||
405 |
// getJointAccelerationDerivatives<LOCAL>(model,data,jointId, |
||
406 |
// v_partial_dq, |
||
407 |
// a_partial_dq,a_partial_dv,a_partial_da); |
||
408 |
// |
||
409 |
// getJointJacobianTimeVariation<LOCAL>(model,data_ref,jointId,rhs); |
||
410 |
// |
||
411 |
// v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero(); |
||
412 |
// getJointVelocityDerivatives<LOCAL>(model,data_ref,jointId, |
||
413 |
// v_partial_dq_ref,v_partial_dv_ref); |
||
414 |
// rhs += v_partial_dq_ref; |
||
415 |
// BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12)); |
||
416 |
// |
||
417 |
// std::cout << "a_partial_dv\n" << a_partial_dv << std::endl; |
||
418 |
// std::cout << "rhs\n" << rhs << std::endl; |
||
419 |
// } |
||
420 |
|||
421 |
// WORLD: da/dv == dJ/dt + dv/dq |
||
422 |
{ |
||
423 |
✓✗✓✗ |
4 |
Data::Matrix6x rhs(6,model.nv); rhs.setZero(); |
424 |
|||
425 |
✓✗ | 2 |
v_partial_dq.setZero(); |
426 |
✓✗ | 2 |
a_partial_dq.setZero(); |
427 |
✓✗ | 2 |
a_partial_dv.setZero(); |
428 |
✓✗ | 2 |
a_partial_da.setZero(); |
429 |
|||
430 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data_ref,q,v,a); |
431 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data,q,v,a); |
432 |
|||
433 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data,jointId,WORLD, |
434 |
v_partial_dq, |
||
435 |
a_partial_dq,a_partial_dv,a_partial_da); |
||
436 |
|||
437 |
✓✗ | 2 |
getJointJacobianTimeVariation(model,data_ref,jointId,WORLD,rhs); |
438 |
|||
439 |
✓✗✓✗ |
2 |
v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero(); |
440 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data_ref,jointId,WORLD, |
441 |
v_partial_dq_ref,v_partial_dv_ref); |
||
442 |
✓✗ | 2 |
rhs += v_partial_dq_ref; |
443 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12)); |
444 |
|||
445 |
// std::cout << "a_partial_dv\n" << a_partial_dv << std::endl; |
||
446 |
// std::cout << "rhs\n" << rhs << std::endl; |
||
447 |
} |
||
448 |
|||
449 |
// // WORLD: da/dq == d/dt(dv/dq) |
||
450 |
// { |
||
451 |
// const double alpha = 1e-8; |
||
452 |
// Eigen::VectorXd q_plus(model.nq), v_plus(model.nv); |
||
453 |
// |
||
454 |
// Data data_plus(model); |
||
455 |
// v_plus = v + alpha * a; |
||
456 |
// q_plus = integrate(model,q,alpha*v); |
||
457 |
// |
||
458 |
// computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a); |
||
459 |
// computeForwardKinematicsDerivatives(model,data_ref,q,v,a); |
||
460 |
// |
||
461 |
// Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero(); |
||
462 |
// Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero(); |
||
463 |
// |
||
464 |
// v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero(); |
||
465 |
// |
||
466 |
// v_partial_dq.setZero(); |
||
467 |
// a_partial_dq.setZero(); |
||
468 |
// a_partial_dv.setZero(); |
||
469 |
// a_partial_da.setZero(); |
||
470 |
// |
||
471 |
// getJointVelocityDerivatives<WORLD>(model,data_ref,jointId, |
||
472 |
// v_partial_dq_ref,v_partial_dv_ref); |
||
473 |
// getJointVelocityDerivatives<WORLD>(model,data_plus,jointId, |
||
474 |
// v_partial_dq_plus,v_partial_dv_plus); |
||
475 |
// |
||
476 |
// getJointAccelerationDerivatives<WORLD>(model,data_ref,jointId, |
||
477 |
// v_partial_dq, |
||
478 |
// a_partial_dq,a_partial_dv,a_partial_da); |
||
479 |
// |
||
480 |
// Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero(); |
||
481 |
// { |
||
482 |
// Data data_fd(model); |
||
483 |
// VectorXd q_fd(model.nq), v_eps(model.nv); v_eps.setZero(); |
||
484 |
// for(int k = 0; k < model.nv; ++k) |
||
485 |
// { |
||
486 |
// v_eps[k] += alpha; |
||
487 |
// q_fd = integrate(model,q,v_eps); |
||
488 |
// forwardKinematics(model,data_fd,q_fd,v,a); |
||
489 |
// a_partial_dq_fd.col(k) = (data_fd.oMi[jointId].act(data_fd.a[jointId]) - data_ref.oa[jointId]).toVector()/alpha; |
||
490 |
// v_eps[k] = 0.; |
||
491 |
// } |
||
492 |
// } |
||
493 |
// |
||
494 |
// Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha; |
||
495 |
// BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha))); |
||
496 |
// |
||
497 |
// std::cout << "a_partial_dq\n" << a_partial_dq << std::endl; |
||
498 |
// std::cout << "a_partial_dq_fd\n" << a_partial_dq_fd << std::endl; |
||
499 |
// std::cout << "rhs\n" << rhs << std::endl; |
||
500 |
// } |
||
501 |
|||
502 |
// LOCAL: da/dq == d/dt(dv/dq) |
||
503 |
{ |
||
504 |
2 |
const double alpha = 1e-8; |
|
505 |
✓✗✓✗ |
4 |
Eigen::VectorXd q_plus(model.nq), v_plus(model.nv); |
506 |
|||
507 |
✓✗ | 4 |
Data data_plus(model); |
508 |
✓✗✓✗ ✓✗ |
2 |
v_plus = v + alpha * a; |
509 |
✓✗✓✗ |
2 |
q_plus = integrate(model,q,alpha*v); |
510 |
|||
511 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a); |
512 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data_ref,q,v,a); |
513 |
|||
514 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero(); |
515 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero(); |
516 |
|||
517 |
✓✗✓✗ |
2 |
v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero(); |
518 |
|||
519 |
✓✗ | 2 |
v_partial_dq.setZero(); |
520 |
✓✗ | 2 |
a_partial_dq.setZero(); |
521 |
✓✗ | 2 |
a_partial_dv.setZero(); |
522 |
✓✗ | 2 |
a_partial_da.setZero(); |
523 |
|||
524 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data_ref,jointId,LOCAL, |
525 |
v_partial_dq_ref,v_partial_dv_ref); |
||
526 |
✓✗ | 2 |
getJointVelocityDerivatives(model,data_plus,jointId,LOCAL, |
527 |
v_partial_dq_plus,v_partial_dv_plus); |
||
528 |
|||
529 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data_ref,jointId,LOCAL, |
530 |
v_partial_dq, |
||
531 |
a_partial_dq,a_partial_dv,a_partial_da); |
||
532 |
|||
533 |
✓✗✓✗ ✓✗ |
4 |
Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha; |
534 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha))); |
535 |
|||
536 |
// std::cout << "a_partial_dq\n" << a_partial_dq << std::endl; |
||
537 |
// std::cout << "rhs\n" << rhs << std::endl; |
||
538 |
} |
||
539 |
|||
540 |
|||
541 |
2 |
} |
|
542 |
|||
543 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematics_hessians) |
544 |
{ |
||
545 |
using namespace Eigen; |
||
546 |
using namespace pinocchio; |
||
547 |
|||
548 |
✓✗ | 4 |
Model model; |
549 |
✓✗ | 2 |
buildModels::humanoidRandom(model,true); |
550 |
|||
551 |
✓✗✓✗ ✓✗ |
4 |
Data data(model), data_ref(model), data_plus(model); |
552 |
|||
553 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
554 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
555 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
556 |
|||
557 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
const Model::JointIndex joint_id = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
558 |
|||
559 |
✓✗ | 2 |
computeJointJacobians(model,data,q); |
560 |
✓✗ | 2 |
computeJointKinematicHessians(model,data); |
561 |
|||
562 |
✓✗ | 4 |
Data data2(model); |
563 |
✓✗ | 2 |
computeJointKinematicHessians(model,data2,q); |
564 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data2.J.isApprox(data.J)); |
565 |
|||
566 |
2 |
const Eigen::DenseIndex matrix_offset = 6 * model.nv; |
|
567 |
|||
568 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
569 |
{ |
||
570 |
✓✗✓✗ ✓✗ |
64 |
Eigen::Map<Data::Matrix6x> dJ(data.kinematic_hessians.data() + k*matrix_offset,6,model.nv); |
571 |
✓✗✓✗ ✓✗ |
64 |
Eigen::Map<Data::Matrix6x> dJ2(data2.kinematic_hessians.data() + k*matrix_offset,6,model.nv); |
572 |
|||
573 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(dJ2.isApprox(dJ)); |
574 |
} |
||
575 |
|||
576 |
✓✓ | 66 |
for(int i = 0; i < model.nv; ++i) |
577 |
{ |
||
578 |
✓✓ | 1120 |
for(int j = i; j < model.nv; ++j) |
579 |
{ |
||
580 |
1056 |
bool j_is_children_of_i = false; |
|
581 |
✓✓ | 8896 |
for(int parent = j; parent >= 0; parent = data.parents_fromRow[(size_t)parent]) |
582 |
{ |
||
583 |
✓✓ | 8416 |
if(parent == i) |
584 |
{ |
||
585 |
576 |
j_is_children_of_i = true; |
|
586 |
576 |
break; |
|
587 |
} |
||
588 |
} |
||
589 |
|||
590 |
✓✓ | 1056 |
if(j_is_children_of_i) |
591 |
{ |
||
592 |
✓✓ | 576 |
if(i==j) |
593 |
{ |
||
594 |
✓✗ | 64 |
Eigen::Map<Data::Motion::Vector6> SixSi( data.kinematic_hessians.data() |
595 |
64 |
+ i * matrix_offset |
|
596 |
✓✗✓✗ |
128 |
+ i * 6); |
597 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
64 |
BOOST_CHECK(SixSi.isZero()); |
598 |
} |
||
599 |
else |
||
600 |
{ |
||
601 |
✓✗ | 512 |
Eigen::Map<Data::Motion::Vector6> SixSj( data.kinematic_hessians.data() |
602 |
512 |
+ i * matrix_offset |
|
603 |
✓✗✓✗ |
1024 |
+ j * 6); |
604 |
|||
605 |
✓✗ | 512 |
Eigen::Map<Data::Motion::Vector6> SjxSi( data.kinematic_hessians.data() |
606 |
512 |
+ j * matrix_offset |
|
607 |
✓✗✓✗ |
1024 |
+ i * 6); |
608 |
|||
609 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
512 |
BOOST_CHECK(SixSj.isApprox(-SjxSi)); |
610 |
} |
||
611 |
} |
||
612 |
else |
||
613 |
{ |
||
614 |
✓✗ | 480 |
Eigen::Map<Data::Motion::Vector6> SixSj( data.kinematic_hessians.data() |
615 |
480 |
+ i * matrix_offset |
|
616 |
✓✗✓✗ |
960 |
+ j * 6); |
617 |
|||
618 |
✓✗ | 480 |
Eigen::Map<Data::Motion::Vector6> SjxSi( data.kinematic_hessians.data() |
619 |
480 |
+ j * matrix_offset |
|
620 |
✓✗✓✗ |
960 |
+ i * 6); |
621 |
|||
622 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
480 |
BOOST_CHECK(SixSj.isZero()); |
623 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
480 |
BOOST_CHECK(SjxSi.isZero()); |
624 |
} |
||
625 |
} |
||
626 |
} |
||
627 |
|||
628 |
2 |
const double eps = 1e-8; |
|
629 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref(6,model.nv), J_plus(6,model.nv); |
630 |
✓✗✓✗ |
2 |
J_ref.setZero(); J_plus.setZero(); |
631 |
|||
632 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
633 |
✓✗✓✗ |
4 |
VectorXd v_plus(VectorXd::Zero(model.nv)); |
634 |
|||
635 |
2 |
const Eigen::DenseIndex outer_offset = model.nv * 6; |
|
636 |
|||
637 |
// WORLD |
||
638 |
✓✗ | 2 |
getJointJacobian(model,data_ref,joint_id,WORLD,J_ref); |
639 |
✓✗ | 4 |
Data::Tensor3x kinematic_hessian_world = getJointKinematicHessian(model,data,joint_id,WORLD); |
640 |
✓✓ | 66 |
for(Eigen::DenseIndex k = 0; k < model.nv; ++k) |
641 |
{ |
||
642 |
✓✗ | 64 |
v_plus[k] = eps; |
643 |
✓✗ | 128 |
const VectorXd q_plus = integrate(model,q,v_plus); |
644 |
✓✗ | 64 |
computeJointJacobians(model,data_plus,q_plus); |
645 |
✓✗ | 64 |
J_plus.setZero(); |
646 |
✓✗ | 64 |
getJointJacobian(model,data_plus,joint_id,WORLD,J_plus); |
647 |
|||
648 |
✓✗✓✗ ✓✗ |
64 |
Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps; |
649 |
✓✗✓✗ ✓✗ |
64 |
Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_world.data() + k * outer_offset,6,model.nv); |
650 |
|||
651 |
// std::cout << "k: " << k << std::endl; |
||
652 |
// std::cout << "dJ_dq:\n" << dJ_dq << std::endl; |
||
653 |
// std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl; |
||
654 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
64 |
BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps))); |
655 |
✓✗ | 64 |
v_plus[k] = 0.; |
656 |
} |
||
657 |
|||
658 |
// LOCAL_WORLD_ALIGNED |
||
659 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
660 |
✓✗ | 2 |
getJointJacobian(model,data_ref,joint_id,LOCAL_WORLD_ALIGNED,J_ref); |
661 |
✓✗ | 4 |
Data::Tensor3x kinematic_hessian_local_world_aligned = getJointKinematicHessian(model,data,joint_id,LOCAL_WORLD_ALIGNED); |
662 |
✓✗ | 4 |
Data::Matrix3x dt_last_fd(3,model.nv); |
663 |
✓✓ | 66 |
for(Eigen::DenseIndex k = 0; k < model.nv; ++k) |
664 |
{ |
||
665 |
✓✗ | 64 |
v_plus[k] = eps; |
666 |
✓✗ | 128 |
const VectorXd q_plus = integrate(model,q,v_plus); |
667 |
✓✗ | 64 |
computeJointJacobians(model,data_plus,q_plus); |
668 |
✓✗ | 64 |
J_plus.setZero(); |
669 |
✓✗ | 64 |
getJointJacobian(model,data_plus,joint_id,LOCAL_WORLD_ALIGNED,J_plus); |
670 |
|||
671 |
✓✗✓✗ |
64 |
SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id]; |
672 |
✓✗✓✗ |
64 |
tMt_plus.rotation().setIdentity(); |
673 |
|||
674 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
dt_last_fd.col(k) = (data_plus.oMi[joint_id].translation() - data_ref.oMi[joint_id].translation())/eps; |
675 |
|||
676 |
✓✗✓✗ ✓✗ |
64 |
Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps; |
677 |
✓✗✓✗ ✓✗ |
64 |
Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_local_world_aligned.data() + k * outer_offset,6,model.nv); |
678 |
|||
679 |
// std::cout << "k: " << k << std::endl; |
||
680 |
// std::cout << "dJ_dq:\n" << dJ_dq << std::endl; |
||
681 |
// std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl; |
||
682 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
64 |
BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps))); |
683 |
✓✗ | 64 |
v_plus[k] = 0.; |
684 |
} |
||
685 |
|||
686 |
✓✗✓✗ |
4 |
Data::Matrix6x J_world(6,model.nv); J_world.setZero(); |
687 |
✓✗ | 2 |
getJointJacobian(model,data_ref,joint_id,LOCAL_WORLD_ALIGNED,J_world); |
688 |
|||
689 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(dt_last_fd.isApprox(J_world.topRows<3>(),sqrt(eps))); |
690 |
|||
691 |
// LOCAL |
||
692 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
693 |
✓✗ | 2 |
getJointJacobian(model,data_ref,joint_id,LOCAL,J_ref); |
694 |
✓✗ | 4 |
Data::Tensor3x kinematic_hessian_local = getJointKinematicHessian(model,data,joint_id,LOCAL); |
695 |
✓✓ | 66 |
for(Eigen::DenseIndex k = 0; k < model.nv; ++k) |
696 |
{ |
||
697 |
✓✗ | 64 |
v_plus[k] = eps; |
698 |
✓✗ | 128 |
const VectorXd q_plus = integrate(model,q,v_plus); |
699 |
✓✗ | 64 |
computeJointJacobians(model,data_plus,q_plus); |
700 |
✓✗ | 64 |
J_plus.setZero(); |
701 |
✓✗ | 64 |
getJointJacobian(model,data_plus,joint_id,LOCAL,J_plus); |
702 |
|||
703 |
// const SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id]; |
||
704 |
|||
705 |
✓✗✓✗ ✓✗ |
64 |
Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps; |
706 |
✓✗✓✗ ✓✗ |
64 |
Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_local.data() + k * outer_offset,6,model.nv); |
707 |
|||
708 |
// std::cout << "k: " << k << std::endl; |
||
709 |
// std::cout << "dJ_dq:\n" << dJ_dq << std::endl; |
||
710 |
// std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl; |
||
711 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
64 |
BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps))); |
712 |
✓✗ | 64 |
v_plus[k] = 0.; |
713 |
} |
||
714 |
2 |
} |
|
715 |
|||
716 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |