| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2015-2019 CNRS INRIA |
| 3 |
|
|
// |
| 4 |
|
|
|
| 5 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
| 6 |
|
|
#include "pinocchio/algorithm/centroidal.hpp" |
| 7 |
|
|
#include "pinocchio/algorithm/rnea.hpp" |
| 8 |
|
|
#include "pinocchio/algorithm/jacobian.hpp" |
| 9 |
|
|
#include "pinocchio/algorithm/center-of-mass.hpp" |
| 10 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
| 11 |
|
|
|
| 12 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
| 13 |
|
|
|
| 14 |
|
|
#include "pinocchio/utils/timer.hpp" |
| 15 |
|
|
|
| 16 |
|
|
#include <iostream> |
| 17 |
|
|
|
| 18 |
|
|
#include <boost/test/unit_test.hpp> |
| 19 |
|
|
#include <boost/utility/binary.hpp> |
| 20 |
|
|
|
| 21 |
|
|
template<typename JointModel> |
| 22 |
|
✗ |
static void addJointAndBody( |
| 23 |
|
|
pinocchio::Model & model, |
| 24 |
|
|
const pinocchio::JointModelBase<JointModel> & joint, |
| 25 |
|
|
const std::string & parent_name, |
| 26 |
|
|
const std::string & name, |
| 27 |
|
|
const pinocchio::SE3 placement = pinocchio::SE3::Random(), |
| 28 |
|
|
bool setRandomLimits = true) |
| 29 |
|
|
{ |
| 30 |
|
|
using namespace pinocchio; |
| 31 |
|
|
typedef typename JointModel::ConfigVector_t CV; |
| 32 |
|
|
typedef typename JointModel::TangentVector_t TV; |
| 33 |
|
|
|
| 34 |
|
|
Model::JointIndex idx; |
| 35 |
|
|
|
| 36 |
|
✗ |
if (setRandomLimits) |
| 37 |
|
✗ |
idx = model.addJoint( |
| 38 |
|
|
model.getJointId(parent_name), joint, SE3::Random(), name + "_joint", |
| 39 |
|
|
TV::Random() + TV::Constant(1), TV::Random() + TV::Constant(1), |
| 40 |
|
|
CV::Random() - CV::Constant(1), CV::Random() + CV::Constant(1)); |
| 41 |
|
|
else |
| 42 |
|
✗ |
idx = model.addJoint(model.getJointId(parent_name), joint, placement, name + "_joint"); |
| 43 |
|
|
|
| 44 |
|
✗ |
model.addJointFrame(idx); |
| 45 |
|
|
|
| 46 |
|
✗ |
model.appendBodyToJoint(idx, Inertia::Random(), SE3::Identity()); |
| 47 |
|
✗ |
model.addBodyFrame(name + "_body", idx); |
| 48 |
|
|
} |
| 49 |
|
|
|
| 50 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
| 51 |
|
|
|
| 52 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_ccrba) |
| 53 |
|
|
{ |
| 54 |
|
✗ |
pinocchio::Model model; |
| 55 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
| 56 |
|
✗ |
pinocchio::Data data(model), data_ref(model); |
| 57 |
|
|
|
| 58 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
| 59 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
| 60 |
|
✗ |
Eigen::VectorXd q = randomConfiguration(model); |
| 61 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv); |
| 62 |
|
|
|
| 63 |
|
✗ |
pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL); |
| 64 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 65 |
|
✗ |
data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 66 |
|
|
|
| 67 |
|
✗ |
data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]); |
| 68 |
|
|
|
| 69 |
|
✗ |
pinocchio::Data data_ref_other(model); |
| 70 |
|
✗ |
pinocchio::crba(model, data_ref_other, q, pinocchio::Convention::WORLD); |
| 71 |
|
✗ |
data_ref_other.M.triangularView<Eigen::StrictlyLower>() = |
| 72 |
|
✗ |
data_ref_other.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 73 |
|
✗ |
BOOST_CHECK(data_ref_other.M.isApprox(data_ref.M)); |
| 74 |
|
|
|
| 75 |
|
✗ |
pinocchio::SE3 cMo(pinocchio::SE3::Matrix3::Identity(), -data_ref.Ycrb[0].lever()); |
| 76 |
|
✗ |
BOOST_CHECK(data_ref.Ycrb[0].isApprox(data_ref_other.oYcrb[0])); |
| 77 |
|
|
|
| 78 |
|
✗ |
ccrba(model, data, q, v); |
| 79 |
|
✗ |
BOOST_CHECK(data.com[0].isApprox(-cMo.translation(), 1e-12)); |
| 80 |
|
✗ |
BOOST_CHECK(data.oYcrb[0].matrix().isApprox(data_ref.Ycrb[0].matrix(), 1e-12)); |
| 81 |
|
|
|
| 82 |
|
✗ |
pinocchio::Inertia Ig_ref(cMo.act(data.oYcrb[0])); |
| 83 |
|
✗ |
BOOST_CHECK(data.Ig.matrix().isApprox(Ig_ref.matrix(), 1e-12)); |
| 84 |
|
|
|
| 85 |
|
✗ |
pinocchio::SE3 oM1(data_ref.liMi[1]); |
| 86 |
|
✗ |
pinocchio::SE3 cM1(cMo * oM1); |
| 87 |
|
|
|
| 88 |
|
|
pinocchio::Data::Matrix6x Ag_ref( |
| 89 |
|
✗ |
cM1.inverse().toActionMatrix().transpose() * data_ref.M.topRows<6>()); |
| 90 |
|
✗ |
BOOST_CHECK(data.Ag.isApprox(Ag_ref, 1e-12)); |
| 91 |
|
|
} |
| 92 |
|
|
|
| 93 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_centroidal_mapping) |
| 94 |
|
|
{ |
| 95 |
|
✗ |
pinocchio::Model model; |
| 96 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
| 97 |
|
✗ |
pinocchio::Data data(model), data_ref(model); |
| 98 |
|
|
|
| 99 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
| 100 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
| 101 |
|
✗ |
Eigen::VectorXd q = randomConfiguration(model); |
| 102 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
| 103 |
|
|
|
| 104 |
|
✗ |
computeCentroidalMap(model, data, q); |
| 105 |
|
✗ |
ccrba(model, data_ref, q, v); |
| 106 |
|
|
|
| 107 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
| 108 |
|
✗ |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
| 109 |
|
|
|
| 110 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
| 111 |
|
|
|
| 112 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
| 113 |
|
|
} |
| 114 |
|
|
|
| 115 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_dccrb) |
| 116 |
|
|
{ |
| 117 |
|
|
using namespace pinocchio; |
| 118 |
|
✗ |
Model model; |
| 119 |
|
✗ |
buildModels::humanoidRandom(model); |
| 120 |
|
✗ |
addJointAndBody(model, JointModelSpherical(), "larm6_joint", "larm7"); |
| 121 |
|
✗ |
Data data(model), data_ref(model); |
| 122 |
|
|
|
| 123 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
| 124 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
| 125 |
|
|
|
| 126 |
|
|
Eigen::VectorXd q = |
| 127 |
|
✗ |
randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit); |
| 128 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); |
| 129 |
|
✗ |
Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv); |
| 130 |
|
|
|
| 131 |
|
✗ |
const Eigen::VectorXd g = rnea(model, data_ref, q, 0 * v, 0 * a); |
| 132 |
|
✗ |
rnea(model, data_ref, q, v, a); |
| 133 |
|
|
|
| 134 |
|
✗ |
pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL); |
| 135 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 136 |
|
✗ |
data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 137 |
|
|
|
| 138 |
|
✗ |
SE3 cMo(SE3::Identity()); |
| 139 |
|
✗ |
data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]); |
| 140 |
|
✗ |
cMo.translation() = -data_ref.Ycrb[0].lever(); |
| 141 |
|
|
|
| 142 |
|
✗ |
SE3 oM1(data_ref.liMi[1]); |
| 143 |
|
✗ |
SE3 cM1(cMo * oM1); |
| 144 |
|
✗ |
Data::Matrix6x Ag_ref(cM1.toDualActionMatrix() * data_ref.M.topRows<6>()); |
| 145 |
|
|
|
| 146 |
|
✗ |
Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>()))); |
| 147 |
|
|
|
| 148 |
|
✗ |
ccrba(model, data_ref, q, v); |
| 149 |
|
✗ |
dccrba(model, data, q, v); |
| 150 |
|
✗ |
BOOST_CHECK(data.Ag.isApprox(Ag_ref)); |
| 151 |
|
✗ |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
| 152 |
|
✗ |
BOOST_CHECK(data.hg.isApprox(data_ref.hg)); |
| 153 |
|
|
|
| 154 |
|
✗ |
centerOfMass(model, data_ref, q, v, a); |
| 155 |
|
✗ |
BOOST_CHECK(data_ref.com[0].isApprox(data_ref.Ycrb[0].lever())); |
| 156 |
|
✗ |
BOOST_CHECK(data_ref.vcom[0].isApprox(data.hg.linear() / data_ref.M(0, 0))); |
| 157 |
|
✗ |
BOOST_CHECK(data_ref.vcom[0].isApprox(data.vcom[0])); |
| 158 |
|
✗ |
BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear() / data_ref.M(0, 0))); |
| 159 |
|
|
|
| 160 |
|
✗ |
Force hdot(data.Ag * a + data.dAg * v); |
| 161 |
|
✗ |
BOOST_CHECK(hdot.isApprox(hdot_ref)); |
| 162 |
|
|
|
| 163 |
|
✗ |
dccrba(model, data, q, 0 * v); |
| 164 |
|
✗ |
BOOST_CHECK(data.dAg.isZero()); |
| 165 |
|
|
|
| 166 |
|
|
// Check that dYcrb is equal to doYcrb |
| 167 |
|
|
{ |
| 168 |
|
|
// Compute dYcrb |
| 169 |
|
✗ |
Data data_ref(model), data_ref_plus(model), data(model); |
| 170 |
|
|
|
| 171 |
|
✗ |
const double alpha = 1e-8; |
| 172 |
|
✗ |
Eigen::VectorXd q_plus(model.nq); |
| 173 |
|
✗ |
q_plus = integrate(model, q, alpha * v); |
| 174 |
|
|
|
| 175 |
|
✗ |
forwardKinematics(model, data_ref, q); |
| 176 |
|
✗ |
pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL); |
| 177 |
|
✗ |
pinocchio::crba(model, data_ref_plus, q_plus, pinocchio::Convention::LOCAL); |
| 178 |
|
✗ |
forwardKinematics(model, data_ref_plus, q_plus); |
| 179 |
|
✗ |
dccrba(model, data, q, v); |
| 180 |
|
|
|
| 181 |
|
✗ |
for (size_t i = 1; i < (size_t)model.njoints; ++i) |
| 182 |
|
|
{ |
| 183 |
|
✗ |
Inertia::Matrix6 dYcrb = (data_ref_plus.oMi[i].act(data_ref_plus.Ycrb[i]).matrix() |
| 184 |
|
✗ |
- data_ref.oMi[i].act(data_ref.Ycrb[i]).matrix()) |
| 185 |
|
✗ |
/ alpha; |
| 186 |
|
✗ |
BOOST_CHECK(data.doYcrb[i].isApprox(dYcrb, sqrt(alpha))); |
| 187 |
|
|
} |
| 188 |
|
|
} |
| 189 |
|
|
|
| 190 |
|
|
{ |
| 191 |
|
✗ |
Data data(model); |
| 192 |
|
✗ |
ccrba(model, data_ref, q, v); |
| 193 |
|
✗ |
SE3 oMc_ref(SE3::Identity()); |
| 194 |
|
✗ |
oMc_ref.translation() = data_ref.com[0]; |
| 195 |
|
✗ |
const Data::Matrix6x Ag_ref = oMc_ref.toDualActionMatrix() * data_ref.Ag; |
| 196 |
|
✗ |
pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL); |
| 197 |
|
|
const Data::Matrix6x Ag_ref_from_M = |
| 198 |
|
✗ |
data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>(); |
| 199 |
|
|
|
| 200 |
|
✗ |
const double alpha = 1e-8; |
| 201 |
|
✗ |
Eigen::VectorXd q_plus(model.nq); |
| 202 |
|
✗ |
q_plus = integrate(model, q, alpha * v); |
| 203 |
|
✗ |
ccrba(model, data_ref, q_plus, v); |
| 204 |
|
✗ |
SE3 oMc_ref_plus(SE3::Identity()); |
| 205 |
|
✗ |
oMc_ref_plus.translation() = data_ref.com[0]; |
| 206 |
|
✗ |
const Data::Matrix6x Ag_plus_ref = oMc_ref_plus.toDualActionMatrix() * data_ref.Ag; |
| 207 |
|
✗ |
pinocchio::crba(model, data_ref, q_plus, pinocchio::Convention::LOCAL); |
| 208 |
|
|
const Data::Matrix6x Ag_plus_ref_from_M = |
| 209 |
|
✗ |
data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>(); |
| 210 |
|
✗ |
const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref) / alpha; |
| 211 |
|
✗ |
const Data::Matrix6x dAg_ref_from_M = (Ag_plus_ref_from_M - Ag_ref_from_M) / alpha; |
| 212 |
|
|
|
| 213 |
|
✗ |
dccrba(model, data, q, v); |
| 214 |
|
✗ |
SE3 oMc(SE3::Identity()); |
| 215 |
|
✗ |
oMc.translation() = data.com[0]; |
| 216 |
|
✗ |
Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg; |
| 217 |
|
✗ |
BOOST_CHECK(oMc.isApprox(oMc_ref)); |
| 218 |
|
✗ |
BOOST_CHECK(dAg_ref.isApprox(dAg_ref_from_M, sqrt(alpha))); |
| 219 |
|
|
} |
| 220 |
|
|
|
| 221 |
|
|
// Check for dAg/dt |
| 222 |
|
|
{ |
| 223 |
|
✗ |
Data data(model); |
| 224 |
|
✗ |
ccrba(model, data_ref, q, v); |
| 225 |
|
✗ |
const Data::Matrix6x Ag_ref = data_ref.Ag; |
| 226 |
|
|
|
| 227 |
|
✗ |
const double alpha = 1e-8; |
| 228 |
|
✗ |
Eigen::VectorXd q_plus(model.nq); |
| 229 |
|
✗ |
q_plus = integrate(model, q, alpha * v); |
| 230 |
|
✗ |
ccrba(model, data_ref, q_plus, v); |
| 231 |
|
✗ |
const Data::Matrix6x Ag_plus_ref = data_ref.Ag; |
| 232 |
|
✗ |
const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref) / alpha; |
| 233 |
|
|
|
| 234 |
|
✗ |
dccrba(model, data, q, v); |
| 235 |
|
✗ |
BOOST_CHECK(data.dAg.isApprox(dAg_ref, sqrt(alpha))); |
| 236 |
|
|
} |
| 237 |
|
|
|
| 238 |
|
|
// Compute tensor dAg/dq |
| 239 |
|
|
{ |
| 240 |
|
✗ |
std::vector<Data::Matrix6x> dAgdq((size_t)model.nv, Data::Matrix6x::Zero(6, model.nv)); |
| 241 |
|
✗ |
Data data(model), data_fd(model); |
| 242 |
|
✗ |
Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv)); |
| 243 |
|
✗ |
ccrba(model, data_fd, q, v); |
| 244 |
|
✗ |
SE3 oMc_ref(SE3::Identity()); |
| 245 |
|
✗ |
oMc_ref.translation() = data_fd.com[0]; |
| 246 |
|
|
|
| 247 |
|
✗ |
Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag; |
| 248 |
|
✗ |
const Force hg0 = oMc_ref.act(data_fd.hg); |
| 249 |
|
|
|
| 250 |
|
✗ |
Data::Matrix6x Ag_fd(6, model.nv); |
| 251 |
|
✗ |
Force hg_fd; |
| 252 |
|
✗ |
const double alpha = 1e-8; |
| 253 |
|
✗ |
Eigen::VectorXd q_plus(model.nq); |
| 254 |
|
✗ |
Data::Matrix6x dhdq(6, model.nv); |
| 255 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
| 256 |
|
|
{ |
| 257 |
|
✗ |
v_fd[k] = alpha; |
| 258 |
|
✗ |
q_plus = integrate(model, q, v_fd); |
| 259 |
|
✗ |
ccrba(model, data_fd, q_plus, v); |
| 260 |
|
✗ |
SE3 oMc_fd(SE3::Identity()); |
| 261 |
|
✗ |
oMc_fd.translation() = data_fd.com[0]; |
| 262 |
|
✗ |
Ag_fd = oMc_fd.toDualActionMatrix() * data_fd.Ag; |
| 263 |
|
✗ |
hg_fd = oMc_fd.act(data_fd.hg); |
| 264 |
|
✗ |
dAgdq[(size_t)k] = (Ag_fd - Ag0) / alpha; |
| 265 |
|
✗ |
dhdq.col(k) = (hg_fd - hg0).toVector() / alpha; |
| 266 |
|
✗ |
v_fd[k] = 0.; |
| 267 |
|
|
} |
| 268 |
|
|
|
| 269 |
|
✗ |
Data::Matrix6x dAg_ref(6, model.nv); |
| 270 |
|
✗ |
dAg_ref.setZero(); |
| 271 |
|
✗ |
for (int k = 0; k < model.nv; ++k) |
| 272 |
|
|
{ |
| 273 |
|
✗ |
dAg_ref.col(k) = dAgdq[(size_t)k] * v; |
| 274 |
|
|
} |
| 275 |
|
|
|
| 276 |
|
✗ |
BOOST_CHECK(dhdq.isApprox(dAg_ref, sqrt(alpha))); |
| 277 |
|
✗ |
BOOST_CHECK((dAg_ref * v).isApprox(dhdq * v, sqrt(alpha))); |
| 278 |
|
|
} |
| 279 |
|
|
} |
| 280 |
|
|
|
| 281 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_centroidal_mapping_time_derivative) |
| 282 |
|
|
{ |
| 283 |
|
✗ |
pinocchio::Model model; |
| 284 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
| 285 |
|
✗ |
pinocchio::Data data(model), data_ref(model); |
| 286 |
|
|
|
| 287 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
| 288 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
| 289 |
|
✗ |
Eigen::VectorXd q = randomConfiguration(model); |
| 290 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
| 291 |
|
|
|
| 292 |
|
✗ |
computeCentroidalMapTimeVariation(model, data, q, v); |
| 293 |
|
✗ |
dccrba(model, data_ref, q, v); |
| 294 |
|
|
|
| 295 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
| 296 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
| 297 |
|
✗ |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
| 298 |
|
✗ |
BOOST_CHECK(data.dAg.isApprox(data_ref.dAg)); |
| 299 |
|
|
|
| 300 |
|
✗ |
computeJointJacobiansTimeVariation(model, data_ref, q, v); |
| 301 |
|
|
|
| 302 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
| 303 |
|
✗ |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
| 304 |
|
|
} |
| 305 |
|
|
|
| 306 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_computeCentroidalMomentum_computeCentroidalMomentumTimeVariation) |
| 307 |
|
|
{ |
| 308 |
|
|
using namespace pinocchio; |
| 309 |
|
✗ |
Model model; |
| 310 |
|
✗ |
buildModels::humanoidRandom(model); |
| 311 |
|
✗ |
addJointAndBody(model, JointModelSpherical(), "larm6_joint", "larm7"); |
| 312 |
|
✗ |
Data data(model), data_fk1(model), data_fk2(model), data_ref(model); |
| 313 |
|
|
|
| 314 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
| 315 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
| 316 |
|
|
|
| 317 |
|
|
Eigen::VectorXd q = |
| 318 |
|
✗ |
randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit); |
| 319 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); |
| 320 |
|
✗ |
Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv); |
| 321 |
|
|
|
| 322 |
|
✗ |
ccrba(model, data_ref, q, v); |
| 323 |
|
✗ |
forwardKinematics(model, data_ref, q, v); |
| 324 |
|
✗ |
centerOfMass(model, data_ref, q, v, false); |
| 325 |
|
✗ |
computeCentroidalMomentum(model, data, q, v); |
| 326 |
|
|
|
| 327 |
|
✗ |
BOOST_CHECK(data.mass[0] == data_ref.mass[0]); |
| 328 |
|
✗ |
BOOST_CHECK(data.com[0].isApprox(data_ref.com[0])); |
| 329 |
|
✗ |
BOOST_CHECK(data.hg.isApprox(data_ref.hg)); |
| 330 |
|
✗ |
for (size_t k = 1; k < (size_t)model.njoints; ++k) |
| 331 |
|
|
{ |
| 332 |
|
✗ |
BOOST_CHECK(data.mass[k] == data_ref.mass[k]); |
| 333 |
|
✗ |
BOOST_CHECK(data.com[k].isApprox(data_ref.com[k])); |
| 334 |
|
✗ |
BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); |
| 335 |
|
|
} |
| 336 |
|
|
|
| 337 |
|
|
// Check other signature |
| 338 |
|
✗ |
forwardKinematics(model, data_fk1, q, v); |
| 339 |
|
✗ |
computeCentroidalMomentum(model, data_fk1); |
| 340 |
|
|
|
| 341 |
|
✗ |
BOOST_CHECK(data_fk1.hg.isApprox(data.hg)); |
| 342 |
|
|
|
| 343 |
|
✗ |
computeCentroidalMomentumTimeVariation(model, data, q, v, a); |
| 344 |
|
✗ |
model.gravity.setZero(); |
| 345 |
|
✗ |
rnea(model, data_ref, q, v, a); |
| 346 |
|
✗ |
dccrba(model, data_ref, q, v); |
| 347 |
|
✗ |
const Force hgdot(data_ref.Ag * a + data_ref.dAg * v); |
| 348 |
|
|
|
| 349 |
|
✗ |
BOOST_CHECK(data.mass[0] == data_ref.mass[0]); |
| 350 |
|
✗ |
BOOST_CHECK(data.com[0].isApprox(data_ref.com[0])); |
| 351 |
|
✗ |
BOOST_CHECK(data.hg.isApprox(data_ref.hg)); |
| 352 |
|
✗ |
BOOST_CHECK(data.dhg.isApprox(hgdot)); |
| 353 |
|
✗ |
for (size_t k = 1; k < (size_t)model.njoints; ++k) |
| 354 |
|
|
{ |
| 355 |
|
✗ |
BOOST_CHECK(data.mass[k] == data_ref.mass[k]); |
| 356 |
|
✗ |
BOOST_CHECK(data.com[k].isApprox(data_ref.com[k])); |
| 357 |
|
✗ |
BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); |
| 358 |
|
✗ |
BOOST_CHECK(data.a[k].isApprox(data_ref.a_gf[k])); |
| 359 |
|
✗ |
BOOST_CHECK(data.f[k].isApprox(data_ref.f[k])); |
| 360 |
|
|
} |
| 361 |
|
|
|
| 362 |
|
|
// Check other signature |
| 363 |
|
✗ |
forwardKinematics(model, data_fk2, q, v, a); |
| 364 |
|
✗ |
computeCentroidalMomentumTimeVariation(model, data_fk2); |
| 365 |
|
|
|
| 366 |
|
✗ |
BOOST_CHECK(data_fk2.hg.isApprox(data.hg)); |
| 367 |
|
✗ |
BOOST_CHECK(data_fk2.dhg.isApprox(data.dhg)); |
| 368 |
|
|
|
| 369 |
|
|
// Check against finite differences |
| 370 |
|
✗ |
Data data_fd(model); |
| 371 |
|
✗ |
const double eps = 1e-8; |
| 372 |
|
✗ |
Eigen::VectorXd v_plus = v + eps * a; |
| 373 |
|
✗ |
Eigen::VectorXd q_plus = integrate(model, q, eps * v); |
| 374 |
|
|
|
| 375 |
|
✗ |
const Force hg = computeCentroidalMomentum(model, data_fd, q, v); |
| 376 |
|
✗ |
const SE3::Vector3 com = data_fd.com[0]; |
| 377 |
|
✗ |
const Force hg_plus = computeCentroidalMomentum(model, data_fd, q_plus, v_plus); |
| 378 |
|
✗ |
const SE3::Vector3 com_plus = data_fd.com[0]; |
| 379 |
|
|
|
| 380 |
|
✗ |
SE3 transform(SE3::Identity()); |
| 381 |
|
✗ |
transform.translation() = com_plus - com; |
| 382 |
|
✗ |
Force dhg_ref = (transform.act(hg_plus) - hg) / eps; |
| 383 |
|
|
|
| 384 |
|
✗ |
BOOST_CHECK(data.dhg.isApprox(dhg_ref, sqrt(eps))); |
| 385 |
|
|
} |
| 386 |
|
|
|
| 387 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
| 388 |
|
|
|