| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2021 CNRS INRIA |
||
3 |
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
||
4 |
// |
||
5 |
|||
6 |
#include "pinocchio/math/fwd.hpp" |
||
7 |
#include "pinocchio/multibody/joint/joints.hpp" |
||
8 |
#include "pinocchio/algorithm/rnea.hpp" |
||
9 |
#include "pinocchio/algorithm/aba.hpp" |
||
10 |
#include "pinocchio/algorithm/crba.hpp" |
||
11 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
12 |
#include "pinocchio/algorithm/compute-all-terms.hpp" |
||
13 |
|||
14 |
#include <boost/test/unit_test.hpp> |
||
15 |
#include <iostream> |
||
16 |
|||
17 |
using namespace pinocchio; |
||
18 |
|||
19 |
template<typename D> |
||
20 |
12 |
void addJointAndBody(Model & model, |
|
21 |
const JointModelBase<D> & jmodel, |
||
22 |
const Model::JointIndex parent_id, |
||
23 |
const SE3 & joint_placement, |
||
24 |
const std::string & joint_name, |
||
25 |
const Inertia & Y) |
||
26 |
{ |
||
27 |
Model::JointIndex idx; |
||
28 |
|||
29 |
✓✗ | 12 |
idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name); |
30 |
✓✗ | 12 |
model.appendBodyToJoint(idx,Y); |
31 |
12 |
} |
|
32 |
|||
33 |
BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned) |
||
34 |
|||
35 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(vsRX) |
36 |
{ |
||
37 |
using namespace pinocchio; |
||
38 |
typedef SE3::Vector3 Vector3; |
||
39 |
typedef SE3::Matrix3 Matrix3; |
||
40 |
|||
41 |
✓✗ | 2 |
Vector3 axis; |
42 |
✓✗✓✗ ✓✗ |
2 |
axis << 1.0, 0.0, 0.0; |
43 |
|||
44 |
✓✗✓✗ |
4 |
Model modelRX, modelRevoluteUnaligned; |
45 |
|||
46 |
✓✗✓✗ ✓✗✓✗ |
2 |
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ()); |
47 |
✓✗✓✗ ✓✗ |
2 |
SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.); |
48 |
|||
49 |
✓✗ | 2 |
JointModelRevoluteUnaligned joint_model_RU(axis); |
50 |
|||
51 |
✓✗✓✗ ✓✗ |
2 |
addJointAndBody(modelRX,JointModelRX(),0,pos,"rx",inertia); |
52 |
✓✗✓✗ |
2 |
addJointAndBody(modelRevoluteUnaligned,joint_model_RU,0,pos,"revolute-unaligned",inertia); |
53 |
|||
54 |
✓✗ | 4 |
Data dataRX(modelRX); |
55 |
✓✗ | 4 |
Data dataRevoluteUnaligned(modelRevoluteUnaligned); |
56 |
|||
57 |
✓✗✓✗ |
4 |
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRX.nq); |
58 |
✓✗✓✗ |
4 |
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelRX.nv); |
59 |
✓✗✓✗ |
4 |
Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv); |
60 |
✓✗✓✗ |
4 |
Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUnaligned.nv); |
61 |
✓✗✓✗ |
4 |
Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv); |
62 |
✓✗ | 4 |
Eigen::VectorXd aRevoluteUnaligned(aRX); |
63 |
|||
64 |
✓✗ | 2 |
forwardKinematics(modelRX, dataRX, q, v); |
65 |
✓✗ | 2 |
forwardKinematics(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v); |
66 |
|||
67 |
✓✗ | 2 |
computeAllTerms(modelRX, dataRX, q, v); |
68 |
✓✗ | 2 |
computeAllTerms(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v); |
69 |
|||
70 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnaligned.oMi[1].isApprox(dataRX.oMi[1])); |
71 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnaligned.liMi[1].isApprox(dataRX.liMi[1])); |
72 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnaligned.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix())); |
73 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnaligned.f[1].toVector().isApprox(dataRX.f[1].toVector())); |
74 |
|||
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnaligned.nle.isApprox(dataRX.nle)); |
76 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnaligned.com[0].isApprox(dataRX.com[0])); |
77 |
|||
78 |
// InverseDynamics == rnea |
||
79 |
✓✗✓✗ |
2 |
tauRX = rnea(modelRX, dataRX, q, v, aRX); |
80 |
✓✗✓✗ |
2 |
tauRevoluteUnaligned = rnea(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v, aRevoluteUnaligned); |
81 |
|||
82 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned)); |
83 |
|||
84 |
// ForwardDynamics == aba |
||
85 |
✓✗✓✗ |
4 |
Eigen::VectorXd aAbaRX = aba(modelRX,dataRX, q, v, tauRX); |
86 |
✓✗✓✗ |
4 |
Eigen::VectorXd aAbaRevoluteUnaligned = aba(modelRevoluteUnaligned,dataRevoluteUnaligned, q, v, tauRevoluteUnaligned); |
87 |
|||
88 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned)); |
89 |
|||
90 |
// CRBA |
||
91 |
✓✗ | 2 |
crba(modelRX, dataRX,q); |
92 |
✓✗ | 2 |
crba(modelRevoluteUnaligned, dataRevoluteUnaligned, q); |
93 |
|||
94 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnaligned.M)); |
95 |
|||
96 |
// Jacobian |
||
97 |
✓✗✓✗ ✓✗ |
4 |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRX;jacobianRX.resize(6,1); jacobianRX.setZero(); |
98 |
✓✗✓✗ ✓✗ |
4 |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRevoluteUnaligned;jacobianRevoluteUnaligned.resize(6,1);jacobianRevoluteUnaligned.setZero(); |
99 |
✓✗ | 2 |
computeJointJacobians(modelRX, dataRX, q); |
100 |
✓✗ | 2 |
computeJointJacobians(modelRevoluteUnaligned, dataRevoluteUnaligned, q); |
101 |
✓✗ | 2 |
getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianRX); |
102 |
✓✗ | 2 |
getJointJacobian(modelRevoluteUnaligned, dataRevoluteUnaligned, 1, LOCAL, jacobianRevoluteUnaligned); |
103 |
|||
104 |
|||
105 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned)); |
106 |
2 |
} |
|
107 |
BOOST_AUTO_TEST_SUITE_END() |
||
108 |
|||
109 |
BOOST_AUTO_TEST_SUITE(JointRevoluteUnboundedUnaligned) |
||
110 |
|||
111 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(vsRUX) |
112 |
{ |
||
113 |
using namespace pinocchio; |
||
114 |
typedef SE3::Vector3 Vector3; |
||
115 |
typedef SE3::Matrix3 Matrix3; |
||
116 |
|||
117 |
✓✗ | 2 |
Vector3 axis; |
118 |
✓✗✓✗ ✓✗ |
2 |
axis << 1.0, 0.0, 0.0; |
119 |
|||
120 |
✓✗✓✗ |
4 |
Model modelRUX, modelRevoluteUboundedUnaligned; |
121 |
|||
122 |
✓✗✓✗ ✓✗✓✗ |
2 |
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ()); |
123 |
✓✗✓✗ ✓✗ |
2 |
SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.); |
124 |
|||
125 |
✓✗ | 2 |
JointModelRevoluteUnboundedUnaligned joint_model_RUU(axis); |
126 |
typedef traits< JointRevoluteUnboundedUnalignedTpl<double> >::TangentVector_t TangentVector; |
||
127 |
|||
128 |
✓✗✓✗ ✓✗ |
2 |
addJointAndBody(modelRUX,JointModelRUBX(),0,pos,"rux",inertia); |
129 |
✓✗✓✗ |
2 |
addJointAndBody(modelRevoluteUboundedUnaligned,joint_model_RUU,0,pos,"revolute-unbounded-unaligned",inertia); |
130 |
|||
131 |
✓✗ | 4 |
Data dataRUX(modelRUX); |
132 |
✓✗ | 4 |
Data dataRevoluteUnboundedUnaligned(modelRevoluteUboundedUnaligned); |
133 |
|||
134 |
✓✗✓✗ |
4 |
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRUX.nq); |
135 |
✓✗ | 2 |
q.normalize(); |
136 |
✓✗✓✗ |
2 |
TangentVector v = TangentVector::Ones (modelRUX.nv); |
137 |
✓✗✓✗ |
4 |
Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRUX.nv); |
138 |
✓✗✓✗ |
4 |
Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUboundedUnaligned.nv); |
139 |
✓✗✓✗ |
4 |
Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRUX.nv); |
140 |
✓✗ | 4 |
Eigen::VectorXd aRevoluteUnaligned(aRX); |
141 |
|||
142 |
✓✗ | 2 |
forwardKinematics(modelRUX, dataRUX, q, v); |
143 |
✓✗ | 2 |
forwardKinematics(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v); |
144 |
|||
145 |
✓✗ | 2 |
computeAllTerms(modelRUX, dataRUX, q, v); |
146 |
✓✗ | 2 |
computeAllTerms(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v); |
147 |
|||
148 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnboundedUnaligned.oMi[1].isApprox(dataRUX.oMi[1])); |
149 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnboundedUnaligned.liMi[1].isApprox(dataRUX.liMi[1])); |
150 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnboundedUnaligned.Ycrb[1].matrix().isApprox(dataRUX.Ycrb[1].matrix())); |
151 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnboundedUnaligned.f[1].toVector().isApprox(dataRUX.f[1].toVector())); |
152 |
|||
153 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnboundedUnaligned.nle.isApprox(dataRUX.nle)); |
154 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnboundedUnaligned.com[0].isApprox(dataRUX.com[0])); |
155 |
|||
156 |
// InverseDynamics == rnea |
||
157 |
✓✗✓✗ |
2 |
tauRX = rnea(modelRUX, dataRUX, q, v, aRX); |
158 |
✓✗✓✗ |
2 |
tauRevoluteUnaligned = rnea(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v, aRevoluteUnaligned); |
159 |
|||
160 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned)); |
161 |
|||
162 |
// ForwardDynamics == aba |
||
163 |
✓✗✓✗ |
4 |
Eigen::VectorXd aAbaRX = aba(modelRUX, dataRUX, q, v, tauRX); |
164 |
✓✗✓✗ |
4 |
Eigen::VectorXd aAbaRevoluteUnaligned = aba(modelRevoluteUboundedUnaligned,dataRevoluteUnboundedUnaligned, q, v, tauRevoluteUnaligned); |
165 |
|||
166 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned)); |
167 |
|||
168 |
// CRBA |
||
169 |
✓✗ | 2 |
crba(modelRUX, dataRUX,q); |
170 |
✓✗ | 2 |
crba(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q); |
171 |
|||
172 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRUX.M.isApprox(dataRevoluteUnboundedUnaligned.M)); |
173 |
|||
174 |
// Jacobian |
||
175 |
✓✗✓✗ ✓✗ |
4 |
Data::Matrix6x jacobianRUX;jacobianRUX.resize(6,1); jacobianRUX.setZero(); |
176 |
✓✗ | 4 |
Data::Matrix6x jacobianRevoluteUnboundedUnaligned; |
177 |
✓✗✓✗ |
2 |
jacobianRevoluteUnboundedUnaligned.resize(6,1); jacobianRevoluteUnboundedUnaligned.setZero(); |
178 |
|||
179 |
✓✗ | 2 |
computeJointJacobians(modelRUX, dataRUX, q); |
180 |
✓✗ | 2 |
computeJointJacobians(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q); |
181 |
✓✗ | 2 |
getJointJacobian(modelRUX, dataRUX, 1, LOCAL, jacobianRUX); |
182 |
✓✗ | 2 |
getJointJacobian(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, 1, LOCAL, jacobianRevoluteUnboundedUnaligned); |
183 |
|||
184 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jacobianRUX.isApprox(jacobianRevoluteUnboundedUnaligned)); |
185 |
2 |
} |
|
186 |
|||
187 |
BOOST_AUTO_TEST_SUITE_END() |
||
188 |
|||
189 |
BOOST_AUTO_TEST_SUITE(JointRevoluteUnbounded) |
||
190 |
|||
191 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(spatial) |
192 |
{ |
||
193 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
194 |
✓✗ | 2 |
Motion v(Motion::Random()); |
195 |
|||
196 |
2 |
MotionRevoluteTpl<double,0,0> mp_x(2.); |
|
197 |
✓✗ | 2 |
Motion mp_dense_x(mp_x); |
198 |
|||
199 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x))); |
200 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x))); |
201 |
|||
202 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x))); |
203 |
|||
204 |
2 |
MotionRevoluteTpl<double,0,1> mp_y(2.); |
|
205 |
✓✗ | 2 |
Motion mp_dense_y(mp_y); |
206 |
|||
207 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y))); |
208 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y))); |
209 |
|||
210 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y))); |
211 |
|||
212 |
2 |
MotionRevoluteTpl<double,0,2> mp_z(2.); |
|
213 |
✓✗ | 2 |
Motion mp_dense_z(mp_z); |
214 |
|||
215 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z))); |
216 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z))); |
217 |
|||
218 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z))); |
219 |
2 |
} |
|
220 |
|||
221 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(vsRX) |
222 |
{ |
||
223 |
typedef SE3::Vector3 Vector3; |
||
224 |
typedef SE3::Matrix3 Matrix3; |
||
225 |
|||
226 |
✓✗✓✗ |
4 |
Model modelRX, modelRevoluteUnbounded; |
227 |
|||
228 |
✓✗✓✗ ✓✗✓✗ |
2 |
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity()); |
229 |
✓✗✓✗ ✓✗ |
2 |
SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.); |
230 |
|||
231 |
✓✗ | 2 |
JointModelRUBX joint_model_RUX; |
232 |
✓✗✓✗ ✓✗✓✗ |
2 |
addJointAndBody(modelRX,JointModelRX(),0,SE3::Identity(),"rx",inertia); |
233 |
✓✗✓✗ ✓✗ |
2 |
addJointAndBody(modelRevoluteUnbounded,joint_model_RUX,0,SE3::Identity(),"revolute unbounded x",inertia); |
234 |
|||
235 |
✓✗ | 4 |
Data dataRX(modelRX); |
236 |
✓✗ | 4 |
Data dataRevoluteUnbounded(modelRevoluteUnbounded); |
237 |
|||
238 |
✓✗✓✗ |
4 |
Eigen::VectorXd q_rx = Eigen::VectorXd::Ones(modelRX.nq); |
239 |
✓✗✓✗ |
4 |
Eigen::VectorXd q_rubx = Eigen::VectorXd::Ones(modelRevoluteUnbounded.nq); |
240 |
✓✗ | 2 |
double ca, sa; double alpha = q_rx(0); SINCOS(alpha, &sa, &ca); |
241 |
✓✗ | 2 |
q_rubx(0) = ca; |
242 |
✓✗ | 2 |
q_rubx(1) = sa; |
243 |
✓✗✓✗ |
4 |
Eigen::VectorXd v_rx = Eigen::VectorXd::Ones(modelRX.nv); |
244 |
✓✗ | 4 |
Eigen::VectorXd v_rubx = v_rx; |
245 |
✓✗✓✗ |
4 |
Eigen::VectorXd tauRX = Eigen::VectorXd::Ones(modelRX.nv); |
246 |
✓✗✓✗ |
4 |
Eigen::VectorXd tauRevoluteUnbounded = Eigen::VectorXd::Ones(modelRevoluteUnbounded.nv); |
247 |
✓✗✓✗ |
4 |
Eigen::VectorXd aRX = Eigen::VectorXd::Ones(modelRX.nv); |
248 |
✓✗ | 4 |
Eigen::VectorXd aRevoluteUnbounded = aRX; |
249 |
|||
250 |
✓✗ | 2 |
forwardKinematics(modelRX, dataRX, q_rx, v_rx); |
251 |
✓✗ | 2 |
forwardKinematics(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx); |
252 |
|||
253 |
✓✗ | 2 |
computeAllTerms(modelRX, dataRX, q_rx, v_rx); |
254 |
✓✗ | 2 |
computeAllTerms(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx); |
255 |
|||
256 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnbounded.oMi[1].isApprox(dataRX.oMi[1])); |
257 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnbounded.liMi[1].isApprox(dataRX.liMi[1])); |
258 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnbounded.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix())); |
259 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnbounded.f[1].toVector().isApprox(dataRX.f[1].toVector())); |
260 |
|||
261 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnbounded.nle.isApprox(dataRX.nle)); |
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRevoluteUnbounded.com[0].isApprox(dataRX.com[0])); |
263 |
|||
264 |
// InverseDynamics == rnea |
||
265 |
✓✗✓✗ |
2 |
tauRX = rnea(modelRX, dataRX, q_rx, v_rx, aRX); |
266 |
✓✗✓✗ |
2 |
tauRevoluteUnbounded = rnea(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx, aRevoluteUnbounded); |
267 |
|||
268 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(tauRX.isApprox(tauRevoluteUnbounded)); |
269 |
|||
270 |
// ForwardDynamics == aba |
||
271 |
✓✗✓✗ |
4 |
Eigen::VectorXd aAbaRX= aba(modelRX,dataRX, q_rx, v_rx, tauRX); |
272 |
✓✗✓✗ |
4 |
Eigen::VectorXd aAbaRevoluteUnbounded = aba(modelRevoluteUnbounded,dataRevoluteUnbounded, q_rubx, v_rubx, tauRevoluteUnbounded); |
273 |
|||
274 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnbounded)); |
275 |
|||
276 |
// crba |
||
277 |
✓✗ | 2 |
crba(modelRX, dataRX,q_rx); |
278 |
✓✗ | 2 |
crba(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx); |
279 |
|||
280 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnbounded.M)); |
281 |
|||
282 |
// Jacobian |
||
283 |
✓✗✓✗ ✓✗ |
4 |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero(); |
284 |
✓✗✓✗ ✓✗ |
4 |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero(); |
285 |
✓✗ | 2 |
computeJointJacobians(modelRX, dataRX, q_rx); |
286 |
✓✗ | 2 |
computeJointJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx); |
287 |
✓✗ | 2 |
getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianPX); |
288 |
✓✗ | 2 |
getJointJacobian(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, LOCAL, jacobianPrismaticUnaligned); |
289 |
|||
290 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned)); |
291 |
|||
292 |
2 |
} |
|
293 |
BOOST_AUTO_TEST_SUITE_END() |
||
294 |
|||
295 |
BOOST_AUTO_TEST_SUITE(JointRevolute) |
||
296 |
|||
297 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(spatial) |
298 |
{ |
||
299 |
typedef TransformRevoluteTpl<double,0,0> TransformX; |
||
300 |
typedef TransformRevoluteTpl<double,0,1> TransformY; |
||
301 |
typedef TransformRevoluteTpl<double,0,2> TransformZ; |
||
302 |
|||
303 |
typedef SE3::Vector3 Vector3; |
||
304 |
|||
305 |
2 |
const double alpha = 0.2; |
|
306 |
2 |
double sin_alpha, cos_alpha; SINCOS(alpha,&sin_alpha,&cos_alpha); |
|
307 |
✓✗✓✗ |
2 |
SE3 Mplain, Mrand(SE3::Random()); |
308 |
|||
309 |
2 |
TransformX Mx(sin_alpha,cos_alpha); |
|
310 |
✓✗ | 2 |
Mplain = Mx; |
311 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(Mplain.translation().isZero()); |
312 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitX()).toRotationMatrix())); |
313 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx)); |
314 |
|||
315 |
2 |
TransformY My(sin_alpha,cos_alpha); |
|
316 |
✓✗ | 2 |
Mplain = My; |
317 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(Mplain.translation().isZero()); |
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitY()).toRotationMatrix())); |
319 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My)); |
320 |
|||
321 |
2 |
TransformZ Mz(sin_alpha,cos_alpha); |
|
322 |
✓✗ | 2 |
Mplain = Mz; |
323 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(Mplain.translation().isZero()); |
324 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitZ()).toRotationMatrix())); |
325 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz)); |
326 |
|||
327 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
328 |
✓✗ | 2 |
Motion v(Motion::Random()); |
329 |
|||
330 |
2 |
MotionRevoluteTpl<double,0,0> mp_x(2.); |
|
331 |
✓✗ | 2 |
Motion mp_dense_x(mp_x); |
332 |
|||
333 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x))); |
334 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x))); |
335 |
|||
336 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x))); |
337 |
|||
338 |
2 |
MotionRevoluteTpl<double,0,1> mp_y(2.); |
|
339 |
✓✗ | 2 |
Motion mp_dense_y(mp_y); |
340 |
|||
341 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y))); |
342 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y))); |
343 |
|||
344 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y))); |
345 |
|||
346 |
2 |
MotionRevoluteTpl<double,0,2> mp_z(2.); |
|
347 |
✓✗ | 2 |
Motion mp_dense_z(mp_z); |
348 |
|||
349 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z))); |
350 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z))); |
351 |
|||
352 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z))); |
353 |
2 |
} |
|
354 |
|||
355 |
BOOST_AUTO_TEST_SUITE_END() |
||
356 |
|||
357 |
BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned) |
||
358 |
|||
359 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(spatial) |
360 |
{ |
||
361 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
362 |
✓✗ | 2 |
Motion v(Motion::Random()); |
363 |
|||
364 |
✓✗✓✗ |
2 |
MotionRevoluteUnaligned mp(MotionRevoluteUnaligned::Vector3(1.,2.,3.),6.); |
365 |
✓✗ | 2 |
Motion mp_dense(mp); |
366 |
|||
367 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense))); |
368 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense))); |
369 |
|||
370 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense))); |
371 |
2 |
} |
|
372 |
|||
373 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |