| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2020 INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
6 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
7 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
8 |
#include "pinocchio/algorithm/kinematics-derivatives.hpp" |
||
9 |
#include "pinocchio/algorithm/frames.hpp" |
||
10 |
#include "pinocchio/algorithm/frames-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_frames_derivatives_velocity) |
21 |
{ |
||
22 |
using namespace Eigen; |
||
23 |
using namespace pinocchio; |
||
24 |
|||
25 |
✓✗ | 4 |
Model model; |
26 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
27 |
|||
28 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
29 |
✓✗✓✗ ✓✗✓✗ |
6 |
Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME); |
30 |
✓✗ | 2 |
FrameIndex frameId = model.addFrame(frame); |
31 |
|||
32 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(model.getFrameId("rand") == frameId); |
33 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(model.frames[frameId].parent == jointId); |
34 |
|||
35 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
36 |
|||
37 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
38 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
39 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
40 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
41 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
42 |
|||
43 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data,q,v,a); |
44 |
|||
45 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq(6,model.nv); partial_dq.setZero(); |
46 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_local_world_aligned(6,model.nv); partial_dq_local_world_aligned.setZero(); |
47 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_local(6,model.nv); partial_dq_local.setZero(); |
48 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv(6,model.nv); partial_dv.setZero(); |
49 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_local_world_aligned(6,model.nv); partial_dv_local_world_aligned.setZero(); |
50 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_local(6,model.nv); partial_dv_local.setZero(); |
51 |
|||
52 |
✓✗ | 2 |
getFrameVelocityDerivatives(model,data,frameId,WORLD, |
53 |
partial_dq,partial_dv); |
||
54 |
|||
55 |
✓✗ | 2 |
getFrameVelocityDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED, |
56 |
partial_dq_local_world_aligned,partial_dv_local_world_aligned); |
||
57 |
|||
58 |
✓✗ | 2 |
getFrameVelocityDerivatives(model,data,frameId,LOCAL, |
59 |
partial_dq_local,partial_dv_local); |
||
60 |
|||
61 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref(6,model.nv); J_ref.setZero(); |
62 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero(); |
63 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero(); |
64 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
65 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,frameId,WORLD,J_ref); |
66 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned); |
67 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local); |
68 |
|||
69 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_ref.oMf[frameId].isApprox(data.oMf[frameId])); |
70 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv.isApprox(J_ref)); |
71 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned)); |
72 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv_local.isApprox(J_ref_local)); |
73 |
|||
74 |
// Check against finite differences |
||
75 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_fd(6,model.nv); partial_dq_fd.setZero(); |
76 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_fd_local_world_aligned(6,model.nv); partial_dq_fd_local_world_aligned.setZero(); |
77 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dq_fd_local(6,model.nv); partial_dq_fd_local.setZero(); |
78 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_fd(6,model.nv); partial_dv_fd.setZero(); |
79 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_fd_local_world_aligned(6,model.nv); partial_dv_fd_local_world_aligned.setZero(); |
80 |
✓✗✓✗ |
4 |
Data::Matrix6x partial_dv_fd_local(6,model.nv); partial_dv_fd_local.setZero(); |
81 |
2 |
const double alpha = 1e-8; |
|
82 |
|||
83 |
// dvel/dv |
||
84 |
✓✗ | 4 |
Eigen::VectorXd v_plus(v); |
85 |
✓✗ | 4 |
Data data_plus(model); |
86 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v); |
87 |
✓✗ | 2 |
Motion v0 = getFrameVelocity(model,data,frameId,WORLD); |
88 |
✓✗ | 2 |
Motion v0_local_world_aligned = getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED); |
89 |
✓✗ | 2 |
Motion v0_local = getFrameVelocity(model,data,frameId,LOCAL); |
90 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
91 |
{ |
||
92 |
✓✗ | 64 |
v_plus[k] += alpha; |
93 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q,v_plus); |
94 |
|||
95 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
partial_dv_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha; |
96 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
partial_dv_fd_local_world_aligned.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - v0_local_world_aligned).toVector()/alpha; |
97 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
partial_dv_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha; |
98 |
✓✗ | 64 |
v_plus[k] -= alpha; |
99 |
} |
||
100 |
|||
101 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,sqrt(alpha))); |
102 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned,sqrt(alpha))); |
103 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local,sqrt(alpha))); |
104 |
|||
105 |
// dvel/dq |
||
106 |
✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv)); |
107 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v); |
108 |
✓✗ | 2 |
updateFramePlacements(model,data_ref); |
109 |
|||
110 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
111 |
{ |
||
112 |
✓✗ | 64 |
v_eps[k] += alpha; |
113 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
114 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q_plus,v); |
115 |
✓✗ | 64 |
updateFramePlacements(model,data_plus); |
116 |
|||
117 |
✓✗ | 64 |
Motion v_plus_local_world_aligned = getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED); |
118 |
✓✗✓✗ ✓✗✓✗ |
64 |
SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation(); |
119 |
✓✗✓✗ ✓✗✓✗ |
64 |
v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans); |
120 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
partial_dq_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha; |
121 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
partial_dq_fd_local_world_aligned.col(k) = (v_plus_local_world_aligned - v0_local_world_aligned).toVector()/alpha; |
122 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
partial_dq_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha; |
123 |
✓✗ | 64 |
v_eps[k] -= alpha; |
124 |
} |
||
125 |
|||
126 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha))); |
127 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned,sqrt(alpha))); |
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha))); |
129 |
|||
130 |
2 |
} |
|
131 |
|||
132 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration) |
133 |
{ |
||
134 |
using namespace Eigen; |
||
135 |
using namespace pinocchio; |
||
136 |
|||
137 |
✓✗ | 4 |
Model model; |
138 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
139 |
|||
140 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
141 |
✓✗✓✗ ✓✗✓✗ |
6 |
Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME); |
142 |
✓✗ | 2 |
FrameIndex frameId = model.addFrame(frame); |
143 |
|||
144 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(model.getFrameId("rand") == frameId); |
145 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(model.frames[frameId].parent == jointId); |
146 |
|||
147 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
148 |
|||
149 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
150 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
151 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
152 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
153 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
154 |
|||
155 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data,q,v,a); |
156 |
|||
157 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero(); |
158 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero(); |
159 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_local_world_aligned(6,model.nv); v_partial_dq_local_world_aligned.setZero(); |
160 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero(); |
161 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_local_world_aligned(6,model.nv); a_partial_dq_local_world_aligned.setZero(); |
162 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero(); |
163 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero(); |
164 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_local_world_aligned(6,model.nv); a_partial_dv_local_world_aligned.setZero(); |
165 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero(); |
166 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero(); |
167 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_local_world_aligned(6,model.nv); a_partial_da_local_world_aligned.setZero(); |
168 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero(); |
169 |
|||
170 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,WORLD, |
171 |
v_partial_dq, |
||
172 |
a_partial_dq,a_partial_dv,a_partial_da); |
||
173 |
|||
174 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED, |
175 |
v_partial_dq_local_world_aligned, |
||
176 |
a_partial_dq_local_world_aligned, |
||
177 |
a_partial_dv_local_world_aligned, |
||
178 |
a_partial_da_local_world_aligned); |
||
179 |
|||
180 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,LOCAL, |
181 |
v_partial_dq_local, |
||
182 |
a_partial_dq_local,a_partial_dv_local,a_partial_da_local); |
||
183 |
|||
184 |
// Check v_partial_dq against getFrameVelocityDerivatives |
||
185 |
{ |
||
186 |
✓✗ | 4 |
Data data_v(model); |
187 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data_v,q,v,a); |
188 |
|||
189 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero(); |
190 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_ref_local_world_aligned(6,model.nv); v_partial_dq_ref_local_world_aligned.setZero(); |
191 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero(); |
192 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero(); |
193 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_ref_local_world_aligned(6,model.nv); v_partial_dv_ref_local_world_aligned.setZero(); |
194 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_ref_local(6,model.nv); v_partial_dv_ref_local.setZero(); |
195 |
|||
196 |
✓✗ | 2 |
getFrameVelocityDerivatives(model,data_v,frameId,WORLD, |
197 |
v_partial_dq_ref,v_partial_dv_ref); |
||
198 |
|||
199 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref)); |
200 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref)); |
201 |
|||
202 |
✓✗ | 2 |
getFrameVelocityDerivatives(model,data_v,frameId,LOCAL_WORLD_ALIGNED, |
203 |
v_partial_dq_ref_local_world_aligned,v_partial_dv_ref_local_world_aligned); |
||
204 |
|||
205 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned)); |
206 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned)); |
207 |
|||
208 |
✓✗ | 2 |
getFrameVelocityDerivatives(model,data_v,frameId,LOCAL, |
209 |
v_partial_dq_ref_local,v_partial_dv_ref_local); |
||
210 |
|||
211 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local)); |
212 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local)); |
213 |
} |
||
214 |
|||
215 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref(6,model.nv); J_ref.setZero(); |
216 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero(); |
217 |
✓✗✓✗ |
4 |
Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero(); |
218 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
219 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,frameId,WORLD,J_ref); |
220 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned); |
221 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local); |
222 |
|||
223 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da.isApprox(J_ref)); |
224 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned)); |
225 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local)); |
226 |
|||
227 |
// Check against finite differences |
||
228 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_fd(6,model.nv); a_partial_da_fd.setZero(); |
229 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_fd_local_world_aligned(6,model.nv); a_partial_da_fd_local_world_aligned.setZero(); |
230 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_fd_local(6,model.nv); a_partial_da_fd_local.setZero(); |
231 |
2 |
const double alpha = 1e-8; |
|
232 |
|||
233 |
✓✗✓✗ |
4 |
Eigen::VectorXd v_plus(v), a_plus(a); |
234 |
✓✗ | 4 |
Data data_plus(model); |
235 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v,a); |
236 |
|||
237 |
// dacc/da |
||
238 |
✓✗ | 2 |
Motion a0 = getFrameAcceleration(model,data,frameId,WORLD); |
239 |
✓✗ | 2 |
Motion a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED); |
240 |
✓✗ | 2 |
Motion a0_local = getFrameAcceleration(model,data,frameId,LOCAL); |
241 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
242 |
{ |
||
243 |
✓✗ | 64 |
a_plus[k] += alpha; |
244 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q,v,a_plus); |
245 |
|||
246 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_da_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha; |
247 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_da_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha; |
248 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_da_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha; |
249 |
✓✗ | 64 |
a_plus[k] -= alpha; |
250 |
} |
||
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd,sqrt(alpha))); |
252 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned,sqrt(alpha))); |
253 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha))); |
254 |
|||
255 |
// dacc/dv |
||
256 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_fd(6,model.nv); a_partial_dv_fd.setZero(); |
257 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_fd_local_world_aligned(6,model.nv); a_partial_dv_fd_local_world_aligned.setZero(); |
258 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_fd_local(6,model.nv); a_partial_dv_fd_local.setZero(); |
259 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
260 |
{ |
||
261 |
✓✗ | 64 |
v_plus[k] += alpha; |
262 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q,v_plus,a); |
263 |
|||
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_dv_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha; a_partial_dv_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha; |
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_dv_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha; |
266 |
✓✗ | 64 |
v_plus[k] -= alpha; |
267 |
} |
||
268 |
|||
269 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd,sqrt(alpha))); |
270 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned,sqrt(alpha))); |
271 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha))); |
272 |
|||
273 |
// dacc/dq |
||
274 |
✓✗ | 2 |
a_partial_dq.setZero(); |
275 |
✓✗ | 2 |
a_partial_dv.setZero(); |
276 |
✓✗ | 2 |
a_partial_da.setZero(); |
277 |
|||
278 |
✓✗ | 2 |
a_partial_dq_local_world_aligned.setZero(); |
279 |
✓✗ | 2 |
a_partial_dv_local_world_aligned.setZero(); |
280 |
✓✗ | 2 |
a_partial_da_local_world_aligned.setZero(); |
281 |
|||
282 |
✓✗ | 2 |
a_partial_dq_local.setZero(); |
283 |
✓✗ | 2 |
a_partial_dv_local.setZero(); |
284 |
✓✗ | 2 |
a_partial_da_local.setZero(); |
285 |
|||
286 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero(); |
287 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_fd_local_world_aligned(6,model.nv); a_partial_dq_fd_local_world_aligned.setZero(); |
288 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_fd_local(6,model.nv); a_partial_dq_fd_local.setZero(); |
289 |
|||
290 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data,q,v,a); |
291 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,WORLD, |
292 |
v_partial_dq, |
||
293 |
a_partial_dq,a_partial_dv,a_partial_da); |
||
294 |
|||
295 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED, |
296 |
v_partial_dq_local_world_aligned, |
||
297 |
a_partial_dq_local_world_aligned, |
||
298 |
a_partial_dv_local_world_aligned, |
||
299 |
a_partial_da_local_world_aligned); |
||
300 |
|||
301 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,LOCAL, |
302 |
v_partial_dq_local, |
||
303 |
a_partial_dq_local,a_partial_dv_local,a_partial_da_local); |
||
304 |
|||
305 |
✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv)); |
306 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v,a); |
307 |
✓✗ | 2 |
updateFramePlacements(model,data_ref); |
308 |
✓✗ | 2 |
a0 = getFrameAcceleration(model,data,frameId,WORLD); |
309 |
✓✗ | 2 |
a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED); |
310 |
✓✗ | 2 |
a0_local = getFrameAcceleration(model,data,frameId,LOCAL); |
311 |
|||
312 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
313 |
{ |
||
314 |
✓✗ | 64 |
v_eps[k] += alpha; |
315 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
316 |
✓✗ | 64 |
forwardKinematics(model,data_plus,q_plus,v,a); |
317 |
✓✗ | 64 |
updateFramePlacements(model,data_plus); |
318 |
|||
319 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_dq_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha; |
320 |
✓✗ | 64 |
Motion a_plus_local_world_aligned = getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED); |
321 |
✓✗✓✗ ✓✗✓✗ |
64 |
const SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation(); |
322 |
✓✗✓✗ ✓✗✓✗ |
64 |
a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans); |
323 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
a_partial_dq_fd_local_world_aligned.col(k) = (a_plus_local_world_aligned - a0_local_world_aligned).toVector()/alpha; |
324 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
a_partial_dq_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha; |
325 |
✓✗ | 64 |
v_eps[k] -= alpha; |
326 |
} |
||
327 |
|||
328 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd,sqrt(alpha))); |
329 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned,sqrt(alpha))); |
330 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha))); |
331 |
|||
332 |
// Test other signatures |
||
333 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_other(6,model.nv); v_partial_dq_other.setZero(); |
334 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_local_other(6,model.nv); v_partial_dq_local_other.setZero(); |
335 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq_local_world_aligned_other(6,model.nv); v_partial_dq_local_world_aligned_other.setZero(); |
336 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_other(6,model.nv); v_partial_dv_other.setZero(); |
337 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_local_other(6,model.nv); v_partial_dv_local_other.setZero(); |
338 |
✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dv_local_world_aligned_other(6,model.nv); v_partial_dv_local_world_aligned_other.setZero(); |
339 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_other(6,model.nv); a_partial_dq_other.setZero(); |
340 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_local_world_aligned_other(6,model.nv); a_partial_dq_local_world_aligned_other.setZero(); |
341 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dq_local_other(6,model.nv); a_partial_dq_local_other.setZero(); |
342 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_other(6,model.nv); a_partial_dv_other.setZero(); |
343 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_local_world_aligned_other(6,model.nv); a_partial_dv_local_world_aligned_other.setZero(); |
344 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_dv_local_other(6,model.nv); a_partial_dv_local_other.setZero(); |
345 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_other(6,model.nv); a_partial_da_other.setZero(); |
346 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_local_world_aligned_other(6,model.nv); a_partial_da_local_world_aligned_other.setZero(); |
347 |
✓✗✓✗ |
4 |
Data::Matrix6x a_partial_da_local_other(6,model.nv); a_partial_da_local_other.setZero(); |
348 |
|||
349 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,WORLD, |
350 |
v_partial_dq_other, |
||
351 |
v_partial_dv_other, |
||
352 |
a_partial_dq_other, |
||
353 |
a_partial_dv_other, |
||
354 |
a_partial_da_other); |
||
355 |
|||
356 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq_other.isApprox(v_partial_dq)); |
357 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dv_other.isApprox(a_partial_da)); |
358 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq_other.isApprox(a_partial_dq)); |
359 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv_other.isApprox(a_partial_dv)); |
360 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_other.isApprox(a_partial_da)); |
361 |
|||
362 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED, |
363 |
v_partial_dq_local_world_aligned_other, |
||
364 |
v_partial_dv_local_world_aligned_other, |
||
365 |
a_partial_dq_local_world_aligned_other, |
||
366 |
a_partial_dv_local_world_aligned_other, |
||
367 |
a_partial_da_local_world_aligned_other); |
||
368 |
|||
369 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq_local_world_aligned_other.isApprox(v_partial_dq_local_world_aligned)); |
370 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dv_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned)); |
371 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq_local_world_aligned_other.isApprox(a_partial_dq_local_world_aligned)); |
372 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv_local_world_aligned_other.isApprox(a_partial_dv_local_world_aligned)); |
373 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned)); |
374 |
|||
375 |
✓✗ | 2 |
getFrameAccelerationDerivatives(model,data,frameId,LOCAL, |
376 |
v_partial_dq_local_other, |
||
377 |
v_partial_dv_local_other, |
||
378 |
a_partial_dq_local_other, |
||
379 |
a_partial_dv_local_other, |
||
380 |
a_partial_da_local_other); |
||
381 |
|||
382 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dq_local_other.isApprox(v_partial_dq_local)); |
383 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_partial_dv_local_other.isApprox(a_partial_da_local)); |
384 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dq_local_other.isApprox(a_partial_dq_local)); |
385 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_dv_local_other.isApprox(a_partial_dv_local)); |
386 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_partial_da_local_other.isApprox(a_partial_da_local)); |
387 |
2 |
} |
|
388 |
|||
389 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |