| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/multibody/model.hpp" |
||
6 |
#include "pinocchio/multibody/data.hpp" |
||
7 |
#include "pinocchio/algorithm/model.hpp" |
||
8 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
9 |
#include "pinocchio/algorithm/frames.hpp" |
||
10 |
#include "pinocchio/algorithm/rnea.hpp" |
||
11 |
#include "pinocchio/algorithm/crba.hpp" |
||
12 |
#include "pinocchio/spatial/act-on-set.hpp" |
||
13 |
#include "pinocchio/parsers/sample-models.hpp" |
||
14 |
#include "pinocchio/utils/timer.hpp" |
||
15 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
16 |
|||
17 |
#include <iostream> |
||
18 |
|||
19 |
#include <boost/test/unit_test.hpp> |
||
20 |
#include <boost/utility/binary.hpp> |
||
21 |
|||
22 |
template<typename Derived> |
||
23 |
1 |
inline bool isFinite(const Eigen::MatrixBase<Derived> & x) |
|
24 |
{ |
||
25 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
return ((x - x).array() == (x - x).array()).all(); |
26 |
} |
||
27 |
|||
28 |
|||
29 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
30 |
|||
31 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(frame_basic) |
32 |
{ |
||
33 |
using namespace pinocchio; |
||
34 |
✓✗ | 4 |
Model model; |
35 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
36 |
|||
37 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(model.frames.size() >= size_t(model.njoints)); |
38 |
112 |
for(Model::FrameVector::const_iterator it = model.frames.begin(); |
|
39 |
✓✓ | 222 |
it != model.frames.end(); ++it) |
40 |
{ |
||
41 |
110 |
const Frame & frame = *it; |
|
42 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
110 |
BOOST_CHECK(frame == frame); |
43 |
✓✗ | 220 |
Frame frame_copy(frame); |
44 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
110 |
BOOST_CHECK(frame_copy == frame); |
45 |
} |
||
46 |
|||
47 |
✓✗ | 4 |
std::ostringstream os; |
48 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
os << Frame("toto",0,0,SE3::Random(),OP_FRAME) << std::endl; |
49 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!os.str().empty()); |
50 |
2 |
} |
|
51 |
|||
52 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(cast) |
53 |
{ |
||
54 |
using namespace pinocchio; |
||
55 |
✓✗✓✗ ✓✗✓✗ |
6 |
Frame frame("toto",0,0,SE3::Random(),OP_FRAME); |
56 |
|||
57 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(frame.cast<double>() == frame); |
58 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(frame.cast<double>().cast<long double>() == frame.cast<long double>()); |
59 |
2 |
} |
|
60 |
|||
61 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_kinematics ) |
62 |
{ |
||
63 |
using namespace Eigen; |
||
64 |
using namespace pinocchio; |
||
65 |
|||
66 |
✓✗ | 4 |
pinocchio::Model model; |
67 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
68 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
69 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
70 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
71 |
✓✗✓✗ ✓✗ |
2 |
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
72 |
✓✗ | 4 |
pinocchio::Data data(model); |
73 |
|||
74 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
75 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
76 |
✓✗ | 2 |
framesForwardKinematics(model, data, q); |
77 |
|||
78 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.oMf[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement)); |
79 |
|||
80 |
2 |
} |
|
81 |
|||
82 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_update_placements ) |
83 |
{ |
||
84 |
using namespace Eigen; |
||
85 |
using namespace pinocchio; |
||
86 |
|||
87 |
✓✗ | 4 |
pinocchio::Model model; |
88 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
89 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
90 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
91 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
92 |
✓✗✓✗ ✓✗ |
2 |
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
93 |
✓✗ | 4 |
pinocchio::Data data(model); |
94 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
95 |
|||
96 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
97 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
98 |
|||
99 |
✓✗ | 2 |
forwardKinematics(model, data, q); |
100 |
✓✗ | 2 |
updateFramePlacements(model, data); |
101 |
|||
102 |
✓✗ | 2 |
framesForwardKinematics(model, data_ref, q); |
103 |
|||
104 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx])); |
105 |
2 |
} |
|
106 |
|||
107 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_update_single_placement ) |
108 |
{ |
||
109 |
using namespace Eigen; |
||
110 |
using namespace pinocchio; |
||
111 |
|||
112 |
✓✗ | 4 |
pinocchio::Model model; |
113 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
114 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
115 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
116 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
117 |
✓✗✓✗ ✓✗ |
2 |
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
118 |
✓✗ | 4 |
pinocchio::Data data(model); |
119 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
120 |
|||
121 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
122 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
123 |
|||
124 |
✓✗ | 2 |
forwardKinematics(model, data, q); |
125 |
✓✗ | 2 |
updateFramePlacement(model, data, frame_idx); |
126 |
|||
127 |
✓✗ | 2 |
framesForwardKinematics(model, data_ref, q); |
128 |
|||
129 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx])); |
130 |
2 |
} |
|
131 |
|||
132 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_velocity ) |
133 |
{ |
||
134 |
using namespace Eigen; |
||
135 |
using namespace pinocchio; |
||
136 |
|||
137 |
✓✗ | 4 |
pinocchio::Model model; |
138 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
139 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
140 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
141 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
142 |
✓✗✓✗ ✓✗ |
2 |
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
143 |
✓✗ | 4 |
pinocchio::Data data(model); |
144 |
|||
145 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
146 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
147 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
148 |
✓✗ | 2 |
forwardKinematics(model, data, q, v); |
149 |
|||
150 |
✓✗ | 2 |
Motion vf = getFrameVelocity(model, data, frame_idx); |
151 |
|||
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx]))); |
153 |
|||
154 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
155 |
✓✗ | 2 |
forwardKinematics(model, data_ref, q, v); |
156 |
✓✗ | 2 |
updateFramePlacements(model, data_ref); |
157 |
✓✗ | 2 |
Motion v_ref = getFrameVelocity(model, data_ref, frame_idx); |
158 |
|||
159 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx))); |
160 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx,LOCAL))); |
161 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_ref.oMf[frame_idx].act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,WORLD))); |
162 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,LOCAL_WORLD_ALIGNED))); |
163 |
2 |
} |
|
164 |
|||
165 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_acceleration ) |
166 |
{ |
||
167 |
using namespace Eigen; |
||
168 |
using namespace pinocchio; |
||
169 |
|||
170 |
✓✗ | 4 |
pinocchio::Model model; |
171 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
172 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
173 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
174 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
175 |
✓✗✓✗ ✓✗ |
2 |
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
176 |
✓✗ | 4 |
pinocchio::Data data(model); |
177 |
|||
178 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
179 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
180 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
181 |
✓✗✓✗ |
4 |
VectorXd a = VectorXd::Ones(model.nv); |
182 |
✓✗ | 2 |
forwardKinematics(model, data, q, v, a); |
183 |
|||
184 |
✓✗ | 2 |
Motion af = getFrameAcceleration(model, data, frame_idx); |
185 |
|||
186 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx]))); |
187 |
|||
188 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
189 |
✓✗ | 2 |
forwardKinematics(model, data_ref, q, v, a); |
190 |
✓✗ | 2 |
updateFramePlacements(model, data_ref); |
191 |
✓✗ | 2 |
Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx); |
192 |
|||
193 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx))); |
194 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx,LOCAL))); |
195 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_ref.oMf[frame_idx].act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,WORLD))); |
196 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,LOCAL_WORLD_ALIGNED))); |
197 |
2 |
} |
|
198 |
|||
199 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_classic_acceleration ) |
200 |
{ |
||
201 |
using namespace Eigen; |
||
202 |
using namespace pinocchio; |
||
203 |
|||
204 |
✓✗ | 4 |
pinocchio::Model model; |
205 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
206 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
207 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
208 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
209 |
✓✗✓✗ ✓✗ |
2 |
Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
210 |
✓✗ | 4 |
pinocchio::Data data(model); |
211 |
|||
212 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
213 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
214 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
215 |
✓✗✓✗ |
4 |
VectorXd a = VectorXd::Ones(model.nv); |
216 |
✓✗ | 2 |
forwardKinematics(model, data, q, v, a); |
217 |
|||
218 |
✓✗ | 2 |
Motion vel = framePlacement.actInv(data.v[parent_idx]); |
219 |
✓✗ | 2 |
Motion acc = framePlacement.actInv(data.a[parent_idx]); |
220 |
✓✗ | 2 |
Vector3d linear; |
221 |
|||
222 |
✓✗ | 2 |
Motion acc_classical_local = acc; |
223 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
linear = acc.linear() + vel.angular().cross(vel.linear()); |
224 |
✓✗✓✗ |
2 |
acc_classical_local.linear() = linear; |
225 |
|||
226 |
✓✗ | 2 |
Motion af = getFrameClassicalAcceleration(model, data, frame_idx); |
227 |
|||
228 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(af.isApprox(acc_classical_local)); |
229 |
|||
230 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
231 |
✓✗ | 2 |
forwardKinematics(model, data_ref, q, v, a); |
232 |
✓✗ | 2 |
updateFramePlacements(model, data_ref); |
233 |
|||
234 |
✓✗ | 2 |
SE3 T_ref = data_ref.oMf[frame_idx]; |
235 |
✓✗ | 2 |
Motion v_ref = getFrameVelocity(model, data_ref, frame_idx); |
236 |
✓✗ | 2 |
Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx); |
237 |
|||
238 |
✓✗ | 2 |
Motion acc_classical_local_ref = a_ref; |
239 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
linear = a_ref.linear() + v_ref.angular().cross(v_ref.linear()); |
240 |
✓✗✓✗ |
2 |
acc_classical_local_ref.linear() = linear; |
241 |
|||
242 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx))); |
243 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,LOCAL))); |
244 |
|||
245 |
✓✗ | 2 |
Motion vel_world_ref = T_ref.act(v_ref); |
246 |
✓✗ | 2 |
Motion acc_classical_world_ref = T_ref.act(a_ref); |
247 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
linear = acc_classical_world_ref.linear() + vel_world_ref.angular().cross(vel_world_ref.linear()); |
248 |
✓✗✓✗ |
2 |
acc_classical_world_ref.linear() = linear; |
249 |
|||
250 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(acc_classical_world_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,WORLD))); |
251 |
|||
252 |
✓✗✓✗ ✓✗✓✗ |
2 |
Motion vel_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(v_ref); |
253 |
✓✗✓✗ ✓✗✓✗ |
2 |
Motion acc_classical_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(a_ref); |
254 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
linear = acc_classical_aligned_ref.linear() + vel_aligned_ref.angular().cross(vel_aligned_ref.linear()); |
255 |
✓✗✓✗ |
2 |
acc_classical_aligned_ref.linear() = linear; |
256 |
|||
257 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(acc_classical_aligned_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,LOCAL_WORLD_ALIGNED))); |
258 |
2 |
} |
|
259 |
|||
260 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_frame_getters) |
261 |
{ |
||
262 |
using namespace Eigen; |
||
263 |
using namespace pinocchio; |
||
264 |
|||
265 |
// Build a simple 1R planar model |
||
266 |
✓✗ | 4 |
Model model; |
267 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
JointIndex parentId = model.addJoint(0, JointModelRZ(), SE3::Identity(), "Joint1"); |
268 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
FrameIndex frameId = model.addFrame(Frame("Frame1", parentId, 0, SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), OP_FRAME)); |
269 |
|||
270 |
✓✗ | 4 |
Data data(model); |
271 |
|||
272 |
// Predetermined configuration values |
||
273 |
✓✗ | 4 |
VectorXd q(model.nq); |
274 |
✓✗ | 2 |
q << M_PI/2; |
275 |
|||
276 |
✓✗ | 4 |
VectorXd v(model.nv); |
277 |
✓✗ | 2 |
v << 1.0; |
278 |
|||
279 |
✓✗ | 4 |
VectorXd a(model.nv); |
280 |
✓✗ | 2 |
a << 0.0; |
281 |
|||
282 |
// Expected velocity |
||
283 |
✓✗ | 2 |
Motion v_local; |
284 |
✓✗✓✗ ✓✗ |
2 |
v_local.linear() = Vector3d(0.0, 1.0, 0.0); |
285 |
✓✗✓✗ ✓✗ |
2 |
v_local.angular() = Vector3d(0.0, 0.0, 1.0); |
286 |
|||
287 |
✓✗ | 2 |
Motion v_world; |
288 |
✓✗✓✗ ✓✗ |
2 |
v_world.linear() = Vector3d::Zero(); |
289 |
✓✗✓✗ ✓✗ |
2 |
v_world.angular() = Vector3d(0.0, 0.0, 1.0); |
290 |
|||
291 |
✓✗ | 2 |
Motion v_align; |
292 |
✓✗✓✗ ✓✗ |
2 |
v_align.linear() = Vector3d(-1.0, 0.0, 0.0); |
293 |
✓✗✓✗ ✓✗ |
2 |
v_align.angular() = Vector3d(0.0, 0.0, 1.0); |
294 |
|||
295 |
// Expected classical acceleration |
||
296 |
✓✗ | 2 |
Motion ac_local; |
297 |
✓✗✓✗ ✓✗ |
2 |
ac_local.linear() = Vector3d(-1.0, 0.0, 0.0); |
298 |
✓✗✓✗ ✓✗ |
2 |
ac_local.angular() = Vector3d::Zero(); |
299 |
|||
300 |
✓✗ | 2 |
Motion ac_world = Motion::Zero(); |
301 |
|||
302 |
✓✗ | 2 |
Motion ac_align; |
303 |
✓✗✓✗ ✓✗ |
2 |
ac_align.linear() = Vector3d(0.0, -1.0, 0.0); |
304 |
✓✗✓✗ ✓✗ |
2 |
ac_align.angular() = Vector3d::Zero(); |
305 |
|||
306 |
// Perform kinematics |
||
307 |
✓✗ | 2 |
forwardKinematics(model,data,q,v,a); |
308 |
|||
309 |
// Check output velocity |
||
310 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId))); |
311 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId,LOCAL))); |
312 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v_world.isApprox(getFrameVelocity(model,data,frameId,WORLD))); |
313 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v_align.isApprox(getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED))); |
314 |
|||
315 |
// Check output acceleration (all zero) |
||
316 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(getFrameAcceleration(model,data,frameId).isZero()); |
317 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(getFrameAcceleration(model,data,frameId,LOCAL).isZero()); |
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(getFrameAcceleration(model,data,frameId,WORLD).isZero()); |
319 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED).isZero()); |
320 |
|||
321 |
// Check output classical acceleration |
||
322 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId))); |
323 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId,LOCAL))); |
324 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(ac_world.isApprox(getFrameClassicalAcceleration(model,data,frameId,WORLD))); |
325 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(ac_align.isApprox(getFrameClassicalAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED))); |
326 |
2 |
} |
|
327 |
|||
328 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian ) |
329 |
{ |
||
330 |
using namespace Eigen; |
||
331 |
using namespace pinocchio; |
||
332 |
|||
333 |
✓✗ | 4 |
Model model; |
334 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
335 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
336 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
337 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
338 |
✓✗✓✗ ✓✗ |
2 |
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
339 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model.existFrame(frame_name)); |
340 |
|||
341 |
✓✗ | 4 |
pinocchio::Data data(model); |
342 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
343 |
|||
344 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
345 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill( 1.); |
346 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
347 |
✓✗✓✗ ✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
348 |
|||
349 |
/// In local frame |
||
350 |
✓✗ | 2 |
Model::Index idx = model.getFrameId(frame_name); |
351 |
2 |
const Frame & frame = model.frames[idx]; |
|
352 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement)); |
353 |
✓✗✓✗ |
4 |
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0); |
354 |
✓✗✓✗ |
4 |
Data::Matrix6x Jff(6,model.nv); Jff.fill(0); |
355 |
✓✗✓✗ |
4 |
Data::Matrix6x Jff2(6,model.nv); Jff2.fill(0); |
356 |
|||
357 |
✓✗ | 2 |
computeJointJacobians(model,data,q); |
358 |
✓✗ | 2 |
updateFramePlacement(model, data, idx); |
359 |
✓✗ | 2 |
getFrameJacobian(model, data, idx, LOCAL, Jff); |
360 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
361 |
✓✗ | 2 |
getJointJacobian(model, data_ref, parent_idx, LOCAL, Jjj); |
362 |
|||
363 |
✓✗✓✗ |
2 |
Motion nu_frame = Motion(Jff*v); |
364 |
✓✗✓✗ |
2 |
Motion nu_joint = Motion(Jjj*v); |
365 |
|||
366 |
✓✗ | 2 |
const SE3::ActionMatrixType jXf = frame.placement.toActionMatrix(); |
367 |
✓✗✓✗ |
4 |
Data::Matrix6x Jjj_from_frame(jXf * Jff); |
368 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jjj_from_frame.isApprox(Jjj)); |
369 |
|||
370 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12)); |
371 |
|||
372 |
// In world frame |
||
373 |
✓✗ | 2 |
getFrameJacobian(model,data,idx,WORLD,Jff); |
374 |
✓✗ | 2 |
getJointJacobian(model, data_ref, parent_idx,WORLD, Jjj); |
375 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jff.isApprox(Jjj)); |
376 |
|||
377 |
✓✗ | 2 |
computeFrameJacobian(model,data,q,idx,WORLD,Jff2); |
378 |
|||
379 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jff2.isApprox(Jjj)); |
380 |
2 |
} |
|
381 |
|||
382 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_frame_jacobian ) |
383 |
{ |
||
384 |
using namespace Eigen; |
||
385 |
using namespace pinocchio; |
||
386 |
|||
387 |
✓✗ | 4 |
Model model; |
388 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
389 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
390 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
391 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
392 |
✓✗✓✗ ✓✗ |
2 |
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
393 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model.existFrame(frame_name)); |
394 |
|||
395 |
✓✗ | 4 |
pinocchio::Data data(model); |
396 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
397 |
|||
398 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
399 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill( 1.); |
400 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
401 |
✓✗✓✗ ✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
402 |
|||
403 |
✓✗ | 2 |
Model::Index idx = model.getFrameId(frame_name); |
404 |
2 |
const Frame & frame = model.frames[idx]; |
|
405 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement)); |
406 |
✓✗✓✗ |
4 |
Data::Matrix6x Jf(6,model.nv); Jf.fill(0); |
407 |
✓✗✓✗ |
4 |
Data::Matrix6x Jf2(6,model.nv); Jf2.fill(0); |
408 |
✓✗✓✗ |
4 |
Data::Matrix6x Jf_ref(6,model.nv); Jf_ref.fill(0); |
409 |
|||
410 |
✓✗ | 2 |
computeFrameJacobian(model, data_ref, q, idx, Jf); |
411 |
|||
412 |
✓✗ | 2 |
computeJointJacobians(model, data_ref, q); |
413 |
✓✗ | 2 |
updateFramePlacement(model, data_ref, idx); |
414 |
✓✗ | 2 |
getFrameJacobian(model, data_ref, idx, LOCAL, Jf_ref); |
415 |
|||
416 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jf.isApprox(Jf_ref)); |
417 |
|||
418 |
✓✗ | 2 |
computeFrameJacobian(model,data,q,idx,LOCAL,Jf2); |
419 |
|||
420 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jf2.isApprox(Jf_ref)); |
421 |
2 |
} |
|
422 |
|||
423 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_frame_jacobian_local_world_oriented ) |
424 |
{ |
||
425 |
using namespace Eigen; |
||
426 |
using namespace pinocchio; |
||
427 |
|||
428 |
✓✗ | 4 |
Model model; |
429 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
430 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
431 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
432 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
433 |
✓✗✓✗ ✓✗ |
2 |
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
434 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(model.existFrame(frame_name)); |
435 |
|||
436 |
✓✗ | 4 |
pinocchio::Data data(model); |
437 |
|||
438 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
439 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill( 1.); |
440 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
441 |
✓✗✓✗ ✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
442 |
|||
443 |
✓✗✓✗ |
2 |
Model::Index idx = model.getFrameId(frame_name); |
444 |
✓✗✓✗ |
4 |
Data::Matrix6x Jf(6,model.nv); Jf.fill(0); |
445 |
✓✗✓✗ |
4 |
Data::Matrix6x Jf2(6,model.nv); Jf2.fill(0); |
446 |
✓✗✓✗ |
4 |
Data::Matrix6x Jf_ref(6,model.nv); Jf_ref.fill(0); |
447 |
|||
448 |
✓✗ | 2 |
computeJointJacobians(model, data, q); |
449 |
✓✗ | 2 |
updateFramePlacement(model, data, idx); |
450 |
✓✗ | 2 |
getFrameJacobian(model, data, idx, LOCAL, Jf_ref); |
451 |
|||
452 |
// Compute the jacobians. |
||
453 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
Jf_ref = SE3(data.oMf[idx].rotation(), Eigen::Vector3d::Zero()).toActionMatrix() * Jf_ref; |
454 |
✓✗ | 2 |
getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, Jf); |
455 |
|||
456 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jf.isApprox(Jf_ref)); |
457 |
|||
458 |
✓✗ | 2 |
computeFrameJacobian(model,data,q,idx,LOCAL_WORLD_ALIGNED,Jf2); |
459 |
|||
460 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jf2.isApprox(Jf_ref)); |
461 |
2 |
} |
|
462 |
|||
463 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation ) |
464 |
{ |
||
465 |
using namespace Eigen; |
||
466 |
using namespace pinocchio; |
||
467 |
|||
468 |
✓✗ | 4 |
pinocchio::Model model; |
469 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
470 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✗ ✗✗ |
2 |
Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1); |
471 |
✓✗ | 4 |
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame"); |
472 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
473 |
✓✗✓✗ ✓✗ |
2 |
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME)); |
474 |
✓✗ | 4 |
pinocchio::Data data(model); |
475 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
476 |
|||
477 |
✓✗✓✗ ✓✗✓✗ |
4 |
VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ); |
478 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Random(model.nv); |
479 |
✓✗✓✗ |
4 |
VectorXd a = VectorXd::Random(model.nv); |
480 |
|||
481 |
✓✗ | 2 |
computeJointJacobiansTimeVariation(model,data,q,v); |
482 |
✓✗ | 2 |
updateFramePlacements(model,data); |
483 |
|||
484 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v,a); |
485 |
✓✗ | 2 |
updateFramePlacements(model,data_ref); |
486 |
|||
487 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(isFinite(data.dJ)); |
488 |
|||
489 |
✓✗ | 2 |
Model::Index idx = model.getFrameId(frame_name); |
490 |
2 |
const Frame & frame = model.frames[idx]; |
|
491 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement)); |
492 |
|||
493 |
✓✗✓✗ |
4 |
Data::Matrix6x J(6,model.nv); J.fill(0.); |
494 |
✓✗✓✗ |
4 |
Data::Matrix6x dJ(6,model.nv); dJ.fill(0.); |
495 |
|||
496 |
// Regarding to the WORLD origin |
||
497 |
✓✗ | 2 |
getFrameJacobian(model,data,idx,WORLD,J); |
498 |
✓✗ | 2 |
getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ); |
499 |
|||
500 |
✓✗✓✗ |
2 |
Motion v_idx(J*v); |
501 |
✓✗ | 2 |
const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]); |
502 |
✓✗ | 2 |
const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local); |
503 |
|||
504 |
✓✗✓✗ ✓✗ |
2 |
const SE3 & wMf = SE3(data_ref.oMf[idx].rotation(),SE3::Vector3::Zero()); |
505 |
✓✗ | 2 |
const Motion & v_ref_local_world_aligned = wMf.act(v_ref_local); |
506 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_idx.isApprox(v_ref)); |
507 |
|||
508 |
✓✗✓✗ ✓✗✓✗ |
2 |
Motion a_idx(J*a + dJ*v); |
509 |
✓✗ | 2 |
const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]); |
510 |
✓✗ | 2 |
const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local); |
511 |
✓✗ | 2 |
const Motion & a_ref_local_world_aligned = wMf.act(a_ref_local); |
512 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_idx.isApprox(a_ref)); |
513 |
|||
514 |
✓✗✓✗ |
2 |
J.fill(0.); dJ.fill(0.); |
515 |
// Regarding to the LOCAL frame |
||
516 |
✓✗ | 2 |
getFrameJacobian(model,data,idx,LOCAL,J); |
517 |
✓✗ | 2 |
getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ); |
518 |
|||
519 |
✓✗✓✗ ✓✗ |
2 |
v_idx = (Motion::Vector6)(J*v); |
520 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_idx.isApprox(v_ref_local)); |
521 |
|||
522 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
a_idx = (Motion::Vector6)(J*a + dJ*v); |
523 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_idx.isApprox(a_ref_local)); |
524 |
|||
525 |
// Regarding to the LOCAL_WORLD_ALIGNED frame |
||
526 |
✓✗ | 2 |
getFrameJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,J); |
527 |
✓✗ | 2 |
getFrameJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ); |
528 |
|||
529 |
✓✗✓✗ ✓✗ |
2 |
v_idx = (Motion::Vector6)(J*v); |
530 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_idx.isApprox(v_ref_local_world_aligned)); |
531 |
|||
532 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
a_idx = (Motion::Vector6)(J*a + dJ*v); |
533 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a_idx.isApprox(a_ref_local_world_aligned)); |
534 |
|||
535 |
// compare to finite differencies |
||
536 |
{ |
||
537 |
✓✗✓✗ |
4 |
Data data_ref(model), data_ref_plus(model); |
538 |
|||
539 |
2 |
const double alpha = 1e-8; |
|
540 |
✓✗ | 4 |
Eigen::VectorXd q_plus(model.nq); |
541 |
✓✗✓✗ |
2 |
q_plus = integrate(model,q,alpha*v); |
542 |
|||
543 |
//data_ref |
||
544 |
✓✗✓✗ ✓✗ |
4 |
Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv), J_ref_local_world_aligned(6,model.nv); |
545 |
✓✗✓✗ ✓✗ |
2 |
J_ref_world.fill(0.); J_ref_local.fill(0.); J_ref_local_world_aligned.fill(0.); |
546 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
547 |
✓✗ | 2 |
updateFramePlacements(model,data_ref); |
548 |
2 |
const SE3 & oMf_q = data_ref.oMf[idx]; |
|
549 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,idx,WORLD,J_ref_world); |
550 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,idx,LOCAL,J_ref_local); |
551 |
✓✗ | 2 |
getFrameJacobian(model,data_ref,idx,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned); |
552 |
|||
553 |
//data_ref_plus |
||
554 |
✓✗✓✗ ✓✗ |
4 |
Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv), J_ref_plus_local_world_aligned(6,model.nv); |
555 |
✓✗✓✗ ✓✗ |
2 |
J_ref_plus_world.fill(0.); J_ref_plus_local.fill(0.); J_ref_plus_local_world_aligned.fill(0.); |
556 |
✓✗ | 2 |
computeJointJacobians(model,data_ref_plus,q_plus); |
557 |
✓✗ | 2 |
updateFramePlacements(model,data_ref_plus); |
558 |
2 |
const SE3 & oMf_q_plus = data_ref_plus.oMf[idx]; |
|
559 |
✓✗ | 2 |
getFrameJacobian(model,data_ref_plus,idx,WORLD,J_ref_plus_world); |
560 |
✓✗ | 2 |
getFrameJacobian(model,data_ref_plus,idx,LOCAL,J_ref_plus_local); |
561 |
✓✗ | 2 |
getFrameJacobian(model,data_ref_plus,idx,LOCAL_WORLD_ALIGNED,J_ref_plus_local_world_aligned); |
562 |
|||
563 |
//Move J_ref_plus_local to reference frame |
||
564 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
J_ref_plus_local = (oMf_q.inverse()*oMf_q_plus).toActionMatrix()*(J_ref_plus_local); |
565 |
|||
566 |
//Move J_ref_plus_local_world_aligned to reference frame |
||
567 |
✓✗ | 2 |
SE3 oMf_translation = SE3::Identity(); |
568 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
oMf_translation.translation() = oMf_q_plus.translation() - oMf_q.translation(); |
569 |
✓✗✓✗ ✓✗ |
2 |
J_ref_plus_local_world_aligned = oMf_translation.toActionMatrix()*(J_ref_plus_local_world_aligned); |
570 |
|||
571 |
✓✗✓✗ ✓✗ |
4 |
Data::Matrix6x dJ_ref_world(6,model.nv), dJ_ref_local(6,model.nv), dJ_ref_local_world_aligned(6,model.nv); |
572 |
✓✗✓✗ ✓✗ |
2 |
dJ_ref_world.fill(0.); dJ_ref_local.fill(0.); dJ_ref_local_world_aligned.fill(0.); |
573 |
✓✗✓✗ ✓✗ |
2 |
dJ_ref_world = (J_ref_plus_world - J_ref_world)/alpha; |
574 |
✓✗✓✗ ✓✗ |
2 |
dJ_ref_local = (J_ref_plus_local - J_ref_local)/alpha; |
575 |
✓✗✓✗ ✓✗ |
2 |
dJ_ref_local_world_aligned = (J_ref_plus_local_world_aligned - J_ref_local_world_aligned)/alpha; |
576 |
|||
577 |
//data |
||
578 |
✓✗ | 2 |
computeJointJacobiansTimeVariation(model,data,q,v); |
579 |
✓✗ | 2 |
forwardKinematics(model,data,q,v); |
580 |
✓✗ | 2 |
updateFramePlacements(model,data); |
581 |
✓✗✓✗ ✓✗ |
4 |
Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv), dJ_local_world_aligned(6,model.nv); |
582 |
✓✗✓✗ ✓✗ |
2 |
dJ_world.fill(0.); dJ_local.fill(0.); dJ_local_world_aligned.fill(0.); |
583 |
✓✗ | 2 |
getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ_world); |
584 |
✓✗ | 2 |
getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ_local); |
585 |
✓✗ | 2 |
getFrameJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ_local_world_aligned); |
586 |
|||
587 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dJ_world.isApprox(dJ_ref_world,sqrt(alpha))); |
588 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha))); |
589 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dJ_local_world_aligned.isApprox(dJ_ref_local_world_aligned,sqrt(alpha))); |
590 |
} |
||
591 |
2 |
} |
|
592 |
|||
593 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_supported_inertia_and_force) |
594 |
{ |
||
595 |
using namespace Eigen; |
||
596 |
using namespace pinocchio; |
||
597 |
|||
598 |
// Create a humanoid robot |
||
599 |
✓✗ | 4 |
Model model_free; |
600 |
✓✗ | 2 |
pinocchio::buildModels::humanoid(model_free, true); |
601 |
✓✗ | 4 |
Data data_free(model_free); |
602 |
|||
603 |
// Set freeflyer limits to allow for randomConfiguration |
||
604 |
✓✗✓✗ ✓✗ |
2 |
model_free.lowerPositionLimit.segment(0, 3) << Vector3d::Constant(-1); |
605 |
✓✗✓✗ ✓✗ |
2 |
model_free.upperPositionLimit.segment(0, 3) << Vector3d::Constant( 1); |
606 |
|||
607 |
// Joint of interest (that will be converted to frame) |
||
608 |
✓✗ | 4 |
const std::string joint_name = "chest2_joint"; |
609 |
✓✗ | 2 |
const JointIndex joint_id = model_free.getJointId(joint_name); |
610 |
|||
611 |
// Duplicate of the model with the joint of interest fixed (to make a frame) |
||
612 |
✓✗✓✗ ✓✗ |
6 |
pinocchio::Model model_fix = buildReducedModel(model_free, {joint_id}, neutral(model_free)); |
613 |
✓✗ | 4 |
Data data_fix(model_fix); |
614 |
✓✗ | 2 |
const FrameIndex frame_id = model_fix.getFrameId(joint_name); |
615 |
|||
616 |
// Const variable to help convert q, v, a from one model to another |
||
617 |
✓✗ | 2 |
const int joint_idx_q(model_free.joints[joint_id].idx_q()); |
618 |
✓✗ | 2 |
const int joint_idx_v(model_free.joints[joint_id].idx_v()); |
619 |
|||
620 |
// Pick random q, v, a |
||
621 |
✓✗ | 4 |
VectorXd q_free = randomConfiguration(model_free); |
622 |
✓✗✓✗ |
4 |
VectorXd v_free(VectorXd::Random(model_free.nv)); |
623 |
✓✗✓✗ |
4 |
VectorXd a_free(VectorXd::Random(model_free.nv)); |
624 |
|||
625 |
// Set joint of interest to q, v, a = 0 to mimic the fixed joint |
||
626 |
✓✗ | 2 |
q_free[joint_idx_q] = 0; |
627 |
✓✗ | 2 |
v_free[joint_idx_v] = 0; |
628 |
✓✗ | 2 |
a_free[joint_idx_v] = 0; |
629 |
|||
630 |
// Adapt configuration for fixed joint model |
||
631 |
✓✗ | 4 |
VectorXd q_fix(model_fix.nq); |
632 |
✓✗✓✗ ✓✗✓✗ |
2 |
q_fix << q_free.head(joint_idx_q), q_free.tail(model_free.nq - joint_idx_q - 1); |
633 |
✓✗ | 4 |
VectorXd v_fix(model_fix.nv); |
634 |
✓✗✓✗ ✓✗✓✗ |
2 |
v_fix << v_free.head(joint_idx_v), v_free.tail(model_free.nv - joint_idx_v - 1); |
635 |
✓✗ | 4 |
VectorXd a_fix(model_fix.nv); |
636 |
✓✗✓✗ ✓✗✓✗ |
2 |
a_fix << a_free.head(joint_idx_v), a_free.tail(model_free.nv - joint_idx_v - 1); |
637 |
|||
638 |
// Compute inertia/force for both models differently |
||
639 |
✓✗ | 2 |
forwardKinematics(model_fix, data_fix, q_fix, v_fix, a_fix); |
640 |
✓✗ | 2 |
crba(model_free, data_free, q_free); |
641 |
|||
642 |
✓✗ | 2 |
Inertia inertia_fix = computeSupportedInertiaByFrame(model_fix, data_fix, frame_id, false); |
643 |
✓✗ | 2 |
Inertia inertia_free(model_free.inertias[joint_id]); |
644 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(inertia_fix.isApprox(inertia_free)); |
645 |
|||
646 |
✓✗✓✗ |
2 |
inertia_fix = computeSupportedInertiaByFrame(model_fix, data_fix, frame_id, true); |
647 |
✓✗ | 2 |
inertia_free = data_free.Ycrb[joint_id]; |
648 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(inertia_fix.isApprox(inertia_free)); |
649 |
|||
650 |
✓✗ | 2 |
rnea(model_fix, data_fix, q_fix, v_fix, a_fix); |
651 |
✓✗ | 2 |
rnea(model_free, data_free, q_free, v_free, a_free); |
652 |
|||
653 |
✓✗ | 2 |
Force force_fix = computeSupportedForceByFrame(model_fix, data_fix, frame_id); |
654 |
✓✗ | 2 |
Force force_free(data_free.f[joint_id]); |
655 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(force_fix.isApprox(force_free)); |
656 |
2 |
} |
|
657 |
BOOST_AUTO_TEST_SUITE_END () |
| Generated by: GCOVR (Version 4.2) |