Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2023 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/math/fwd.hpp" |
6 |
|
|
#include "pinocchio/multibody/joint/joints.hpp" |
7 |
|
|
#include "pinocchio/algorithm/rnea.hpp" |
8 |
|
|
#include "pinocchio/algorithm/aba.hpp" |
9 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
10 |
|
|
#include "pinocchio/algorithm/jacobian.hpp" |
11 |
|
|
#include "pinocchio/algorithm/compute-all-terms.hpp" |
12 |
|
|
|
13 |
|
|
#include <boost/test/unit_test.hpp> |
14 |
|
|
#include <iostream> |
15 |
|
|
|
16 |
|
|
using namespace pinocchio; |
17 |
|
|
using namespace Eigen; |
18 |
|
|
|
19 |
|
|
template<typename D> |
20 |
|
✗ |
void addJointAndBody( |
21 |
|
|
Model & model, |
22 |
|
|
const JointModelBase<D> & jmodel, |
23 |
|
|
const Model::JointIndex parent_id, |
24 |
|
|
const SE3 & joint_placement, |
25 |
|
|
const std::string & joint_name, |
26 |
|
|
const Inertia & Y) |
27 |
|
|
{ |
28 |
|
|
Model::JointIndex idx; |
29 |
|
|
|
30 |
|
✗ |
idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name); |
31 |
|
✗ |
model.appendBodyToJoint(idx, Y); |
32 |
|
|
} |
33 |
|
|
|
34 |
|
|
BOOST_AUTO_TEST_SUITE(JointUniversal) |
35 |
|
|
|
36 |
|
✗ |
BOOST_AUTO_TEST_CASE(vsRXRY) |
37 |
|
|
{ |
38 |
|
|
typedef SE3::Vector3 Vector3; |
39 |
|
|
typedef SE3::Matrix3 Matrix3; |
40 |
|
|
|
41 |
|
✗ |
Vector3 axis1; |
42 |
|
✗ |
axis1 << 1.0, 0.0, 0.0; |
43 |
|
✗ |
Vector3 axis2; |
44 |
|
✗ |
axis2 << 0.0, 1.0, 0.0; |
45 |
|
|
|
46 |
|
✗ |
Model modelUniversal, modelRXRY; |
47 |
|
✗ |
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity()); |
48 |
|
|
|
49 |
|
✗ |
JointModelUniversal joint_model_U(axis1, axis2); |
50 |
|
✗ |
addJointAndBody(modelUniversal, joint_model_U, 0, SE3::Identity(), "universal", inertia); |
51 |
|
|
|
52 |
|
✗ |
JointModelComposite joint_model_RXRY; |
53 |
|
✗ |
joint_model_RXRY.addJoint(JointModelRX()); |
54 |
|
✗ |
joint_model_RXRY.addJoint(JointModelRY()); |
55 |
|
✗ |
addJointAndBody(modelRXRY, joint_model_RXRY, 0, SE3::Identity(), "rxry", inertia); |
56 |
|
|
|
57 |
|
✗ |
Data dataUniversal(modelUniversal); |
58 |
|
✗ |
Data dataRXRY(modelRXRY); |
59 |
|
|
|
60 |
|
✗ |
BOOST_CHECK(modelUniversal.nv == modelRXRY.nv); |
61 |
|
✗ |
BOOST_CHECK(modelUniversal.nq == modelRXRY.nq); |
62 |
|
|
|
63 |
|
✗ |
Eigen::VectorXd q = Eigen::VectorXd::Ones(modelRXRY.nq); |
64 |
|
|
|
65 |
|
✗ |
forwardKinematics(modelRXRY, dataRXRY, q); |
66 |
|
✗ |
forwardKinematics(modelUniversal, dataUniversal, q); |
67 |
|
|
|
68 |
|
✗ |
BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRXRY.oMi.back())); |
69 |
|
✗ |
BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRXRY.liMi.back())); |
70 |
|
✗ |
BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRXRY.Ycrb.back().matrix())); |
71 |
|
|
|
72 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Ones(modelRXRY.nv); |
73 |
|
✗ |
forwardKinematics(modelRXRY, dataRXRY, q, v); |
74 |
|
✗ |
forwardKinematics(modelUniversal, dataUniversal, q, v); |
75 |
|
|
|
76 |
|
✗ |
BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRXRY.oMi.back())); |
77 |
|
✗ |
BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRXRY.liMi.back())); |
78 |
|
✗ |
BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRXRY.Ycrb.back().matrix())); |
79 |
|
|
|
80 |
|
✗ |
computeAllTerms(modelRXRY, dataRXRY, q, v); |
81 |
|
✗ |
computeAllTerms(modelUniversal, dataUniversal, q, v); |
82 |
|
|
|
83 |
|
✗ |
BOOST_CHECK(dataUniversal.com.back().isApprox(dataRXRY.com.back())); |
84 |
|
✗ |
BOOST_CHECK(dataUniversal.nle.isApprox(dataRXRY.nle)); |
85 |
|
✗ |
BOOST_CHECK(dataUniversal.f.back().toVector().isApprox(dataRXRY.f.back().toVector())); |
86 |
|
|
|
87 |
|
|
// InverseDynamics == rnea |
88 |
|
✗ |
Eigen::VectorXd a = Eigen::VectorXd::Ones(modelRXRY.nv); |
89 |
|
|
|
90 |
|
✗ |
Eigen::VectorXd tauRXRY = rnea(modelRXRY, dataRXRY, q, v, a); |
91 |
|
✗ |
Eigen::VectorXd tauUniversal = rnea(modelUniversal, dataUniversal, q, v, a); |
92 |
|
|
|
93 |
|
✗ |
BOOST_CHECK(tauUniversal.isApprox(tauRXRY)); |
94 |
|
|
|
95 |
|
|
// ForwardDynamics == aba |
96 |
|
✗ |
Eigen::VectorXd aAbaRXRY = aba(modelRXRY, dataRXRY, q, v, tauRXRY, Convention::WORLD); |
97 |
|
|
Eigen::VectorXd aAbaUniversal = |
98 |
|
✗ |
aba(modelUniversal, dataUniversal, q, v, tauUniversal, Convention::WORLD); |
99 |
|
|
|
100 |
|
✗ |
BOOST_CHECK(aAbaUniversal.isApprox(aAbaRXRY)); |
101 |
|
|
|
102 |
|
|
// CRBA |
103 |
|
✗ |
crba(modelRXRY, dataRXRY, q, Convention::WORLD); |
104 |
|
✗ |
crba(modelUniversal, dataUniversal, q, Convention::WORLD); |
105 |
|
|
|
106 |
|
✗ |
BOOST_CHECK(dataUniversal.M.isApprox(dataRXRY.M)); |
107 |
|
|
|
108 |
|
|
// Jacobian |
109 |
|
✗ |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRXRY; |
110 |
|
✗ |
jacobianRXRY.resize(6, 2); |
111 |
|
✗ |
jacobianRXRY.setZero(); |
112 |
|
✗ |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianUniversal; |
113 |
|
✗ |
jacobianUniversal.resize(6, 2); |
114 |
|
✗ |
jacobianUniversal.setZero(); |
115 |
|
|
|
116 |
|
✗ |
computeJointJacobians(modelRXRY, dataRXRY, q); |
117 |
|
✗ |
computeJointJacobians(modelUniversal, dataUniversal, q); |
118 |
|
✗ |
getJointJacobian(modelRXRY, dataRXRY, 1, LOCAL, jacobianRXRY); |
119 |
|
✗ |
getJointJacobian(modelUniversal, dataUniversal, 1, LOCAL, jacobianUniversal); |
120 |
|
|
|
121 |
|
✗ |
BOOST_CHECK(jacobianUniversal.isApprox(jacobianRXRY)); |
122 |
|
|
} |
123 |
|
|
|
124 |
|
✗ |
BOOST_AUTO_TEST_CASE(vsRandomAxis) |
125 |
|
|
{ |
126 |
|
|
typedef SE3::Vector3 Vector3; |
127 |
|
|
typedef SE3::Matrix3 Matrix3; |
128 |
|
|
|
129 |
|
✗ |
Vector3 axis1; |
130 |
|
✗ |
axis1 << 0., 0., 1.; |
131 |
|
✗ |
Vector3 axis2; |
132 |
|
✗ |
axis2 << -1., 0., 0.; |
133 |
|
|
|
134 |
|
✗ |
Model modelUniversal, modelRandomAxis; |
135 |
|
✗ |
Inertia inertia = Inertia::Random(); |
136 |
|
|
|
137 |
|
✗ |
JointModelUniversal joint_model_U(axis1, axis2); |
138 |
|
✗ |
addJointAndBody(modelUniversal, joint_model_U, 0, SE3::Identity(), "universal", inertia); |
139 |
|
|
|
140 |
|
✗ |
JointModelComposite joint_model_RandomAxis; |
141 |
|
✗ |
joint_model_RandomAxis.addJoint(JointModelRevoluteUnaligned(axis1)); |
142 |
|
✗ |
joint_model_RandomAxis.addJoint(JointModelRevoluteUnaligned(axis2)); |
143 |
|
✗ |
addJointAndBody( |
144 |
|
✗ |
modelRandomAxis, joint_model_RandomAxis, 0, SE3::Identity(), "random_axis", inertia); |
145 |
|
|
|
146 |
|
✗ |
Data dataUniversal(modelUniversal); |
147 |
|
✗ |
Data dataRandomAxis(modelRandomAxis); |
148 |
|
|
|
149 |
|
✗ |
BOOST_CHECK(modelUniversal.nv == modelRandomAxis.nv); |
150 |
|
✗ |
BOOST_CHECK(modelUniversal.nq == modelRandomAxis.nq); |
151 |
|
|
|
152 |
|
✗ |
Eigen::VectorXd q = Eigen::VectorXd::Ones(modelRandomAxis.nq); |
153 |
|
|
|
154 |
|
✗ |
forwardKinematics(modelRandomAxis, dataRandomAxis, q); |
155 |
|
✗ |
forwardKinematics(modelUniversal, dataUniversal, q); |
156 |
|
|
|
157 |
|
✗ |
BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRandomAxis.oMi.back())); |
158 |
|
✗ |
BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRandomAxis.liMi.back())); |
159 |
|
✗ |
BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRandomAxis.Ycrb.back().matrix())); |
160 |
|
|
|
161 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Ones(modelRandomAxis.nv); |
162 |
|
✗ |
forwardKinematics(modelRandomAxis, dataRandomAxis, q, v); |
163 |
|
✗ |
forwardKinematics(modelUniversal, dataUniversal, q, v); |
164 |
|
|
|
165 |
|
✗ |
BOOST_CHECK(dataUniversal.oMi.back().isApprox(dataRandomAxis.oMi.back())); |
166 |
|
✗ |
BOOST_CHECK(dataUniversal.liMi.back().isApprox(dataRandomAxis.liMi.back())); |
167 |
|
✗ |
BOOST_CHECK(dataUniversal.Ycrb.back().matrix().isApprox(dataRandomAxis.Ycrb.back().matrix())); |
168 |
|
|
|
169 |
|
✗ |
computeAllTerms(modelRandomAxis, dataRandomAxis, q, v); |
170 |
|
✗ |
computeAllTerms(modelUniversal, dataUniversal, q, v); |
171 |
|
|
|
172 |
|
✗ |
BOOST_CHECK(dataUniversal.com.back().isApprox(dataRandomAxis.com.back())); |
173 |
|
✗ |
BOOST_CHECK(dataUniversal.nle.isApprox(dataRandomAxis.nle)); |
174 |
|
✗ |
BOOST_CHECK(dataUniversal.f.back().toVector().isApprox(dataRandomAxis.f.back().toVector())); |
175 |
|
|
|
176 |
|
|
// InverseDynamics == rnea |
177 |
|
✗ |
Eigen::VectorXd a = Eigen::VectorXd::Ones(modelRandomAxis.nv); |
178 |
|
|
|
179 |
|
✗ |
Eigen::VectorXd tauRandomAxis = rnea(modelRandomAxis, dataRandomAxis, q, v, a); |
180 |
|
✗ |
Eigen::VectorXd tauUniversal = rnea(modelUniversal, dataUniversal, q, v, a); |
181 |
|
|
|
182 |
|
✗ |
BOOST_CHECK(tauUniversal.isApprox(tauRandomAxis)); |
183 |
|
|
|
184 |
|
|
// ForwardDynamics == aba |
185 |
|
|
Eigen::VectorXd aAbaRandomAxis = |
186 |
|
✗ |
aba(modelRandomAxis, dataRandomAxis, q, v, tauRandomAxis, Convention::WORLD); |
187 |
|
|
Eigen::VectorXd aAbaUniversal = |
188 |
|
✗ |
aba(modelUniversal, dataUniversal, q, v, tauUniversal, Convention::WORLD); |
189 |
|
|
|
190 |
|
✗ |
BOOST_CHECK(aAbaUniversal.isApprox(aAbaRandomAxis)); |
191 |
|
|
|
192 |
|
|
// CRBA |
193 |
|
✗ |
crba(modelRandomAxis, dataRandomAxis, q, Convention::WORLD); |
194 |
|
✗ |
crba(modelUniversal, dataUniversal, q, Convention::WORLD); |
195 |
|
|
|
196 |
|
✗ |
BOOST_CHECK(dataUniversal.M.isApprox(dataRandomAxis.M)); |
197 |
|
|
|
198 |
|
|
// Jacobian |
199 |
|
✗ |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRandomAxis; |
200 |
|
✗ |
jacobianRandomAxis.resize(6, 2); |
201 |
|
✗ |
jacobianRandomAxis.setZero(); |
202 |
|
✗ |
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianUniversal; |
203 |
|
✗ |
jacobianUniversal.resize(6, 2); |
204 |
|
✗ |
jacobianUniversal.setZero(); |
205 |
|
|
|
206 |
|
✗ |
computeJointJacobians(modelRandomAxis, dataRandomAxis, q); |
207 |
|
✗ |
computeJointJacobians(modelUniversal, dataUniversal, q); |
208 |
|
✗ |
getJointJacobian(modelRandomAxis, dataRandomAxis, 1, LOCAL, jacobianRandomAxis); |
209 |
|
✗ |
getJointJacobian(modelUniversal, dataUniversal, 1, LOCAL, jacobianUniversal); |
210 |
|
|
|
211 |
|
✗ |
BOOST_CHECK(jacobianUniversal.isApprox(jacobianRandomAxis)); |
212 |
|
|
} |
213 |
|
|
|
214 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
215 |
|
|
|