| 1 |
/* |
| 2 |
* Copyright (c) 2011-2022, The DART development contributors |
| 3 |
* All rights reserved. |
| 4 |
* |
| 5 |
* The list of contributors can be found at: |
| 6 |
* https://github.com/dartsim/dart/blob/master/LICENSE |
| 7 |
* |
| 8 |
* This file is provided under the following "BSD-style" License: |
| 9 |
* Redistribution and use in source and binary forms, with or |
| 10 |
* without modification, are permitted provided that the following |
| 11 |
* conditions are met: |
| 12 |
* * Redistributions of source code must retain the above copyright |
| 13 |
* notice, this list of conditions and the following disclaimer. |
| 14 |
* * Redistributions in binary form must reproduce the above |
| 15 |
* copyright notice, this list of conditions and the following |
| 16 |
* disclaimer in the documentation and/or other materials provided |
| 17 |
* with the distribution. |
| 18 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND |
| 19 |
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, |
| 20 |
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF |
| 21 |
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
| 22 |
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR |
| 23 |
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
| 24 |
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
| 25 |
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF |
| 26 |
* USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
| 27 |
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 28 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 29 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 30 |
* POSSIBILITY OF SUCH DAMAGE. |
| 31 |
*/ |
| 32 |
|
| 33 |
#include <iostream> |
| 34 |
|
| 35 |
#include <Eigen/Dense> |
| 36 |
#include <gtest/gtest.h> |
| 37 |
|
| 38 |
#include "dart/common/Console.hpp" |
| 39 |
#include "dart/dynamics/BodyNode.hpp" |
| 40 |
#include "dart/dynamics/SimpleFrame.hpp" |
| 41 |
#include "dart/dynamics/Skeleton.hpp" |
| 42 |
#include "dart/math/Geometry.hpp" |
| 43 |
#include "dart/math/Helpers.hpp" |
| 44 |
#include "dart/math/Random.hpp" |
| 45 |
#include "dart/simulation/World.hpp" |
| 46 |
#include "dart/utils/SkelParser.hpp" |
| 47 |
|
| 48 |
#include "TestHelpers.hpp" |
| 49 |
|
| 50 |
using namespace Eigen; |
| 51 |
using namespace dart; |
| 52 |
|
| 53 |
//============================================================================== |
| 54 |
class DynamicsTest : public ::testing::Test |
| 55 |
{ |
| 56 |
public: |
| 57 |
// Get Skel file URI to test. |
| 58 |
const std::vector<common::Uri>& getList() const; |
| 59 |
|
| 60 |
// Get reference frames |
| 61 |
const std::vector<SimpleFrame*>& getRefFrames() const; |
| 62 |
|
| 63 |
// Randomize the properties of all the reference frames |
| 64 |
void randomizeRefFrames(); |
| 65 |
|
| 66 |
// Get mass matrix of _skel using Jacobians and inertias of each body |
| 67 |
// in _skel. |
| 68 |
MatrixXd getMassMatrix(dynamics::SkeletonPtr _skel); |
| 69 |
|
| 70 |
// Get augmented mass matrix of _skel using Jacobians and inertias of |
| 71 |
// each body in _skel. |
| 72 |
MatrixXd getAugMassMatrix(dynamics::SkeletonPtr _skel); |
| 73 |
|
| 74 |
// Compare velocities computed by recursive method, Jacobian, and finite |
| 75 |
// difference. |
| 76 |
void testJacobians(const common::Uri& uri); |
| 77 |
|
| 78 |
// Compare velocities and accelerations with actual vaules and approximates |
| 79 |
// using finite differece method. |
| 80 |
void testFiniteDifferenceGeneralizedCoordinates(const common::Uri& uri); |
| 81 |
|
| 82 |
// Compare spatial velocities computed by forward kinematics and finite |
| 83 |
// difference. |
| 84 |
void testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri); |
| 85 |
|
| 86 |
// Compare accelerations computed by recursive method, Jacobian, and finite |
| 87 |
// difference. |
| 88 |
void testFiniteDifferenceBodyNodeAcceleration(const common::Uri& uri); |
| 89 |
|
| 90 |
// Test if the recursive forward kinematics algorithm computes |
| 91 |
// transformations, spatial velocities, and spatial accelerations correctly. |
| 92 |
void testForwardKinematics(const common::Uri& uri); |
| 93 |
|
| 94 |
// Test inverse dynamics with various joint forces. |
| 95 |
void testInverseDynamics(const common::Uri& uri); |
| 96 |
|
| 97 |
// Compare dynamics terms in equations of motion such as mass matrix, mass |
| 98 |
// inverse matrix, Coriolis force vector, gravity force vector, and external |
| 99 |
// force vector. |
| 100 |
void compareEquationsOfMotion(const common::Uri& uri); |
| 101 |
|
| 102 |
// Test skeleton's COM and its related quantities. |
| 103 |
void testCenterOfMass(const common::Uri& uri); |
| 104 |
|
| 105 |
// Test if the com acceleration is equal to the gravity |
| 106 |
void testCenterOfMassFreeFall(const common::Uri& uri); |
| 107 |
|
| 108 |
// |
| 109 |
void testConstraintImpulse(const common::Uri& uri); |
| 110 |
|
| 111 |
// Test impulse based dynamics |
| 112 |
void testImpulseBasedDynamics(const common::Uri& uri); |
| 113 |
|
| 114 |
protected: |
| 115 |
// Sets up the test fixture. |
| 116 |
void SetUp() override; |
| 117 |
|
| 118 |
// Skel file list. |
| 119 |
std::vector<common::Uri> fileList; |
| 120 |
|
| 121 |
std::vector<SimpleFrame*> refFrames; |
| 122 |
}; |
| 123 |
|
| 124 |
//============================================================================== |
| 125 |
void DynamicsTest::SetUp() |
| 126 |
{ |
| 127 |
// Create a list of skel files to test with |
| 128 |
fileList.push_back("dart://sample/skel/test/chainwhipa.skel"); |
| 129 |
fileList.push_back("dart://sample/skel/test/single_pendulum.skel"); |
| 130 |
fileList.push_back( |
| 131 |
"dart://sample/skel/test/single_pendulum_euler_joint.skel"); |
| 132 |
fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel"); |
| 133 |
fileList.push_back("dart://sample/skel/test/double_pendulum.skel"); |
| 134 |
fileList.push_back( |
| 135 |
"dart://sample/skel/test/double_pendulum_euler_joint.skel"); |
| 136 |
fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel"); |
| 137 |
fileList.push_back( |
| 138 |
"dart://sample/skel/test/serial_chain_revolute_joint.skel"); |
| 139 |
fileList.push_back( |
| 140 |
"dart://sample/skel/test/serial_chain_eulerxyz_joint.skel"); |
| 141 |
fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel"); |
| 142 |
fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel"); |
| 143 |
fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel"); |
| 144 |
fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel"); |
| 145 |
fileList.push_back( |
| 146 |
"dart://sample/skel/test/simple_tree_structure_euler_joint.skel"); |
| 147 |
fileList.push_back( |
| 148 |
"dart://sample/skel/test/simple_tree_structure_ball_joint.skel"); |
| 149 |
fileList.push_back("dart://sample/skel/test/tree_structure.skel"); |
| 150 |
fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel"); |
| 151 |
fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel"); |
| 152 |
fileList.push_back("dart://sample/skel/fullbody1.skel"); |
| 153 |
|
| 154 |
// Create a list of reference frames to use during tests |
| 155 |
refFrames.push_back(new SimpleFrame(Frame::World(), "refFrame1")); |
| 156 |
refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame2")); |
| 157 |
refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame3")); |
| 158 |
refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame4")); |
| 159 |
refFrames.push_back(new SimpleFrame(Frame::World(), "refFrame5")); |
| 160 |
refFrames.push_back(new SimpleFrame(refFrames.back(), "refFrame6")); |
| 161 |
} |
| 162 |
|
| 163 |
//============================================================================== |
| 164 |
const std::vector<common::Uri>& DynamicsTest::getList() const |
| 165 |
{ |
| 166 |
return fileList; |
| 167 |
} |
| 168 |
|
| 169 |
//============================================================================== |
| 170 |
const std::vector<SimpleFrame*>& DynamicsTest::getRefFrames() const |
| 171 |
{ |
| 172 |
return refFrames; |
| 173 |
} |
| 174 |
|
| 175 |
//============================================================================== |
| 176 |
void DynamicsTest::randomizeRefFrames() |
| 177 |
{ |
| 178 |
for (std::size_t i = 0; i < refFrames.size(); ++i) |
| 179 |
{ |
| 180 |
SimpleFrame* F = refFrames[i]; |
| 181 |
|
| 182 |
Eigen::Vector3d p = Random::uniform<Eigen::Vector3d>(-100, 100); |
| 183 |
Eigen::Vector3d theta = Random::uniform<Eigen::Vector3d>( |
| 184 |
-2 * dart::math::constantsd::pi(), 2 * dart::math::constantsd::pi()); |
| 185 |
|
| 186 |
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); |
| 187 |
tf.translate(p); |
| 188 |
tf.linear() = math::eulerXYZToMatrix(theta); |
| 189 |
|
| 190 |
F->setRelativeTransform(tf); |
| 191 |
F->setRelativeSpatialVelocity(Random::uniform<Eigen::Vector6d>(-100, 100)); |
| 192 |
F->setRelativeSpatialAcceleration( |
| 193 |
Random::uniform<Eigen::Vector6d>(-100, 100)); |
| 194 |
} |
| 195 |
} |
| 196 |
|
| 197 |
//============================================================================== |
| 198 |
MatrixXd DynamicsTest::getMassMatrix(dynamics::SkeletonPtr _skel) |
| 199 |
{ |
| 200 |
int skelDof = _skel->getNumDofs(); |
| 201 |
|
| 202 |
MatrixXd skelM = MatrixXd::Zero(skelDof, skelDof); // Mass matrix of skeleton |
| 203 |
MatrixXd M; // Body mass |
| 204 |
MatrixXd I; // Body inertia |
| 205 |
MatrixXd J; // Body Jacobian |
| 206 |
|
| 207 |
for (std::size_t i = 0; i < _skel->getNumBodyNodes(); ++i) |
| 208 |
{ |
| 209 |
dynamics::BodyNode* body = _skel->getBodyNode(i); |
| 210 |
|
| 211 |
int dof = body->getNumDependentGenCoords(); |
| 212 |
I = body->getSpatialInertia(); |
| 213 |
J = body->getJacobian(); |
| 214 |
|
| 215 |
EXPECT_EQ(I.rows(), 6); |
| 216 |
EXPECT_EQ(I.cols(), 6); |
| 217 |
EXPECT_EQ(J.rows(), 6); |
| 218 |
EXPECT_EQ(J.cols(), dof); |
| 219 |
|
| 220 |
M = J.transpose() * I * J; // (dof x dof) matrix |
| 221 |
|
| 222 |
for (int j = 0; j < dof; ++j) |
| 223 |
{ |
| 224 |
int jIdx = body->getDependentGenCoordIndex(j); |
| 225 |
|
| 226 |
for (int k = 0; k < dof; ++k) |
| 227 |
{ |
| 228 |
int kIdx = body->getDependentGenCoordIndex(k); |
| 229 |
|
| 230 |
skelM(jIdx, kIdx) += M(j, k); |
| 231 |
} |
| 232 |
} |
| 233 |
} |
| 234 |
|
| 235 |
return skelM; |
| 236 |
} |
| 237 |
|
| 238 |
//============================================================================== |
| 239 |
MatrixXd DynamicsTest::getAugMassMatrix(dynamics::SkeletonPtr _skel) |
| 240 |
{ |
| 241 |
int dof = _skel->getNumDofs(); |
| 242 |
double dt = _skel->getTimeStep(); |
| 243 |
|
| 244 |
MatrixXd M = getMassMatrix(_skel); |
| 245 |
MatrixXd D = MatrixXd::Zero(dof, dof); |
| 246 |
MatrixXd K = MatrixXd::Zero(dof, dof); |
| 247 |
MatrixXd AugM; |
| 248 |
|
| 249 |
// Compute diagonal matrices of joint damping and joint stiffness |
| 250 |
for (std::size_t i = 0; i < _skel->getNumBodyNodes(); ++i) |
| 251 |
{ |
| 252 |
dynamics::BodyNode* body = _skel->getBodyNode(i); |
| 253 |
dynamics::Joint* joint = body->getParentJoint(); |
| 254 |
|
| 255 |
EXPECT_TRUE(body != nullptr); |
| 256 |
EXPECT_TRUE(joint != nullptr); |
| 257 |
|
| 258 |
int dof = joint->getNumDofs(); |
| 259 |
|
| 260 |
for (int j = 0; j < dof; ++j) |
| 261 |
{ |
| 262 |
int idx = joint->getIndexInSkeleton(j); |
| 263 |
|
| 264 |
D(idx, idx) = joint->getDampingCoefficient(j); |
| 265 |
K(idx, idx) = joint->getSpringStiffness(j); |
| 266 |
} |
| 267 |
} |
| 268 |
|
| 269 |
AugM = M + (dt * D) + (dt * dt * K); |
| 270 |
|
| 271 |
return AugM; |
| 272 |
} |
| 273 |
|
| 274 |
template <typename T> |
| 275 |
void printComparisonError( |
| 276 |
const std::string& _comparison, |
| 277 |
const std::string& _name, |
| 278 |
const std::string& _frame, |
| 279 |
const T& fk, |
| 280 |
const T& jac) |
| 281 |
{ |
| 282 |
std::cout << "Disagreement between FK and Jacobian results for " |
| 283 |
<< _comparison << " of '" << _name |
| 284 |
<< "' with a reference Frame of '" << _frame << "'\n" |
| 285 |
<< "FK: " << fk.transpose() << "\n" |
| 286 |
<< "Jac: " << jac.transpose() << "\n"; |
| 287 |
} |
| 288 |
|
| 289 |
//============================================================================== |
| 290 |
void compareBodyNodeFkToJacobian( |
| 291 |
const BodyNode* bn, const Frame* refFrame, double tolerance) |
| 292 |
{ |
| 293 |
using math::AngularJacobian; |
| 294 |
using math::Jacobian; |
| 295 |
using math::LinearJacobian; |
| 296 |
|
| 297 |
ConstSkeletonPtr skel = bn->getSkeleton(); |
| 298 |
|
| 299 |
VectorXd dq = skel->getVelocities(); |
| 300 |
VectorXd ddq = skel->getAccelerations(); |
| 301 |
|
| 302 |
const std::vector<std::size_t>& coords = bn->getDependentGenCoordIndices(); |
| 303 |
VectorXd dqSeg = skel->getVelocities(coords); |
| 304 |
VectorXd ddqSeg = skel->getAccelerations(coords); |
| 305 |
|
| 306 |
//-- Spatial Jacobian tests -------------------------------------------------- |
| 307 |
|
| 308 |
Vector6d SpatialVelFk = bn->getSpatialVelocity(Frame::World(), refFrame); |
| 309 |
Vector6d SpatialAccFk = bn->getSpatialAcceleration(Frame::World(), refFrame); |
| 310 |
|
| 311 |
Jacobian SpatialJacSeg = bn->getJacobian(refFrame); |
| 312 |
Jacobian SpatialJacDerivSeg = bn->getJacobianSpatialDeriv(refFrame); |
| 313 |
|
| 314 |
Vector6d SpatialVelJacSeg = SpatialJacSeg * dqSeg; |
| 315 |
Vector6d SpatialAccJacSeg |
| 316 |
= SpatialJacSeg * ddqSeg + SpatialJacDerivSeg * dqSeg; |
| 317 |
|
| 318 |
Jacobian SpatialJac = skel->getJacobian(bn, refFrame); |
| 319 |
Jacobian SpatialJacDeriv = skel->getJacobianSpatialDeriv(bn, refFrame); |
| 320 |
|
| 321 |
Vector6d SpatialVelJac = SpatialJac * dq; |
| 322 |
Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq; |
| 323 |
|
| 324 |
bool spatialVelSegEqual = equals(SpatialVelFk, SpatialVelJacSeg, tolerance); |
| 325 |
bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance); |
| 326 |
EXPECT_TRUE(spatialVelSegEqual); |
| 327 |
EXPECT_TRUE(spatialVelEqual); |
| 328 |
if (!spatialVelSegEqual) |
| 329 |
printComparisonError( |
| 330 |
"spatial velocity (seg)", |
| 331 |
bn->getName(), |
| 332 |
refFrame->getName(), |
| 333 |
SpatialVelFk, |
| 334 |
SpatialVelJacSeg); |
| 335 |
if (!spatialVelEqual) |
| 336 |
printComparisonError( |
| 337 |
"spatial velocity", |
| 338 |
bn->getName(), |
| 339 |
refFrame->getName(), |
| 340 |
SpatialVelFk, |
| 341 |
SpatialVelJac); |
| 342 |
|
| 343 |
bool spatialAccSegEqual = equals(SpatialAccFk, SpatialAccJacSeg, tolerance); |
| 344 |
bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance); |
| 345 |
EXPECT_TRUE(spatialAccSegEqual); |
| 346 |
EXPECT_TRUE(spatialAccEqual); |
| 347 |
if (!spatialAccSegEqual) |
| 348 |
printComparisonError( |
| 349 |
"spatial acceleration (seg)", |
| 350 |
bn->getName(), |
| 351 |
refFrame->getName(), |
| 352 |
SpatialAccFk, |
| 353 |
SpatialAccJacSeg); |
| 354 |
if (!spatialAccEqual) |
| 355 |
printComparisonError( |
| 356 |
"spatial acceleration", |
| 357 |
bn->getName(), |
| 358 |
refFrame->getName(), |
| 359 |
SpatialAccFk, |
| 360 |
SpatialAccJac); |
| 361 |
|
| 362 |
//-- Linear Jacobian tests --------------------------------------------------- |
| 363 |
|
| 364 |
Vector3d LinearVelFk = bn->getLinearVelocity(Frame::World(), refFrame); |
| 365 |
Vector3d LinearAccFk = bn->getLinearAcceleration(Frame::World(), refFrame); |
| 366 |
|
| 367 |
LinearJacobian LinearJacSeg = bn->getLinearJacobian(refFrame); |
| 368 |
LinearJacobian LinearJacDerivSeg = bn->getLinearJacobianDeriv(refFrame); |
| 369 |
|
| 370 |
Vector3d LinearVelJacSeg = LinearJacSeg * dqSeg; |
| 371 |
Vector3d LinearAccJacSeg = LinearJacSeg * ddqSeg + LinearJacDerivSeg * dqSeg; |
| 372 |
|
| 373 |
LinearJacobian LinearJac = skel->getLinearJacobian(bn, refFrame); |
| 374 |
LinearJacobian LinearJacDeriv = skel->getLinearJacobianDeriv(bn, refFrame); |
| 375 |
|
| 376 |
Vector3d LinearVelJac = LinearJac * dq; |
| 377 |
Vector3d LinearAccJac = LinearJac * ddq + LinearJacDeriv * dq; |
| 378 |
|
| 379 |
bool linearVelSegEqual = equals(LinearVelFk, LinearVelJacSeg, tolerance); |
| 380 |
bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance); |
| 381 |
EXPECT_TRUE(linearVelSegEqual); |
| 382 |
EXPECT_TRUE(linearVelEqual); |
| 383 |
if (!linearVelSegEqual) |
| 384 |
printComparisonError( |
| 385 |
"linear velocity (seg)", |
| 386 |
bn->getName(), |
| 387 |
refFrame->getName(), |
| 388 |
LinearVelFk, |
| 389 |
LinearVelJacSeg); |
| 390 |
if (!linearVelEqual) |
| 391 |
printComparisonError( |
| 392 |
"linear velocity", |
| 393 |
bn->getName(), |
| 394 |
refFrame->getName(), |
| 395 |
LinearVelFk, |
| 396 |
LinearVelJac); |
| 397 |
|
| 398 |
bool linearAccSegEqual = equals(LinearAccFk, LinearAccJacSeg, tolerance); |
| 399 |
bool linearAccEqual = equals(LinearAccFk, LinearAccJac, tolerance); |
| 400 |
EXPECT_TRUE(linearAccSegEqual); |
| 401 |
EXPECT_TRUE(linearAccEqual); |
| 402 |
if (!linearAccSegEqual) |
| 403 |
printComparisonError( |
| 404 |
"linear acceleration (seg)", |
| 405 |
bn->getName(), |
| 406 |
refFrame->getName(), |
| 407 |
LinearAccFk, |
| 408 |
LinearAccJacSeg); |
| 409 |
if (!linearAccEqual) |
| 410 |
printComparisonError( |
| 411 |
"linear acceleration", |
| 412 |
bn->getName(), |
| 413 |
refFrame->getName(), |
| 414 |
LinearAccFk, |
| 415 |
LinearAccJac); |
| 416 |
|
| 417 |
//-- Angular Jacobian tests |
| 418 |
|
| 419 |
Vector3d AngularVelFk = bn->getAngularVelocity(Frame::World(), refFrame); |
| 420 |
Vector3d AngularAccFk = bn->getAngularAcceleration(Frame::World(), refFrame); |
| 421 |
|
| 422 |
AngularJacobian AngularJacSeg = bn->getAngularJacobian(refFrame); |
| 423 |
AngularJacobian AngularJacDerivSeg = bn->getAngularJacobianDeriv(refFrame); |
| 424 |
|
| 425 |
Vector3d AngularVelJacSeg = AngularJacSeg * dqSeg; |
| 426 |
Vector3d AngularAccJacSeg |
| 427 |
= AngularJacSeg * ddqSeg + AngularJacDerivSeg * dqSeg; |
| 428 |
|
| 429 |
AngularJacobian AngularJac = skel->getAngularJacobian(bn, refFrame); |
| 430 |
AngularJacobian AngularJacDeriv = skel->getAngularJacobianDeriv(bn, refFrame); |
| 431 |
|
| 432 |
Vector3d AngularVelJac = AngularJac * dq; |
| 433 |
Vector3d AngularAccJac = AngularJac * ddq + AngularJacDeriv * dq; |
| 434 |
|
| 435 |
bool angularVelSegEqual = equals(AngularVelFk, AngularVelJacSeg, tolerance); |
| 436 |
bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance); |
| 437 |
EXPECT_TRUE(angularVelSegEqual); |
| 438 |
EXPECT_TRUE(angularVelEqual); |
| 439 |
if (!angularVelSegEqual) |
| 440 |
printComparisonError( |
| 441 |
"angular velocity (seg)", |
| 442 |
bn->getName(), |
| 443 |
refFrame->getName(), |
| 444 |
AngularVelFk, |
| 445 |
AngularVelJacSeg); |
| 446 |
if (!angularVelEqual) |
| 447 |
printComparisonError( |
| 448 |
"angular velocity", |
| 449 |
bn->getName(), |
| 450 |
refFrame->getName(), |
| 451 |
AngularVelFk, |
| 452 |
AngularVelJac); |
| 453 |
|
| 454 |
bool angularAccSegEqual = equals(AngularAccFk, AngularAccJacSeg, tolerance); |
| 455 |
bool angularAccEqual = equals(AngularAccFk, AngularAccJac, tolerance); |
| 456 |
EXPECT_TRUE(angularAccSegEqual); |
| 457 |
EXPECT_TRUE(angularAccEqual); |
| 458 |
if (!angularAccSegEqual) |
| 459 |
printComparisonError( |
| 460 |
"angular acceleration (seg)", |
| 461 |
bn->getName(), |
| 462 |
refFrame->getName(), |
| 463 |
AngularAccFk, |
| 464 |
AngularAccJacSeg); |
| 465 |
if (!angularAccEqual) |
| 466 |
printComparisonError( |
| 467 |
"angular acceleration", |
| 468 |
bn->getName(), |
| 469 |
refFrame->getName(), |
| 470 |
AngularAccFk, |
| 471 |
AngularAccJac); |
| 472 |
} |
| 473 |
|
| 474 |
//============================================================================== |
| 475 |
void compareBodyNodeFkToJacobian( |
| 476 |
const BodyNode* bn, |
| 477 |
const Frame* refFrame, |
| 478 |
const Eigen::Vector3d& offset, |
| 479 |
double tolerance) |
| 480 |
{ |
| 481 |
using math::AngularJacobian; |
| 482 |
using math::Jacobian; |
| 483 |
using math::LinearJacobian; |
| 484 |
|
| 485 |
ConstSkeletonPtr skel = bn->getSkeleton(); |
| 486 |
|
| 487 |
VectorXd dq = skel->getVelocities(); |
| 488 |
VectorXd ddq = skel->getAccelerations(); |
| 489 |
|
| 490 |
const std::vector<std::size_t>& coords = bn->getDependentGenCoordIndices(); |
| 491 |
VectorXd dqSeg = skel->getVelocities(coords); |
| 492 |
VectorXd ddqSeg = skel->getAccelerations(coords); |
| 493 |
|
| 494 |
//-- Spatial Jacobian tests -------------------------------------------------- |
| 495 |
|
| 496 |
Vector6d SpatialVelFk |
| 497 |
= bn->getSpatialVelocity(offset, Frame::World(), refFrame); |
| 498 |
Vector6d SpatialAccFk |
| 499 |
= bn->getSpatialAcceleration(offset, Frame::World(), refFrame); |
| 500 |
|
| 501 |
Jacobian SpatialJacSeg = bn->getJacobian(offset, refFrame); |
| 502 |
Jacobian SpatialJacDerivSeg = bn->getJacobianSpatialDeriv(offset, refFrame); |
| 503 |
|
| 504 |
Vector6d SpatialVelJacSeg = SpatialJacSeg * dqSeg; |
| 505 |
Vector6d SpatialAccJacSeg |
| 506 |
= SpatialJacSeg * ddqSeg + SpatialJacDerivSeg * dqSeg; |
| 507 |
|
| 508 |
Jacobian SpatialJac = skel->getJacobian(bn, offset, refFrame); |
| 509 |
Jacobian SpatialJacDeriv |
| 510 |
= skel->getJacobianSpatialDeriv(bn, offset, refFrame); |
| 511 |
|
| 512 |
Vector6d SpatialVelJac = SpatialJac * dq; |
| 513 |
Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq; |
| 514 |
|
| 515 |
bool spatialVelSegEqual = equals(SpatialVelFk, SpatialVelJacSeg, tolerance); |
| 516 |
bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance); |
| 517 |
EXPECT_TRUE(spatialVelSegEqual); |
| 518 |
EXPECT_TRUE(spatialVelEqual); |
| 519 |
if (!spatialVelSegEqual) |
| 520 |
printComparisonError( |
| 521 |
"spatial velocity w/ offset (seg)", |
| 522 |
bn->getName(), |
| 523 |
refFrame->getName(), |
| 524 |
SpatialVelFk, |
| 525 |
SpatialVelJacSeg); |
| 526 |
if (!spatialVelEqual) |
| 527 |
printComparisonError( |
| 528 |
"spatial velocity w/ offset", |
| 529 |
bn->getName(), |
| 530 |
refFrame->getName(), |
| 531 |
SpatialVelFk, |
| 532 |
SpatialVelJac); |
| 533 |
|
| 534 |
bool spatialAccSegEqual = equals(SpatialAccFk, SpatialAccJacSeg, tolerance); |
| 535 |
bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance); |
| 536 |
EXPECT_TRUE(spatialAccSegEqual); |
| 537 |
EXPECT_TRUE(spatialAccEqual); |
| 538 |
if (!spatialAccSegEqual) |
| 539 |
printComparisonError( |
| 540 |
"spatial acceleration w/ offset (seg)", |
| 541 |
bn->getName(), |
| 542 |
refFrame->getName(), |
| 543 |
SpatialAccFk, |
| 544 |
SpatialAccJacSeg); |
| 545 |
if (!spatialAccEqual) |
| 546 |
printComparisonError( |
| 547 |
"spatial acceleration w/ offset", |
| 548 |
bn->getName(), |
| 549 |
refFrame->getName(), |
| 550 |
SpatialAccFk, |
| 551 |
SpatialAccJac); |
| 552 |
|
| 553 |
//-- Linear Jacobian tests --------------------------------------------------- |
| 554 |
|
| 555 |
Vector3d LinearVelFk |
| 556 |
= bn->getLinearVelocity(offset, Frame::World(), refFrame); |
| 557 |
Vector3d LinearAccFk |
| 558 |
= bn->getLinearAcceleration(offset, Frame::World(), refFrame); |
| 559 |
|
| 560 |
LinearJacobian LinearJacSeg = bn->getLinearJacobian(offset, refFrame); |
| 561 |
LinearJacobian LinearJacDerivSeg |
| 562 |
= bn->getLinearJacobianDeriv(offset, refFrame); |
| 563 |
|
| 564 |
Vector3d LinearVelJacSeg = LinearJacSeg * dqSeg; |
| 565 |
Vector3d LinearAccJacSeg = LinearJacSeg * ddqSeg + LinearJacDerivSeg * dqSeg; |
| 566 |
|
| 567 |
LinearJacobian LinearJac = skel->getLinearJacobian(bn, offset, refFrame); |
| 568 |
LinearJacobian LinearJacDeriv |
| 569 |
= skel->getLinearJacobianDeriv(bn, offset, refFrame); |
| 570 |
|
| 571 |
Vector3d LinearVelJac = LinearJac * dq; |
| 572 |
Vector3d LinearAccJac = LinearJac * ddq + LinearJacDeriv * dq; |
| 573 |
|
| 574 |
bool linearVelSegEqual = equals(LinearVelFk, LinearVelJacSeg, tolerance); |
| 575 |
bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance); |
| 576 |
EXPECT_TRUE(linearVelSegEqual); |
| 577 |
EXPECT_TRUE(linearVelEqual); |
| 578 |
if (!linearVelSegEqual) |
| 579 |
printComparisonError( |
| 580 |
"linear velocity w/ offset (seg)", |
| 581 |
bn->getName(), |
| 582 |
refFrame->getName(), |
| 583 |
LinearVelFk, |
| 584 |
LinearVelJacSeg); |
| 585 |
if (!linearVelEqual) |
| 586 |
printComparisonError( |
| 587 |
"linear velocity w/ offset", |
| 588 |
bn->getName(), |
| 589 |
refFrame->getName(), |
| 590 |
LinearVelFk, |
| 591 |
LinearVelJac); |
| 592 |
|
| 593 |
bool linearAccSegEqual = equals(LinearAccFk, LinearAccJacSeg, tolerance); |
| 594 |
bool linearAccEqual = equals(LinearAccFk, LinearAccJac, tolerance); |
| 595 |
EXPECT_TRUE(linearAccSegEqual); |
| 596 |
EXPECT_TRUE(linearAccEqual); |
| 597 |
if (!linearAccSegEqual) |
| 598 |
printComparisonError( |
| 599 |
"linear acceleration w/ offset (seg)", |
| 600 |
bn->getName(), |
| 601 |
refFrame->getName(), |
| 602 |
LinearAccFk, |
| 603 |
LinearAccJacSeg); |
| 604 |
if (!linearAccEqual) |
| 605 |
printComparisonError( |
| 606 |
"linear acceleration w/ offset", |
| 607 |
bn->getName(), |
| 608 |
refFrame->getName(), |
| 609 |
LinearAccFk, |
| 610 |
LinearAccJac); |
| 611 |
|
| 612 |
//-- Angular Jacobian tests -------------------------------------------------- |
| 613 |
|
| 614 |
Vector3d AngularVelFk = bn->getAngularVelocity(Frame::World(), refFrame); |
| 615 |
Vector3d AngularAccFk = bn->getAngularAcceleration(Frame::World(), refFrame); |
| 616 |
|
| 617 |
AngularJacobian AngularJacSeg = bn->getAngularJacobian(refFrame); |
| 618 |
AngularJacobian AngularJacDerivSeg = bn->getAngularJacobianDeriv(refFrame); |
| 619 |
|
| 620 |
Vector3d AngularVelJacSeg = AngularJacSeg * dqSeg; |
| 621 |
Vector3d AngularAccJacSeg |
| 622 |
= AngularJacSeg * ddqSeg + AngularJacDerivSeg * dqSeg; |
| 623 |
|
| 624 |
AngularJacobian AngularJac = skel->getAngularJacobian(bn, refFrame); |
| 625 |
AngularJacobian AngularJacDeriv = skel->getAngularJacobianDeriv(bn, refFrame); |
| 626 |
|
| 627 |
Vector3d AngularVelJac = AngularJac * dq; |
| 628 |
Vector3d AngularAccJac = AngularJac * ddq + AngularJacDeriv * dq; |
| 629 |
|
| 630 |
bool angularVelSegEqual = equals(AngularVelFk, AngularVelJacSeg, tolerance); |
| 631 |
bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance); |
| 632 |
EXPECT_TRUE(angularVelSegEqual); |
| 633 |
EXPECT_TRUE(angularVelEqual); |
| 634 |
if (!angularVelSegEqual) |
| 635 |
printComparisonError( |
| 636 |
"angular velocity w/ offset (seg)", |
| 637 |
bn->getName(), |
| 638 |
refFrame->getName(), |
| 639 |
AngularVelFk, |
| 640 |
AngularVelJacSeg); |
| 641 |
if (!angularVelEqual) |
| 642 |
printComparisonError( |
| 643 |
"angular velocity w/ offset", |
| 644 |
bn->getName(), |
| 645 |
refFrame->getName(), |
| 646 |
AngularVelFk, |
| 647 |
AngularVelJac); |
| 648 |
|
| 649 |
bool angularAccSegEqual = equals(AngularAccFk, AngularAccJacSeg, tolerance); |
| 650 |
bool angularAccEqual = equals(AngularAccFk, AngularAccJac, tolerance); |
| 651 |
EXPECT_TRUE(angularAccSegEqual); |
| 652 |
if (!angularAccSegEqual) |
| 653 |
printComparisonError( |
| 654 |
"angular acceleration w/ offset (seg)", |
| 655 |
bn->getName(), |
| 656 |
refFrame->getName(), |
| 657 |
AngularAccFk, |
| 658 |
AngularAccJacSeg); |
| 659 |
EXPECT_TRUE(angularAccEqual); |
| 660 |
if (!angularAccEqual) |
| 661 |
printComparisonError( |
| 662 |
"angular acceleration w/ offset", |
| 663 |
bn->getName(), |
| 664 |
refFrame->getName(), |
| 665 |
AngularAccFk, |
| 666 |
AngularAccJac); |
| 667 |
} |
| 668 |
|
| 669 |
//============================================================================== |
| 670 |
template <typename T> |
| 671 |
void printComparisonError( |
| 672 |
const std::string& _comparison, |
| 673 |
const std::string& _nameBN, |
| 674 |
const std::string& _nameBNRelativeTo, |
| 675 |
const std::string& _frame, |
| 676 |
const T& fk, |
| 677 |
const T& jac) |
| 678 |
{ |
| 679 |
std::cout << "Disagreement between FK and relative Jacobian results for " |
| 680 |
<< _comparison << " of '" << _nameBN << "' relative to '" |
| 681 |
<< _nameBNRelativeTo << "' with a reference Frame of '" << _frame |
| 682 |
<< "'\n" |
| 683 |
<< "FK: " << fk.transpose() << "\n" |
| 684 |
<< "Jac: " << jac.transpose() << "\n"; |
| 685 |
} |
| 686 |
|
| 687 |
//============================================================================== |
| 688 |
void compareBodyNodeFkToJacobianRelative( |
| 689 |
const JacobianNode* bn, |
| 690 |
const JacobianNode* relativeTo, |
| 691 |
const Frame* refFrame, |
| 692 |
double tolerance) |
| 693 |
{ |
| 694 |
using math::AngularJacobian; |
| 695 |
using math::Jacobian; |
| 696 |
using math::LinearJacobian; |
| 697 |
|
| 698 |
assert(bn->getSkeleton() == relativeTo->getSkeleton()); |
| 699 |
auto skel = bn->getSkeleton(); |
| 700 |
|
| 701 |
VectorXd dq = skel->getVelocities(); |
| 702 |
VectorXd ddq = skel->getAccelerations(); |
| 703 |
|
| 704 |
//-- Spatial Jacobian tests -------------------------------------------------- |
| 705 |
|
| 706 |
Vector6d SpatialVelFk = bn->getSpatialVelocity(relativeTo, refFrame); |
| 707 |
Vector6d SpatialAccFk = bn->getSpatialAcceleration(relativeTo, refFrame); |
| 708 |
|
| 709 |
Jacobian SpatialJac = skel->getJacobian(bn, relativeTo, refFrame); |
| 710 |
Jacobian SpatialJacDeriv |
| 711 |
= skel->getJacobianSpatialDeriv(bn, relativeTo, refFrame); |
| 712 |
|
| 713 |
Vector6d SpatialVelJac = SpatialJac * dq; |
| 714 |
Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq; |
| 715 |
|
| 716 |
bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance); |
| 717 |
EXPECT_TRUE(spatialVelEqual); |
| 718 |
if (!spatialVelEqual) |
| 719 |
{ |
| 720 |
printComparisonError( |
| 721 |
"spatial velocity", |
| 722 |
bn->getName(), |
| 723 |
relativeTo->getName(), |
| 724 |
refFrame->getName(), |
| 725 |
SpatialVelFk, |
| 726 |
SpatialVelJac); |
| 727 |
} |
| 728 |
|
| 729 |
bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance); |
| 730 |
EXPECT_TRUE(spatialAccEqual); |
| 731 |
if (!spatialAccEqual) |
| 732 |
{ |
| 733 |
printComparisonError( |
| 734 |
"spatial acceleration", |
| 735 |
bn->getName(), |
| 736 |
relativeTo->getName(), |
| 737 |
refFrame->getName(), |
| 738 |
SpatialAccFk, |
| 739 |
SpatialAccJac); |
| 740 |
} |
| 741 |
|
| 742 |
//-- Linear Jacobian tests -------------------------------------------------- |
| 743 |
|
| 744 |
Vector3d LinearVelFk = bn->getLinearVelocity(relativeTo, refFrame); |
| 745 |
|
| 746 |
LinearJacobian LinearJac = skel->getLinearJacobian(bn, relativeTo, refFrame); |
| 747 |
|
| 748 |
Vector3d LinearVelJac = LinearJac * dq; |
| 749 |
|
| 750 |
bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance); |
| 751 |
EXPECT_TRUE(linearVelEqual); |
| 752 |
if (!linearVelEqual) |
| 753 |
{ |
| 754 |
printComparisonError( |
| 755 |
"linear velocity", |
| 756 |
bn->getName(), |
| 757 |
relativeTo->getName(), |
| 758 |
refFrame->getName(), |
| 759 |
LinearVelFk, |
| 760 |
LinearVelJac); |
| 761 |
} |
| 762 |
|
| 763 |
//-- Angular Jacobian tests -------------------------------------------------- |
| 764 |
|
| 765 |
Vector3d AngularVelFk = bn->getAngularVelocity(relativeTo, refFrame); |
| 766 |
|
| 767 |
AngularJacobian AngularJac |
| 768 |
= skel->getAngularJacobian(bn, relativeTo, refFrame); |
| 769 |
|
| 770 |
Vector3d AngularVelJac = AngularJac * dq; |
| 771 |
|
| 772 |
bool angularVelEqual = equals(AngularVelFk, AngularVelJac, tolerance); |
| 773 |
EXPECT_TRUE(angularVelEqual); |
| 774 |
if (!angularVelEqual) |
| 775 |
{ |
| 776 |
printComparisonError( |
| 777 |
"angular velocity", |
| 778 |
bn->getName(), |
| 779 |
relativeTo->getName(), |
| 780 |
refFrame->getName(), |
| 781 |
AngularVelFk, |
| 782 |
AngularVelJac); |
| 783 |
} |
| 784 |
} |
| 785 |
|
| 786 |
//============================================================================== |
| 787 |
void compareBodyNodeFkToJacobianRelative( |
| 788 |
const JacobianNode* bn, |
| 789 |
const Eigen::Vector3d& _offset, |
| 790 |
const JacobianNode* relativeTo, |
| 791 |
const Frame* refFrame, |
| 792 |
double tolerance) |
| 793 |
{ |
| 794 |
using math::AngularJacobian; |
| 795 |
using math::Jacobian; |
| 796 |
using math::LinearJacobian; |
| 797 |
|
| 798 |
assert(bn->getSkeleton() == relativeTo->getSkeleton()); |
| 799 |
auto skel = bn->getSkeleton(); |
| 800 |
|
| 801 |
VectorXd dq = skel->getVelocities(); |
| 802 |
VectorXd ddq = skel->getAccelerations(); |
| 803 |
|
| 804 |
//-- Spatial Jacobian tests -------------------------------------------------- |
| 805 |
|
| 806 |
Vector6d SpatialVelFk = bn->getSpatialVelocity(_offset, relativeTo, refFrame); |
| 807 |
Vector6d SpatialAccFk |
| 808 |
= bn->getSpatialAcceleration(_offset, relativeTo, refFrame); |
| 809 |
|
| 810 |
Jacobian SpatialJac = skel->getJacobian(bn, _offset, relativeTo, refFrame); |
| 811 |
Jacobian SpatialJacDeriv |
| 812 |
= skel->getJacobianSpatialDeriv(bn, _offset, relativeTo, refFrame); |
| 813 |
|
| 814 |
Vector6d SpatialVelJac = SpatialJac * dq; |
| 815 |
Vector6d SpatialAccJac = SpatialJac * ddq + SpatialJacDeriv * dq; |
| 816 |
|
| 817 |
bool spatialVelEqual = equals(SpatialVelFk, SpatialVelJac, tolerance); |
| 818 |
EXPECT_TRUE(spatialVelEqual); |
| 819 |
if (!spatialVelEqual) |
| 820 |
{ |
| 821 |
printComparisonError( |
| 822 |
"spatial velocity w/ offset", |
| 823 |
bn->getName(), |
| 824 |
relativeTo->getName(), |
| 825 |
refFrame->getName(), |
| 826 |
SpatialVelFk, |
| 827 |
SpatialVelJac); |
| 828 |
} |
| 829 |
|
| 830 |
bool spatialAccEqual = equals(SpatialAccFk, SpatialAccJac, tolerance); |
| 831 |
EXPECT_TRUE(spatialAccEqual); |
| 832 |
if (!spatialAccEqual) |
| 833 |
{ |
| 834 |
printComparisonError( |
| 835 |
"spatial acceleration w/ offset", |
| 836 |
bn->getName(), |
| 837 |
relativeTo->getName(), |
| 838 |
refFrame->getName(), |
| 839 |
SpatialAccFk, |
| 840 |
SpatialAccJac); |
| 841 |
} |
| 842 |
|
| 843 |
//-- Linear Jacobian tests -------------------------------------------------- |
| 844 |
|
| 845 |
Vector3d LinearVelFk = bn->getLinearVelocity(_offset, relativeTo, refFrame); |
| 846 |
|
| 847 |
LinearJacobian LinearJac |
| 848 |
= skel->getLinearJacobian(bn, _offset, relativeTo, refFrame); |
| 849 |
|
| 850 |
Vector3d LinearVelJac = LinearJac * dq; |
| 851 |
|
| 852 |
bool linearVelEqual = equals(LinearVelFk, LinearVelJac, tolerance); |
| 853 |
EXPECT_TRUE(linearVelEqual); |
| 854 |
if (!linearVelEqual) |
| 855 |
{ |
| 856 |
printComparisonError( |
| 857 |
"linear velocity w/ offset", |
| 858 |
bn->getName(), |
| 859 |
relativeTo->getName(), |
| 860 |
refFrame->getName(), |
| 861 |
LinearVelFk, |
| 862 |
LinearVelJac); |
| 863 |
} |
| 864 |
} |
| 865 |
|
| 866 |
//============================================================================== |
| 867 |
void DynamicsTest::testJacobians(const common::Uri& uri) |
| 868 |
{ |
| 869 |
using namespace std; |
| 870 |
using namespace Eigen; |
| 871 |
using namespace dart; |
| 872 |
using namespace math; |
| 873 |
using namespace dynamics; |
| 874 |
using namespace simulation; |
| 875 |
using namespace utils; |
| 876 |
|
| 877 |
//----------------------------- Settings ------------------------------------- |
| 878 |
const double TOLERANCE = 1.0e-6; |
| 879 |
#ifndef NDEBUG // Debug mode |
| 880 |
int nTestItr = 2; |
| 881 |
#else |
| 882 |
int nTestItr = 5; |
| 883 |
#endif |
| 884 |
double qLB = -0.5 * constantsd::pi(); |
| 885 |
double qUB = 0.5 * constantsd::pi(); |
| 886 |
double dqLB = -0.5 * constantsd::pi(); |
| 887 |
double dqUB = 0.5 * constantsd::pi(); |
| 888 |
double ddqLB = -0.5 * constantsd::pi(); |
| 889 |
double ddqUB = 0.5 * constantsd::pi(); |
| 890 |
Vector3d gravity(0.0, -9.81, 0.0); |
| 891 |
|
| 892 |
// load skeleton |
| 893 |
WorldPtr world = SkelParser::readWorld(uri); |
| 894 |
assert(world != nullptr); |
| 895 |
world->setGravity(gravity); |
| 896 |
|
| 897 |
//------------------------------ Tests --------------------------------------- |
| 898 |
for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) |
| 899 |
{ |
| 900 |
SkeletonPtr skeleton = world->getSkeleton(i); |
| 901 |
assert(skeleton != nullptr); |
| 902 |
int dof = skeleton->getNumDofs(); |
| 903 |
|
| 904 |
for (int j = 0; j < nTestItr; ++j) |
| 905 |
{ |
| 906 |
// For the second half of the tests, scramble up the Skeleton |
| 907 |
if (j > std::ceil(nTestItr / 2)) |
| 908 |
{ |
| 909 |
SkeletonPtr copy = skeleton->cloneSkeleton(); |
| 910 |
std::size_t maxNode = skeleton->getNumBodyNodes() - 1; |
| 911 |
BodyNode* bn1 = skeleton->getBodyNode( |
| 912 |
math::Random::uniform<std::size_t>(0, maxNode)); |
| 913 |
BodyNode* bn2 = skeleton->getBodyNode( |
| 914 |
math::Random::uniform<std::size_t>(0, maxNode)); |
| 915 |
|
| 916 |
if (bn1 != bn2) |
| 917 |
{ |
| 918 |
BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2; |
| 919 |
BodyNode* parent = child == bn1 ? bn2 : bn1; |
| 920 |
|
| 921 |
child->moveTo(parent); |
| 922 |
} |
| 923 |
|
| 924 |
EXPECT_TRUE(skeleton->getNumBodyNodes() == copy->getNumBodyNodes()); |
| 925 |
EXPECT_TRUE(skeleton->getNumDofs() == copy->getNumDofs()); |
| 926 |
} |
| 927 |
|
| 928 |
// Generate a random state |
| 929 |
VectorXd q = VectorXd(dof); |
| 930 |
VectorXd dq = VectorXd(dof); |
| 931 |
VectorXd ddq = VectorXd(dof); |
| 932 |
for (int k = 0; k < dof; ++k) |
| 933 |
{ |
| 934 |
q[k] = math::Random::uniform(qLB, qUB); |
| 935 |
dq[k] = math::Random::uniform(dqLB, dqUB); |
| 936 |
ddq[k] = math::Random::uniform(ddqLB, ddqUB); |
| 937 |
} |
| 938 |
skeleton->setPositions(q); |
| 939 |
skeleton->setVelocities(dq); |
| 940 |
skeleton->setAccelerations(ddq); |
| 941 |
|
| 942 |
randomizeRefFrames(); |
| 943 |
|
| 944 |
// For each body node |
| 945 |
for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k) |
| 946 |
{ |
| 947 |
const BodyNode* bn = skeleton->getBodyNode(k); |
| 948 |
|
| 949 |
// Compare results using the World reference Frame |
| 950 |
compareBodyNodeFkToJacobian(bn, Frame::World(), TOLERANCE); |
| 951 |
compareBodyNodeFkToJacobian( |
| 952 |
bn, Frame::World(), bn->getLocalCOM(), TOLERANCE); |
| 953 |
compareBodyNodeFkToJacobian( |
| 954 |
bn, |
| 955 |
Frame::World(), |
| 956 |
Random::uniform<Eigen::Vector3d>(-10, 10), |
| 957 |
TOLERANCE); |
| 958 |
|
| 959 |
// Compare results using this BodyNode's own reference Frame |
| 960 |
compareBodyNodeFkToJacobian(bn, bn, TOLERANCE); |
| 961 |
compareBodyNodeFkToJacobian(bn, bn, bn->getLocalCOM(), TOLERANCE); |
| 962 |
compareBodyNodeFkToJacobian( |
| 963 |
bn, bn, Random::uniform<Eigen::Vector3d>(-10, 10), TOLERANCE); |
| 964 |
|
| 965 |
// Compare results using the randomized reference Frames |
| 966 |
for (std::size_t r = 0; r < refFrames.size(); ++r) |
| 967 |
{ |
| 968 |
compareBodyNodeFkToJacobian(bn, refFrames[r], TOLERANCE); |
| 969 |
compareBodyNodeFkToJacobian( |
| 970 |
bn, refFrames[r], bn->getLocalCOM(), TOLERANCE); |
| 971 |
compareBodyNodeFkToJacobian( |
| 972 |
bn, |
| 973 |
refFrames[r], |
| 974 |
Random::uniform<Eigen::Vector3d>(-10, 10), |
| 975 |
TOLERANCE); |
| 976 |
} |
| 977 |
|
| 978 |
// -- Relative Jacobian tests |
| 979 |
|
| 980 |
compareBodyNodeFkToJacobianRelative(bn, bn, Frame::World(), TOLERANCE); |
| 981 |
|
| 982 |
#ifndef NDEBUG // Debug mode |
| 983 |
if (skeleton->getNumBodyNodes() == 0u) |
| 984 |
continue; |
| 985 |
|
| 986 |
for (std::size_t l = skeleton->getNumBodyNodes() - 1; |
| 987 |
l < skeleton->getNumBodyNodes(); |
| 988 |
++l) |
| 989 |
#else |
| 990 |
for (std::size_t l = 0; l < skeleton->getNumBodyNodes(); ++l) |
| 991 |
#endif |
| 992 |
{ |
| 993 |
const BodyNode* relativeTo = skeleton->getBodyNode(l); |
| 994 |
|
| 995 |
compareBodyNodeFkToJacobianRelative( |
| 996 |
bn, relativeTo, Frame::World(), TOLERANCE); |
| 997 |
compareBodyNodeFkToJacobianRelative( |
| 998 |
bn, bn->getLocalCOM(), relativeTo, Frame::World(), TOLERANCE); |
| 999 |
compareBodyNodeFkToJacobianRelative( |
| 1000 |
bn, |
| 1001 |
Random::uniform<Eigen::Vector3d>(-10, 10), |
| 1002 |
relativeTo, |
| 1003 |
Frame::World(), |
| 1004 |
TOLERANCE); |
| 1005 |
|
| 1006 |
compareBodyNodeFkToJacobianRelative(bn, relativeTo, bn, TOLERANCE); |
| 1007 |
compareBodyNodeFkToJacobianRelative( |
| 1008 |
bn, bn->getLocalCOM(), relativeTo, bn, TOLERANCE); |
| 1009 |
compareBodyNodeFkToJacobianRelative( |
| 1010 |
bn, |
| 1011 |
Random::uniform<Eigen::Vector3d>(-10, 10), |
| 1012 |
relativeTo, |
| 1013 |
bn, |
| 1014 |
TOLERANCE); |
| 1015 |
|
| 1016 |
compareBodyNodeFkToJacobianRelative( |
| 1017 |
bn, relativeTo, relativeTo, TOLERANCE); |
| 1018 |
compareBodyNodeFkToJacobianRelative( |
| 1019 |
bn, bn->getLocalCOM(), relativeTo, relativeTo, TOLERANCE); |
| 1020 |
compareBodyNodeFkToJacobianRelative( |
| 1021 |
bn, |
| 1022 |
Random::uniform<Eigen::Vector3d>(-10, 10), |
| 1023 |
relativeTo, |
| 1024 |
relativeTo, |
| 1025 |
TOLERANCE); |
| 1026 |
|
| 1027 |
for (std::size_t r = 0; r < refFrames.size(); ++r) |
| 1028 |
{ |
| 1029 |
compareBodyNodeFkToJacobianRelative( |
| 1030 |
bn, relativeTo, refFrames[r], TOLERANCE); |
| 1031 |
compareBodyNodeFkToJacobianRelative( |
| 1032 |
bn, bn->getLocalCOM(), relativeTo, refFrames[r], TOLERANCE); |
| 1033 |
compareBodyNodeFkToJacobianRelative( |
| 1034 |
bn, |
| 1035 |
Random::uniform<Eigen::Vector3d>(-10, 10), |
| 1036 |
relativeTo, |
| 1037 |
refFrames[r], |
| 1038 |
TOLERANCE); |
| 1039 |
} |
| 1040 |
} |
| 1041 |
} |
| 1042 |
} |
| 1043 |
} |
| 1044 |
} |
| 1045 |
|
| 1046 |
//============================================================================== |
| 1047 |
void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates( |
| 1048 |
const common::Uri& uri) |
| 1049 |
{ |
| 1050 |
using namespace std; |
| 1051 |
using namespace Eigen; |
| 1052 |
using namespace dart; |
| 1053 |
using namespace math; |
| 1054 |
using namespace dynamics; |
| 1055 |
using namespace simulation; |
| 1056 |
using namespace utils; |
| 1057 |
|
| 1058 |
//----------------------------- Settings ------------------------------------- |
| 1059 |
#ifndef NDEBUG // Debug mode |
| 1060 |
int nRandomItr = 2; |
| 1061 |
#else |
| 1062 |
int nRandomItr = 10; |
| 1063 |
#endif |
| 1064 |
double qLB = -0.5 * constantsd::pi(); |
| 1065 |
double qUB = 0.5 * constantsd::pi(); |
| 1066 |
double dqLB = -0.3 * constantsd::pi(); |
| 1067 |
double dqUB = 0.3 * constantsd::pi(); |
| 1068 |
double ddqLB = -0.1 * constantsd::pi(); |
| 1069 |
double ddqUB = 0.1 * constantsd::pi(); |
| 1070 |
Vector3d gravity(0.0, -9.81, 0.0); |
| 1071 |
double timeStep = 1e-3; |
| 1072 |
double TOLERANCE = 5e-4; |
| 1073 |
|
| 1074 |
// load skeleton |
| 1075 |
WorldPtr world = SkelParser::readWorld(uri); |
| 1076 |
assert(world != nullptr); |
| 1077 |
world->setGravity(gravity); |
| 1078 |
world->setTimeStep(timeStep); |
| 1079 |
|
| 1080 |
//------------------------------ Tests --------------------------------------- |
| 1081 |
for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) |
| 1082 |
{ |
| 1083 |
SkeletonPtr skeleton = world->getSkeleton(i); |
| 1084 |
assert(skeleton != nullptr); |
| 1085 |
int dof = skeleton->getNumDofs(); |
| 1086 |
|
| 1087 |
for (int j = 0; j < nRandomItr; ++j) |
| 1088 |
{ |
| 1089 |
// Generate a random state and ddq |
| 1090 |
VectorXd q0 = VectorXd(dof); |
| 1091 |
VectorXd dq0 = VectorXd(dof); |
| 1092 |
VectorXd ddq0 = VectorXd(dof); |
| 1093 |
for (int k = 0; k < dof; ++k) |
| 1094 |
{ |
| 1095 |
q0[k] = math::Random::uniform(qLB, qUB); |
| 1096 |
dq0[k] = math::Random::uniform(dqLB, dqUB); |
| 1097 |
ddq0[k] = math::Random::uniform(ddqLB, ddqUB); |
| 1098 |
} |
| 1099 |
|
| 1100 |
skeleton->setPositions(q0); |
| 1101 |
skeleton->setVelocities(dq0); |
| 1102 |
skeleton->setAccelerations(ddq0); |
| 1103 |
|
| 1104 |
skeleton->integratePositions(timeStep); |
| 1105 |
VectorXd q1 = skeleton->getPositions(); |
| 1106 |
skeleton->integrateVelocities(timeStep); |
| 1107 |
VectorXd dq1 = skeleton->getVelocities(); |
| 1108 |
|
| 1109 |
skeleton->integratePositions(timeStep); |
| 1110 |
VectorXd q2 = skeleton->getPositions(); |
| 1111 |
skeleton->integrateVelocities(timeStep); |
| 1112 |
VectorXd dq2 = skeleton->getVelocities(); |
| 1113 |
|
| 1114 |
VectorXd dq0FD = skeleton->getPositionDifferences(q1, q0) / timeStep; |
| 1115 |
VectorXd dq1FD = skeleton->getPositionDifferences(q2, q1) / timeStep; |
| 1116 |
VectorXd ddqFD1 |
| 1117 |
= skeleton->getVelocityDifferences(dq1FD, dq0FD) / timeStep; |
| 1118 |
VectorXd ddqFD2 = skeleton->getVelocityDifferences(dq2, dq1) / timeStep; |
| 1119 |
|
| 1120 |
EXPECT_TRUE(equals(dq0, dq0FD, TOLERANCE)) |
| 1121 |
<< "dq0 : " << dq0.transpose() << "\ndq0FD: " << dq0FD.transpose(); |
| 1122 |
|
| 1123 |
EXPECT_TRUE(equals(dq1, dq1FD, TOLERANCE)) |
| 1124 |
<< "dq1 : " << dq1.transpose() << "\ndq1FD: " << dq1FD.transpose(); |
| 1125 |
|
| 1126 |
EXPECT_TRUE(equals(ddq0, ddqFD1, 10 * TOLERANCE)) |
| 1127 |
<< "ddq0 : " << ddq0.transpose() |
| 1128 |
<< "\nddqFD1: " << ddqFD1.transpose(); |
| 1129 |
|
| 1130 |
EXPECT_TRUE(equals(ddq0, ddqFD2, 10 * TOLERANCE)) |
| 1131 |
<< "ddq0 : " << ddq0.transpose() |
| 1132 |
<< "\nddqFD2: " << ddqFD2.transpose() << std::endl; |
| 1133 |
} |
| 1134 |
} |
| 1135 |
} |
| 1136 |
|
| 1137 |
//============================================================================== |
| 1138 |
void DynamicsTest::testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri) |
| 1139 |
{ |
| 1140 |
using namespace std; |
| 1141 |
using namespace Eigen; |
| 1142 |
using namespace dart; |
| 1143 |
using namespace math; |
| 1144 |
using namespace dynamics; |
| 1145 |
using namespace simulation; |
| 1146 |
using namespace utils; |
| 1147 |
|
| 1148 |
//----------------------------- Settings ------------------------------------- |
| 1149 |
#ifndef NDEBUG // Debug mode |
| 1150 |
int nRandomItr = 2; |
| 1151 |
std::size_t numSteps = 1e+1; |
| 1152 |
#else |
| 1153 |
int nRandomItr = 10; |
| 1154 |
std::size_t numSteps = 1e+3; |
| 1155 |
#endif |
| 1156 |
double qLB = -0.5 * constantsd::pi(); |
| 1157 |
double qUB = 0.5 * constantsd::pi(); |
| 1158 |
double dqLB = -0.5 * constantsd::pi(); |
| 1159 |
double dqUB = 0.5 * constantsd::pi(); |
| 1160 |
double ddqLB = -0.5 * constantsd::pi(); |
| 1161 |
double ddqUB = 0.5 * constantsd::pi(); |
| 1162 |
Vector3d gravity(0.0, -9.81, 0.0); |
| 1163 |
double timeStep = 1.0e-6; |
| 1164 |
const double tol = timeStep * 1e+2; |
| 1165 |
|
| 1166 |
// load skeleton |
| 1167 |
WorldPtr world = SkelParser::readWorld(uri); |
| 1168 |
assert(world != nullptr); |
| 1169 |
world->setGravity(gravity); |
| 1170 |
world->setTimeStep(timeStep); |
| 1171 |
|
| 1172 |
//------------------------------ Tests --------------------------------------- |
| 1173 |
for (int i = 0; i < nRandomItr; ++i) |
| 1174 |
{ |
| 1175 |
for (std::size_t j = 0; j < world->getNumSkeletons(); ++j) |
| 1176 |
{ |
| 1177 |
SkeletonPtr skeleton = world->getSkeleton(j); |
| 1178 |
EXPECT_NE(skeleton, nullptr); |
| 1179 |
|
| 1180 |
std::size_t dof = skeleton->getNumDofs(); |
| 1181 |
std::size_t numBodies = skeleton->getNumBodyNodes(); |
| 1182 |
|
| 1183 |
// Generate random states |
| 1184 |
VectorXd q = Random::uniform<Eigen::VectorXd>(dof, qLB, qUB); |
| 1185 |
VectorXd dq = Random::uniform<Eigen::VectorXd>(dof, dqLB, dqUB); |
| 1186 |
VectorXd ddq = Random::uniform<Eigen::VectorXd>(dof, ddqLB, ddqUB); |
| 1187 |
|
| 1188 |
skeleton->setPositions(q); |
| 1189 |
skeleton->setVelocities(dq); |
| 1190 |
skeleton->setAccelerations(ddq); |
| 1191 |
|
| 1192 |
common::aligned_map<dynamics::BodyNodePtr, Eigen::Isometry3d> Tmap; |
| 1193 |
for (auto k = 0u; k < numBodies; ++k) |
| 1194 |
{ |
| 1195 |
auto body = skeleton->getBodyNode(k); |
| 1196 |
Tmap[body] = body->getTransform(); |
| 1197 |
} |
| 1198 |
|
| 1199 |
for (std::size_t k = 0; k < numSteps; ++k) |
| 1200 |
{ |
| 1201 |
skeleton->integrateVelocities(skeleton->getTimeStep()); |
| 1202 |
skeleton->integratePositions(skeleton->getTimeStep()); |
| 1203 |
|
| 1204 |
for (std::size_t l = 0; l < skeleton->getNumBodyNodes(); ++l) |
| 1205 |
{ |
| 1206 |
BodyNodePtr body = skeleton->getBodyNode(l); |
| 1207 |
|
| 1208 |
Isometry3d T1 = Tmap[body]; |
| 1209 |
Isometry3d T2 = body->getTransform(); |
| 1210 |
|
| 1211 |
Vector6d V_diff = math::logMap(T1.inverse() * T2) / timeStep; |
| 1212 |
Vector6d V_actual = body->getSpatialVelocity(); |
| 1213 |
|
| 1214 |
bool checkSpatialVelocity = equals(V_diff, V_actual, tol); |
| 1215 |
EXPECT_TRUE(checkSpatialVelocity); |
| 1216 |
if (!checkSpatialVelocity) |
| 1217 |
{ |
| 1218 |
std::cout << "[" << body->getName() << "]" << std::endl; |
| 1219 |
std::cout << "V_diff : " << V_diff.transpose() << std::endl; |
| 1220 |
std::cout << "V_actual : " << V_actual.transpose() << std::endl; |
| 1221 |
std::cout << std::endl; |
| 1222 |
} |
| 1223 |
|
| 1224 |
Tmap[body] = body->getTransform(); |
| 1225 |
} |
| 1226 |
} |
| 1227 |
} |
| 1228 |
} |
| 1229 |
} |
| 1230 |
|
| 1231 |
//============================================================================== |
| 1232 |
void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( |
| 1233 |
const common::Uri& uri) |
| 1234 |
{ |
| 1235 |
using namespace std; |
| 1236 |
using namespace Eigen; |
| 1237 |
using namespace dart; |
| 1238 |
using namespace math; |
| 1239 |
using namespace dynamics; |
| 1240 |
using namespace simulation; |
| 1241 |
using namespace utils; |
| 1242 |
|
| 1243 |
//----------------------------- Settings ------------------------------------- |
| 1244 |
const double TOLERANCE = 1.0e-2; |
| 1245 |
#ifndef NDEBUG // Debug mode |
| 1246 |
int nRandomItr = 2; |
| 1247 |
#else |
| 1248 |
int nRandomItr = 10; |
| 1249 |
#endif |
| 1250 |
double qLB = -0.5 * constantsd::pi(); |
| 1251 |
double qUB = 0.5 * constantsd::pi(); |
| 1252 |
double dqLB = -0.5 * constantsd::pi(); |
| 1253 |
double dqUB = 0.5 * constantsd::pi(); |
| 1254 |
double ddqLB = -0.5 * constantsd::pi(); |
| 1255 |
double ddqUB = 0.5 * constantsd::pi(); |
| 1256 |
Vector3d gravity(0.0, -9.81, 0.0); |
| 1257 |
double timeStep = 1.0e-6; |
| 1258 |
|
| 1259 |
// load skeleton |
| 1260 |
WorldPtr world = SkelParser::readWorld(uri); |
| 1261 |
assert(world != nullptr); |
| 1262 |
world->setGravity(gravity); |
| 1263 |
world->setTimeStep(timeStep); |
| 1264 |
|
| 1265 |
//------------------------------ Tests --------------------------------------- |
| 1266 |
for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) |
| 1267 |
{ |
| 1268 |
SkeletonPtr skeleton = world->getSkeleton(i); |
| 1269 |
assert(skeleton != nullptr); |
| 1270 |
int dof = skeleton->getNumDofs(); |
| 1271 |
|
| 1272 |
for (int j = 0; j < nRandomItr; ++j) |
| 1273 |
{ |
| 1274 |
// Generate a random state and ddq |
| 1275 |
VectorXd q = VectorXd(dof); |
| 1276 |
VectorXd dq = VectorXd(dof); |
| 1277 |
VectorXd ddq = VectorXd(dof); |
| 1278 |
for (int k = 0; k < dof; ++k) |
| 1279 |
{ |
| 1280 |
q[k] = math::Random::uniform(qLB, qUB); |
| 1281 |
dq[k] = math::Random::uniform(dqLB, dqUB); |
| 1282 |
ddq[k] = math::Random::uniform(ddqLB, ddqUB); |
| 1283 |
} |
| 1284 |
|
| 1285 |
// For each body node |
| 1286 |
for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k) |
| 1287 |
{ |
| 1288 |
BodyNode* bn = skeleton->getBodyNode(k); |
| 1289 |
|
| 1290 |
// Calculation of velocities and Jacobian at k-th time step |
| 1291 |
skeleton->setPositions(q); |
| 1292 |
skeleton->setVelocities(dq); |
| 1293 |
skeleton->setAccelerations(ddq); |
| 1294 |
|
| 1295 |
Vector3d BodyLinVel1 = bn->getLinearVelocity(Frame::World(), bn); |
| 1296 |
Vector3d BodyAngVel1 = bn->getAngularVelocity(Frame::World(), bn); |
| 1297 |
Vector3d WorldLinVel1 = bn->getLinearVelocity(); |
| 1298 |
Vector3d WorldAngVel1 = bn->getAngularVelocity(); |
| 1299 |
// Isometry3d T1 = bn->getTransform(); |
| 1300 |
|
| 1301 |
// Get accelerations and time derivatives of Jacobians at k-th time step |
| 1302 |
Vector3d BodyLinAcc1 = bn->getSpatialAcceleration().tail<3>(); |
| 1303 |
Vector3d BodyAngAcc1 = bn->getSpatialAcceleration().head<3>(); |
| 1304 |
Vector3d WorldLinAcc1 = bn->getLinearAcceleration(); |
| 1305 |
Vector3d WorldAngAcc1 = bn->getAngularAcceleration(); |
| 1306 |
|
| 1307 |
// Calculation of velocities and Jacobian at (k+1)-th time step |
| 1308 |
skeleton->integrateVelocities(skeleton->getTimeStep()); |
| 1309 |
skeleton->integratePositions(skeleton->getTimeStep()); |
| 1310 |
|
| 1311 |
Vector3d BodyLinVel2 = bn->getLinearVelocity(Frame::World(), bn); |
| 1312 |
Vector3d BodyAngVel2 = bn->getAngularVelocity(Frame::World(), bn); |
| 1313 |
Vector3d WorldLinVel2 = bn->getLinearVelocity(); |
| 1314 |
Vector3d WorldAngVel2 = bn->getAngularVelocity(); |
| 1315 |
// Isometry3d T2 = bn->getTransform(); |
| 1316 |
|
| 1317 |
// Get accelerations and time derivatives of Jacobians at k-th time step |
| 1318 |
Vector3d BodyLinAcc2 = bn->getSpatialAcceleration().tail<3>(); |
| 1319 |
Vector3d BodyAngAcc2 = bn->getSpatialAcceleration().head<3>(); |
| 1320 |
Vector3d WorldLinAcc2 = bn->getLinearAcceleration(); |
| 1321 |
Vector3d WorldAngAcc2 = bn->getAngularAcceleration(); |
| 1322 |
|
| 1323 |
// Calculation of approximated accelerations |
| 1324 |
Vector3d BodyLinAccApprox = (BodyLinVel2 - BodyLinVel1) / timeStep; |
| 1325 |
Vector3d BodyAngAccApprox = (BodyAngVel2 - BodyAngVel1) / timeStep; |
| 1326 |
Vector3d WorldLinAccApprox = (WorldLinVel2 - WorldLinVel1) / timeStep; |
| 1327 |
Vector3d WorldAngAccApprox = (WorldAngVel2 - WorldAngVel1) / timeStep; |
| 1328 |
|
| 1329 |
// Comparing two velocities |
| 1330 |
EXPECT_TRUE(equals(BodyLinAcc1, BodyLinAccApprox, TOLERANCE)); |
| 1331 |
EXPECT_TRUE(equals(BodyAngAcc1, BodyAngAccApprox, TOLERANCE)); |
| 1332 |
EXPECT_TRUE(equals(BodyLinAcc2, BodyLinAccApprox, TOLERANCE)); |
| 1333 |
EXPECT_TRUE(equals(BodyAngAcc2, BodyAngAccApprox, TOLERANCE)); |
| 1334 |
EXPECT_TRUE(equals(WorldLinAcc1, WorldLinAccApprox, TOLERANCE)); |
| 1335 |
EXPECT_TRUE(equals(WorldAngAcc1, WorldAngAccApprox, TOLERANCE)); |
| 1336 |
EXPECT_TRUE(equals(WorldLinAcc2, WorldLinAccApprox, TOLERANCE)); |
| 1337 |
EXPECT_TRUE(equals(WorldAngAcc2, WorldAngAccApprox, TOLERANCE)); |
| 1338 |
|
| 1339 |
// Debugging code |
| 1340 |
if (!equals(BodyLinAcc1, BodyLinAccApprox, TOLERANCE)) |
| 1341 |
{ |
| 1342 |
cout << "BodyLinAcc1 :" << BodyLinAcc1.transpose() << endl; |
| 1343 |
cout << "BodyLinAccApprox:" << BodyLinAccApprox.transpose() << endl; |
| 1344 |
} |
| 1345 |
if (!equals(BodyAngAcc1, BodyAngAccApprox, TOLERANCE)) |
| 1346 |
{ |
| 1347 |
cout << "BodyAngAcc1 :" << BodyAngAcc1.transpose() << endl; |
| 1348 |
cout << "BodyAngAccApprox:" << BodyAngAccApprox.transpose() << endl; |
| 1349 |
} |
| 1350 |
if (!equals(BodyLinAcc2, BodyLinAccApprox, TOLERANCE)) |
| 1351 |
{ |
| 1352 |
cout << "BodyLinAcc2 :" << BodyLinAcc2.transpose() << endl; |
| 1353 |
cout << "BodyLinAccApprox:" << BodyLinAccApprox.transpose() << endl; |
| 1354 |
} |
| 1355 |
if (!equals(BodyAngAcc2, BodyAngAccApprox, TOLERANCE)) |
| 1356 |
{ |
| 1357 |
cout << "BodyAngAcc2 :" << BodyAngAcc2.transpose() << endl; |
| 1358 |
cout << "BodyAngAccApprox:" << BodyAngAccApprox.transpose() << endl; |
| 1359 |
} |
| 1360 |
if (!equals(WorldLinAcc1, WorldLinAccApprox, TOLERANCE)) |
| 1361 |
{ |
| 1362 |
cout << "WorldLinAcc1 :" << WorldLinAcc1.transpose() << endl; |
| 1363 |
cout << "WorldLinAccApprox:" << WorldLinAccApprox.transpose() << endl; |
| 1364 |
} |
| 1365 |
if (!equals(WorldAngAcc1, WorldAngAccApprox, TOLERANCE)) |
| 1366 |
{ |
| 1367 |
cout << "WorldAngAcc1 :" << WorldAngAcc1.transpose() << endl; |
| 1368 |
cout << "WorldAngAccApprox:" << WorldAngAccApprox.transpose() << endl; |
| 1369 |
} |
| 1370 |
if (!equals(WorldLinAcc2, WorldLinAccApprox, TOLERANCE)) |
| 1371 |
{ |
| 1372 |
cout << "WorldLinAcc2 :" << WorldLinAcc2.transpose() << endl; |
| 1373 |
cout << "WorldLinAccApprox:" << WorldLinAccApprox.transpose() << endl; |
| 1374 |
} |
| 1375 |
if (!equals(WorldAngAcc2, WorldAngAccApprox, TOLERANCE)) |
| 1376 |
{ |
| 1377 |
cout << "WorldAngAcc2 :" << WorldAngAcc2.transpose() << endl; |
| 1378 |
cout << "WorldAngAccApprox:" << WorldAngAccApprox.transpose() << endl; |
| 1379 |
} |
| 1380 |
} |
| 1381 |
} |
| 1382 |
} |
| 1383 |
} |
| 1384 |
|
| 1385 |
//============================================================================== |
| 1386 |
void testForwardKinematicsSkeleton(const dynamics::SkeletonPtr& skel) |
| 1387 |
{ |
| 1388 |
#ifndef NDEBUG // Debug mode |
| 1389 |
std::size_t nRandomItr = 1e+1; |
| 1390 |
std::size_t numSteps = 1e+1; |
| 1391 |
#else |
| 1392 |
std::size_t nRandomItr = 1e+2; |
| 1393 |
std::size_t numSteps = 1e+2; |
| 1394 |
#endif |
| 1395 |
double qLB = -0.5 * constantsd::pi(); |
| 1396 |
double qUB = 0.5 * constantsd::pi(); |
| 1397 |
double dqLB = -0.3 * constantsd::pi(); |
| 1398 |
double dqUB = 0.3 * constantsd::pi(); |
| 1399 |
double ddqLB = -0.1 * constantsd::pi(); |
| 1400 |
double ddqUB = 0.1 * constantsd::pi(); |
| 1401 |
double timeStep = 1e-6; |
| 1402 |
|
| 1403 |
EXPECT_NE(skel, nullptr); |
| 1404 |
|
| 1405 |
skel->setTimeStep(timeStep); |
| 1406 |
|
| 1407 |
auto dof = skel->getNumDofs(); |
| 1408 |
auto numBodies = skel->getNumBodyNodes(); |
| 1409 |
|
| 1410 |
Eigen::VectorXd q; |
| 1411 |
Eigen::VectorXd dq; |
| 1412 |
Eigen::VectorXd ddq; |
| 1413 |
|
| 1414 |
common::aligned_map<dynamics::BodyNodePtr, Eigen::Isometry3d> Tmap; |
| 1415 |
common::aligned_map<dynamics::BodyNodePtr, Eigen::Vector6d> Vmap; |
| 1416 |
common::aligned_map<dynamics::BodyNodePtr, Eigen::Vector6d> dVmap; |
| 1417 |
|
| 1418 |
for (auto j = 0u; j < numBodies; ++j) |
| 1419 |
{ |
| 1420 |
auto body = skel->getBodyNode(j); |
| 1421 |
|
| 1422 |
Tmap[body] = Eigen::Isometry3d::Identity(); |
| 1423 |
Vmap[body] = Eigen::Vector6d::Zero(); |
| 1424 |
dVmap[body] = Eigen::Vector6d::Zero(); |
| 1425 |
} |
| 1426 |
|
| 1427 |
for (auto i = 0u; i < nRandomItr; ++i) |
| 1428 |
{ |
| 1429 |
q = Random::uniform<Eigen::VectorXd>(dof, qLB, qUB); |
| 1430 |
dq = Random::uniform<Eigen::VectorXd>(dof, dqLB, dqUB); |
| 1431 |
ddq = Random::uniform<Eigen::VectorXd>(dof, ddqLB, ddqUB); |
| 1432 |
|
| 1433 |
skel->setPositions(q); |
| 1434 |
skel->setVelocities(dq); |
| 1435 |
skel->setAccelerations(ddq); |
| 1436 |
|
| 1437 |
for (auto j = 0u; j < numSteps; ++j) |
| 1438 |
{ |
| 1439 |
for (auto k = 0u; k < numBodies; ++k) |
| 1440 |
{ |
| 1441 |
auto body = skel->getBodyNode(k); |
| 1442 |
auto joint = skel->getJoint(k); |
| 1443 |
auto parentBody = body->getParentBodyNode(); |
| 1444 |
Eigen::MatrixXd S = joint->getRelativeJacobian(); |
| 1445 |
Eigen::MatrixXd dS = joint->getRelativeJacobianTimeDeriv(); |
| 1446 |
Eigen::VectorXd jointQ = joint->getPositions(); |
| 1447 |
Eigen::VectorXd jointDQ = joint->getVelocities(); |
| 1448 |
Eigen::VectorXd jointDDQ = joint->getAccelerations(); |
| 1449 |
|
| 1450 |
Eigen::Isometry3d relT = body->getRelativeTransform(); |
| 1451 |
Eigen::Vector6d relV = S * jointDQ; |
| 1452 |
Eigen::Vector6d relDV = dS * jointDQ + S * jointDDQ; |
| 1453 |
|
| 1454 |
if (parentBody) |
| 1455 |
{ |
| 1456 |
Tmap[body] = Tmap[parentBody] * relT; |
| 1457 |
Vmap[body] = math::AdInvT(relT, Vmap[parentBody]) + relV; |
| 1458 |
dVmap[body] = math::AdInvT(relT, dVmap[parentBody]) |
| 1459 |
+ math::ad(Vmap[body], S * jointDQ) + relDV; |
| 1460 |
} |
| 1461 |
else |
| 1462 |
{ |
| 1463 |
Tmap[body] = relT; |
| 1464 |
Vmap[body] = relV; |
| 1465 |
dVmap[body] = relDV; |
| 1466 |
} |
| 1467 |
|
| 1468 |
bool checkT |
| 1469 |
= equals(body->getTransform().matrix(), Tmap[body].matrix()); |
| 1470 |
bool checkV = equals(body->getSpatialVelocity(), Vmap[body]); |
| 1471 |
bool checkDV = equals(body->getSpatialAcceleration(), dVmap[body]); |
| 1472 |
|
| 1473 |
EXPECT_TRUE(checkT); |
| 1474 |
EXPECT_TRUE(checkV); |
| 1475 |
EXPECT_TRUE(checkDV); |
| 1476 |
|
| 1477 |
if (!checkT) |
| 1478 |
{ |
| 1479 |
std::cout << "[" << body->getName() << "]" << std::endl; |
| 1480 |
std::cout << "actual T : " << std::endl |
| 1481 |
<< body->getTransform().matrix() << std::endl; |
| 1482 |
std::cout << "expected T: " << std::endl |
| 1483 |
<< Tmap[body].matrix() << std::endl; |
| 1484 |
std::cout << std::endl; |
| 1485 |
} |
| 1486 |
|
| 1487 |
if (!checkV) |
| 1488 |
{ |
| 1489 |
std::cout << "[" << body->getName() << "]" << std::endl; |
| 1490 |
std::cout << "actual V : " << body->getSpatialVelocity().transpose() |
| 1491 |
<< std::endl; |
| 1492 |
std::cout << "expected V: " << Vmap[body].transpose() << std::endl; |
| 1493 |
std::cout << std::endl; |
| 1494 |
} |
| 1495 |
|
| 1496 |
if (!checkDV) |
| 1497 |
{ |
| 1498 |
std::cout << "[" << body->getName() << "]" << std::endl; |
| 1499 |
std::cout << "actual DV : " |
| 1500 |
<< body->getSpatialAcceleration().transpose() << std::endl; |
| 1501 |
std::cout << "expected DV: " << dVmap[body].transpose() << std::endl; |
| 1502 |
std::cout << std::endl; |
| 1503 |
} |
| 1504 |
} |
| 1505 |
} |
| 1506 |
} |
| 1507 |
} |
| 1508 |
|
| 1509 |
//============================================================================== |
| 1510 |
void DynamicsTest::testForwardKinematics(const common::Uri& uri) |
| 1511 |
{ |
| 1512 |
auto world = utils::SkelParser::readWorld(uri); |
| 1513 |
EXPECT_TRUE(world != nullptr); |
| 1514 |
|
| 1515 |
auto numSkeletons = world->getNumSkeletons(); |
| 1516 |
for (auto i = 0u; i < numSkeletons; ++i) |
| 1517 |
{ |
| 1518 |
auto skeleton = world->getSkeleton(i); |
| 1519 |
testForwardKinematicsSkeleton(skeleton); |
| 1520 |
} |
| 1521 |
} |
| 1522 |
|
| 1523 |
//============================================================================== |
| 1524 |
void DynamicsTest::testInverseDynamics(const common::Uri& uri) |
| 1525 |
{ |
| 1526 |
//---------------------------- Settings -------------------------------------- |
| 1527 |
// Number of random state tests for each skeleton |
| 1528 |
#ifndef NDEBUG // Debug mode |
| 1529 |
const std::size_t nRandomItr = 2; |
| 1530 |
#else |
| 1531 |
const std::size_t nRandomItr = 100; |
| 1532 |
#endif |
| 1533 |
|
| 1534 |
// Lower and upper bound of configuration for system |
| 1535 |
const double lb = -1.0 * math::constantsd::pi(); |
| 1536 |
const double ub = 1.0 * math::constantsd::pi(); |
| 1537 |
|
| 1538 |
// Lower and upper bound of joint damping and stiffness |
| 1539 |
const double lbD = 0.0; |
| 1540 |
const double ubD = 10.0; |
| 1541 |
const double lbK = 0.0; |
| 1542 |
const double ubK = 10.0; |
| 1543 |
|
| 1544 |
// Lower and upper bound of joint forces |
| 1545 |
const double lbF = -10.0; |
| 1546 |
const double ubF = 10.0; |
| 1547 |
|
| 1548 |
simulation::WorldPtr world = utils::SkelParser::readWorld(uri); |
| 1549 |
ASSERT_TRUE(world != nullptr); |
| 1550 |
|
| 1551 |
for (std::size_t i = 0; i < world->getNumSkeletons(); ++i) |
| 1552 |
{ |
| 1553 |
dynamics::SkeletonPtr skel = world->getSkeleton(i); |
| 1554 |
const std::size_t dofs = skel->getNumDofs(); |
| 1555 |
|
| 1556 |
for (std::size_t j = 0; j < nRandomItr; ++j) |
| 1557 |
{ |
| 1558 |
// Random joint stiffness and damping coefficients |
| 1559 |
for (std::size_t l = 0; l < dofs; ++l) |
| 1560 |
{ |
| 1561 |
dynamics::DegreeOfFreedom* dof = skel->getDof(l); |
| 1562 |
dof->setDampingCoefficient(math::Random::uniform(lbD, ubD)); |
| 1563 |
dof->setSpringStiffness(math::Random::uniform(lbK, ubK)); |
| 1564 |
|
| 1565 |
double lbRP = dof->getPositionLowerLimit(); |
| 1566 |
double ubRP = dof->getPositionUpperLimit(); |
| 1567 |
lbRP = std::max(lbRP, -math::constantsd::pi()); |
| 1568 |
ubRP = std::min(ubRP, +math::constantsd::pi()); |
| 1569 |
dof->setRestPosition(math::Random::uniform(lbRP, ubRP)); |
| 1570 |
} |
| 1571 |
|
| 1572 |
// Set random states and forces |
| 1573 |
const VectorXd q0 = math::Random::uniform<VectorXd>(dofs, lb, ub); |
| 1574 |
const VectorXd dq0 = math::Random::uniform<VectorXd>(dofs, lb, ub); |
| 1575 |
const VectorXd f0 = math::Random::uniform<VectorXd>(dofs, lbF, ubF); |
| 1576 |
|
| 1577 |
// Forward dynamics to compute accelerations |
| 1578 |
skel->setPositions(q0); |
| 1579 |
skel->setVelocities(dq0); |
| 1580 |
skel->setForces(f0); |
| 1581 |
skel->computeForwardDynamics(); |
| 1582 |
const VectorXd ddq = skel->getAccelerations(); |
| 1583 |
|
| 1584 |
// Test inverse dynamics to ensure the result joint forces are identical |
| 1585 |
// to the forces used to compute the input accelerations |
| 1586 |
skel->setAccelerations(ddq); |
| 1587 |
skel->computeInverseDynamics(false, true, true); |
| 1588 |
const VectorXd f = skel->getForces(); |
| 1589 |
EXPECT_TRUE(equals(f, f0)); |
| 1590 |
|
| 1591 |
// Test forward dynamics to ensure the result joint accelerations are |
| 1592 |
// identical to the accelerations used to compute the input forces |
| 1593 |
skel->setForces(f); |
| 1594 |
skel->computeForwardDynamics(); |
| 1595 |
const VectorXd ddq2 = skel->getAccelerations(); |
| 1596 |
EXPECT_TRUE(equals(ddq, ddq2)); |
| 1597 |
} |
| 1598 |
} |
| 1599 |
} |
| 1600 |
|
| 1601 |
//============================================================================== |
| 1602 |
void DynamicsTest::compareEquationsOfMotion(const common::Uri& uri) |
| 1603 |
{ |
| 1604 |
using namespace std; |
| 1605 |
using namespace Eigen; |
| 1606 |
using namespace dart; |
| 1607 |
using namespace math; |
| 1608 |
using namespace dynamics; |
| 1609 |
using namespace simulation; |
| 1610 |
using namespace utils; |
| 1611 |
|
| 1612 |
//---------------------------- Settings -------------------------------------- |
| 1613 |
// Number of random state tests for each skeletons |
| 1614 |
#ifndef NDEBUG // Debug mode |
| 1615 |
std::size_t nRandomItr = 2; |
| 1616 |
#else |
| 1617 |
std::size_t nRandomItr = 100; |
| 1618 |
#endif |
| 1619 |
|
| 1620 |
// Lower and upper bound of configuration for system |
| 1621 |
double lb = -1.0 * constantsd::pi(); |
| 1622 |
double ub = 1.0 * constantsd::pi(); |
| 1623 |
|
| 1624 |
// Lower and upper bound of joint damping and stiffness |
| 1625 |
double lbD = 0.0; |
| 1626 |
double ubD = 10.0; |
| 1627 |
double lbK = 0.0; |
| 1628 |
double ubK = 10.0; |
| 1629 |
|
| 1630 |
simulation::WorldPtr myWorld; |
| 1631 |
|
| 1632 |
//----------------------------- Tests ---------------------------------------- |
| 1633 |
// Check whether multiplication of mass matrix and its inverse is identity |
| 1634 |
// matrix. |
| 1635 |
myWorld = utils::SkelParser::readWorld(uri); |
| 1636 |
EXPECT_TRUE(myWorld != nullptr); |
| 1637 |
|
| 1638 |
for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) |
| 1639 |
{ |
| 1640 |
dynamics::SkeletonPtr skel = myWorld->getSkeleton(i); |
| 1641 |
|
| 1642 |
std::size_t dof = skel->getNumDofs(); |
| 1643 |
// int nBodyNodes = skel->getNumBodyNodes(); |
| 1644 |
|
| 1645 |
if (dof == 0) |
| 1646 |
{ |
| 1647 |
dtmsg << "Skeleton [" << skel->getName() << "] is skipped since it has " |
| 1648 |
<< "0 DOF." << endl; |
| 1649 |
continue; |
| 1650 |
} |
| 1651 |
|
| 1652 |
for (std::size_t j = 0; j < nRandomItr; ++j) |
| 1653 |
{ |
| 1654 |
// Random joint stiffness and damping coefficient |
| 1655 |
for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k) |
| 1656 |
{ |
| 1657 |
BodyNode* body = skel->getBodyNode(k); |
| 1658 |
Joint* joint = body->getParentJoint(); |
| 1659 |
std::size_t localDof = joint->getNumDofs(); |
| 1660 |
|
| 1661 |
for (std::size_t l = 0; l < localDof; ++l) |
| 1662 |
{ |
| 1663 |
joint->setDampingCoefficient(l, Random::uniform(lbD, ubD)); |
| 1664 |
joint->setSpringStiffness(l, Random::uniform(lbK, ubK)); |
| 1665 |
|
| 1666 |
double lbRP = joint->getPositionLowerLimit(l); |
| 1667 |
double ubRP = joint->getPositionUpperLimit(l); |
| 1668 |
if (lbRP < -constantsd::pi()) |
| 1669 |
lbRP = -constantsd::pi(); |
| 1670 |
if (ubRP > constantsd::pi()) |
| 1671 |
ubRP = constantsd::pi(); |
| 1672 |
joint->setRestPosition(l, Random::uniform(lbRP, ubRP)); |
| 1673 |
} |
| 1674 |
} |
| 1675 |
|
| 1676 |
// Set random states |
| 1677 |
Skeleton::Configuration x = skel->getConfiguration( |
| 1678 |
Skeleton::CONFIG_POSITIONS | Skeleton::CONFIG_VELOCITIES); |
| 1679 |
for (auto k = 0u; k < skel->getNumDofs(); ++k) |
| 1680 |
{ |
| 1681 |
x.mPositions[k] = Random::uniform(lb, ub); |
| 1682 |
x.mVelocities[k] = Random::uniform(lb, ub); |
| 1683 |
} |
| 1684 |
skel->setConfiguration(x); |
| 1685 |
|
| 1686 |
//------------------------ Mass Matrix Test ---------------------------- |
| 1687 |
// Get matrices |
| 1688 |
MatrixXd M = skel->getMassMatrix(); |
| 1689 |
MatrixXd M2 = getMassMatrix(skel); |
| 1690 |
MatrixXd InvM = skel->getInvMassMatrix(); |
| 1691 |
MatrixXd M_InvM = M * InvM; |
| 1692 |
MatrixXd InvM_M = InvM * M; |
| 1693 |
|
| 1694 |
MatrixXd AugM = skel->getAugMassMatrix(); |
| 1695 |
MatrixXd AugM2 = getAugMassMatrix(skel); |
| 1696 |
MatrixXd InvAugM = skel->getInvAugMassMatrix(); |
| 1697 |
MatrixXd AugM_InvAugM = AugM * InvAugM; |
| 1698 |
MatrixXd InvAugM_AugM = InvAugM * AugM; |
| 1699 |
|
| 1700 |
MatrixXd I = MatrixXd::Identity(dof, dof); |
| 1701 |
|
| 1702 |
bool failure = false; |
| 1703 |
|
| 1704 |
// Check if the number of generalized coordinates and dimension of mass |
| 1705 |
// matrix are same. |
| 1706 |
EXPECT_EQ(M.rows(), (int)dof); |
| 1707 |
EXPECT_EQ(M.cols(), (int)dof); |
| 1708 |
|
| 1709 |
// Check mass matrix |
| 1710 |
EXPECT_TRUE(equals(M, M2, 1e-6)); |
| 1711 |
if (!equals(M, M2, 1e-6)) |
| 1712 |
{ |
| 1713 |
cout << "M :" << endl << M << endl << endl; |
| 1714 |
cout << "M2:" << endl << M2 << endl << endl; |
| 1715 |
failure = true; |
| 1716 |
} |
| 1717 |
|
| 1718 |
// Check augmented mass matrix |
| 1719 |
EXPECT_TRUE(equals(AugM, AugM2, 1e-6)); |
| 1720 |
if (!equals(AugM, AugM2, 1e-6)) |
| 1721 |
{ |
| 1722 |
cout << "AugM :" << endl << AugM << endl << endl; |
| 1723 |
cout << "AugM2:" << endl << AugM2 << endl << endl; |
| 1724 |
failure = true; |
| 1725 |
} |
| 1726 |
|
| 1727 |
// Check if both of (M * InvM) and (InvM * M) are identity. |
| 1728 |
EXPECT_TRUE(equals(M_InvM, I, 1e-6)); |
| 1729 |
if (!equals(M_InvM, I, 1e-6)) |
| 1730 |
{ |
| 1731 |
cout << "InvM :" << endl << InvM << endl << endl; |
| 1732 |
failure = true; |
| 1733 |
} |
| 1734 |
EXPECT_TRUE(equals(InvM_M, I, 1e-6)); |
| 1735 |
if (!equals(InvM_M, I, 1e-6)) |
| 1736 |
{ |
| 1737 |
cout << "InvM_M:" << endl << InvM_M << endl << endl; |
| 1738 |
failure = true; |
| 1739 |
} |
| 1740 |
|
| 1741 |
// Check if both of (M * InvM) and (InvM * M) are identity. |
| 1742 |
EXPECT_TRUE(equals(AugM_InvAugM, I, 1e-6)); |
| 1743 |
if (!equals(AugM_InvAugM, I, 1e-6)) |
| 1744 |
{ |
| 1745 |
cout << "AugM_InvAugM :" << endl << AugM_InvAugM << endl << endl; |
| 1746 |
failure = true; |
| 1747 |
} |
| 1748 |
EXPECT_TRUE(equals(InvAugM_AugM, I, 1e-6)); |
| 1749 |
if (!equals(InvAugM_AugM, I, 1e-6)) |
| 1750 |
{ |
| 1751 |
cout << "InvAugM_AugM:" << endl << InvAugM_AugM << endl << endl; |
| 1752 |
} |
| 1753 |
|
| 1754 |
//------- Coriolis Force Vector and Combined Force Vector Tests -------- |
| 1755 |
// Get C1, Coriolis force vector using recursive method |
| 1756 |
VectorXd C = skel->getCoriolisForces(); |
| 1757 |
VectorXd Cg = skel->getCoriolisAndGravityForces(); |
| 1758 |
|
| 1759 |
// Get C2, Coriolis force vector using inverse dynamics algorithm |
| 1760 |
Vector3d oldGravity = skel->getGravity(); |
| 1761 |
VectorXd oldTau = skel->getForces(); |
| 1762 |
VectorXd oldDdq = skel->getAccelerations(); |
| 1763 |
// TODO(JS): Save external forces of body nodes |
| 1764 |
|
| 1765 |
skel->clearInternalForces(); |
| 1766 |
skel->clearExternalForces(); |
| 1767 |
skel->setAccelerations(VectorXd::Zero(dof)); |
| 1768 |
|
| 1769 |
EXPECT_TRUE(skel->getForces() == VectorXd::Zero(dof)); |
| 1770 |
EXPECT_TRUE(skel->getExternalForces() == VectorXd::Zero(dof)); |
| 1771 |
EXPECT_TRUE(skel->getAccelerations() == VectorXd::Zero(dof)); |
| 1772 |
|
| 1773 |
skel->setGravity(Vector3d::Zero()); |
| 1774 |
EXPECT_TRUE(skel->getGravity() == Vector3d::Zero()); |
| 1775 |
skel->computeInverseDynamics(false, false); |
| 1776 |
VectorXd C2 = skel->getForces(); |
| 1777 |
|
| 1778 |
skel->setGravity(oldGravity); |
| 1779 |
EXPECT_TRUE(skel->getGravity() == oldGravity); |
| 1780 |
skel->computeInverseDynamics(false, false); |
| 1781 |
VectorXd Cg2 = skel->getForces(); |
| 1782 |
|
| 1783 |
EXPECT_TRUE(equals(C, C2, 1e-6)); |
| 1784 |
if (!equals(C, C2, 1e-6)) |
| 1785 |
{ |
| 1786 |
cout << "C :" << C.transpose() << endl; |
| 1787 |
cout << "C2:" << C2.transpose() << endl; |
| 1788 |
failure = true; |
| 1789 |
} |
| 1790 |
|
| 1791 |
EXPECT_TRUE(equals(Cg, Cg2, 1e-6)); |
| 1792 |
if (!equals(Cg, Cg2, 1e-6)) |
| 1793 |
{ |
| 1794 |
cout << "Cg :" << Cg.transpose() << endl; |
| 1795 |
cout << "Cg2:" << Cg2.transpose() << endl; |
| 1796 |
failure = true; |
| 1797 |
} |
| 1798 |
|
| 1799 |
skel->setForces(oldTau); |
| 1800 |
skel->setAccelerations(oldDdq); |
| 1801 |
// TODO(JS): Restore external forces of body nodes |
| 1802 |
|
| 1803 |
//------------------- Combined Force Vector Test ----------------------- |
| 1804 |
// TODO(JS): Not implemented yet. |
| 1805 |
|
| 1806 |
//---------------------- Damping Force Test ---------------------------- |
| 1807 |
// TODO(JS): Not implemented yet. |
| 1808 |
|
| 1809 |
//--------------------- External Force Test ---------------------------- |
| 1810 |
// TODO(JS): Not implemented yet. |
| 1811 |
|
| 1812 |
if (failure) |
| 1813 |
{ |
| 1814 |
std::cout << "Failure occurred in the World of file: " << uri.toString() |
| 1815 |
<< "\nWith Skeleton named: " << skel->getName() << "\n\n"; |
| 1816 |
} |
| 1817 |
} |
| 1818 |
} |
| 1819 |
} |
| 1820 |
|
| 1821 |
//============================================================================== |
| 1822 |
void compareCOMJacobianToFk( |
| 1823 |
const SkeletonPtr& skel, const Frame* refFrame, double tolerance) |
| 1824 |
{ |
| 1825 |
VectorXd dq = skel->getVelocities(); |
| 1826 |
VectorXd ddq = skel->getAccelerations(); |
| 1827 |
|
| 1828 |
Vector6d comSpatialVelFk |
| 1829 |
= skel->getCOMSpatialVelocity(Frame::World(), refFrame); |
| 1830 |
Vector6d comSpatialAccFk |
| 1831 |
= skel->getCOMSpatialAcceleration(Frame::World(), refFrame); |
| 1832 |
|
| 1833 |
math::Jacobian comSpatialJac = skel->getCOMJacobian(refFrame); |
| 1834 |
math::Jacobian comSpatialJacDeriv |
| 1835 |
= skel->getCOMJacobianSpatialDeriv(refFrame); |
| 1836 |
|
| 1837 |
Vector6d comSpatialVelJac = comSpatialJac * dq; |
| 1838 |
Vector6d comSpatialAccJac = comSpatialJac * ddq + comSpatialJacDeriv * dq; |
| 1839 |
|
| 1840 |
bool spatialVelEqual = equals(comSpatialVelFk, comSpatialVelJac, tolerance); |
| 1841 |
EXPECT_TRUE(spatialVelEqual); |
| 1842 |
if (!spatialVelEqual) |
| 1843 |
printComparisonError( |
| 1844 |
"COM spatial velocity", |
| 1845 |
skel->getName(), |
| 1846 |
refFrame->getName(), |
| 1847 |
comSpatialVelFk, |
| 1848 |
comSpatialVelJac); |
| 1849 |
|
| 1850 |
bool spatialAccEqual = equals(comSpatialAccFk, comSpatialAccJac, tolerance); |
| 1851 |
EXPECT_TRUE(spatialAccEqual); |
| 1852 |
if (!spatialAccEqual) |
| 1853 |
printComparisonError( |
| 1854 |
"COM spatial acceleration", |
| 1855 |
skel->getName(), |
| 1856 |
refFrame->getName(), |
| 1857 |
comSpatialAccFk, |
| 1858 |
comSpatialAccJac); |
| 1859 |
|
| 1860 |
Vector3d comLinearVelFk |
| 1861 |
= skel->getCOMLinearVelocity(Frame::World(), refFrame); |
| 1862 |
Vector3d comLinearAccFk |
| 1863 |
= skel->getCOMLinearAcceleration(Frame::World(), refFrame); |
| 1864 |
|
| 1865 |
math::LinearJacobian comLinearJac = skel->getCOMLinearJacobian(refFrame); |
| 1866 |
math::LinearJacobian comLinearJacDeriv |
| 1867 |
= skel->getCOMLinearJacobianDeriv(refFrame); |
| 1868 |
|
| 1869 |
Vector3d comLinearVelJac = comLinearJac * dq; |
| 1870 |
Vector3d comLinearAccJac = comLinearJac * ddq + comLinearJacDeriv * dq; |
| 1871 |
|
| 1872 |
bool linearVelEqual = equals(comLinearVelFk, comLinearVelJac); |
| 1873 |
EXPECT_TRUE(linearVelEqual); |
| 1874 |
if (!linearVelEqual) |
| 1875 |
printComparisonError( |
| 1876 |
"COM linear velocity", |
| 1877 |
skel->getName(), |
| 1878 |
refFrame->getName(), |
| 1879 |
comLinearVelFk, |
| 1880 |
comLinearVelJac); |
| 1881 |
|
| 1882 |
bool linearAccEqual = equals(comLinearAccFk, comLinearAccJac); |
| 1883 |
EXPECT_TRUE(linearAccEqual); |
| 1884 |
if (!linearAccEqual) |
| 1885 |
printComparisonError( |
| 1886 |
"COM linear acceleration", |
| 1887 |
skel->getName(), |
| 1888 |
refFrame->getName(), |
| 1889 |
comLinearAccFk, |
| 1890 |
comLinearAccJac); |
| 1891 |
} |
| 1892 |
|
| 1893 |
//============================================================================== |
| 1894 |
void DynamicsTest::testCenterOfMass(const common::Uri& uri) |
| 1895 |
{ |
| 1896 |
using namespace std; |
| 1897 |
using namespace Eigen; |
| 1898 |
using namespace dart; |
| 1899 |
using namespace math; |
| 1900 |
using namespace dynamics; |
| 1901 |
using namespace simulation; |
| 1902 |
using namespace utils; |
| 1903 |
|
| 1904 |
//---------------------------- Settings -------------------------------------- |
| 1905 |
// Number of random state tests for each skeletons |
| 1906 |
#ifndef NDEBUG // Debug mode |
| 1907 |
std::size_t nRandomItr = 2; |
| 1908 |
#else |
| 1909 |
std::size_t nRandomItr = 100; |
| 1910 |
#endif |
| 1911 |
|
| 1912 |
// Lower and upper bound of configuration for system |
| 1913 |
double lb = -1.5 * constantsd::pi(); |
| 1914 |
double ub = 1.5 * constantsd::pi(); |
| 1915 |
|
| 1916 |
// Lower and upper bound of joint damping and stiffness |
| 1917 |
double lbD = 0.0; |
| 1918 |
double ubD = 10.0; |
| 1919 |
double lbK = 0.0; |
| 1920 |
double ubK = 10.0; |
| 1921 |
|
| 1922 |
simulation::WorldPtr myWorld; |
| 1923 |
|
| 1924 |
//----------------------------- Tests ---------------------------------------- |
| 1925 |
// Check whether multiplication of mass matrix and its inverse is identity |
| 1926 |
// matrix. |
| 1927 |
myWorld = utils::SkelParser::readWorld(uri); |
| 1928 |
EXPECT_TRUE(myWorld != nullptr); |
| 1929 |
|
| 1930 |
for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) |
| 1931 |
{ |
| 1932 |
dynamics::SkeletonPtr skeleton = myWorld->getSkeleton(i); |
| 1933 |
|
| 1934 |
std::size_t dof = skeleton->getNumDofs(); |
| 1935 |
if (dof == 0) |
| 1936 |
{ |
| 1937 |
dtmsg << "Skeleton [" << skeleton->getName() << "] is skipped since it " |
| 1938 |
<< "has 0 DOF." << endl; |
| 1939 |
continue; |
| 1940 |
} |
| 1941 |
|
| 1942 |
for (std::size_t j = 0; j < nRandomItr; ++j) |
| 1943 |
{ |
| 1944 |
// For the second half of the tests, scramble up the Skeleton |
| 1945 |
if (j > ceil(nRandomItr / 2)) |
| 1946 |
{ |
| 1947 |
SkeletonPtr copy = skeleton->cloneSkeleton(); |
| 1948 |
std::size_t maxNode = skeleton->getNumBodyNodes() - 1; |
| 1949 |
BodyNode* bn1 = skeleton->getBodyNode( |
| 1950 |
math::Random::uniform<std::size_t>(0, maxNode)); |
| 1951 |
BodyNode* bn2 = skeleton->getBodyNode( |
| 1952 |
math::Random::uniform<std::size_t>(0, maxNode)); |
| 1953 |
|
| 1954 |
if (bn1 != bn2) |
| 1955 |
{ |
| 1956 |
BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2; |
| 1957 |
BodyNode* parent = child == bn1 ? bn2 : bn1; |
| 1958 |
|
| 1959 |
child->moveTo(parent); |
| 1960 |
} |
| 1961 |
|
| 1962 |
EXPECT_TRUE(skeleton->getNumBodyNodes() == copy->getNumBodyNodes()); |
| 1963 |
EXPECT_TRUE(skeleton->getNumDofs() == copy->getNumDofs()); |
| 1964 |
} |
| 1965 |
|
| 1966 |
// Random joint stiffness and damping coefficient |
| 1967 |
for (std::size_t k = 0; k < skeleton->getNumBodyNodes(); ++k) |
| 1968 |
{ |
| 1969 |
BodyNode* body = skeleton->getBodyNode(k); |
| 1970 |
Joint* joint = body->getParentJoint(); |
| 1971 |
int localDof = joint->getNumDofs(); |
| 1972 |
|
| 1973 |
for (int l = 0; l < localDof; ++l) |
| 1974 |
{ |
| 1975 |
joint->setDampingCoefficient(l, Random::uniform(lbD, ubD)); |
| 1976 |
joint->setSpringStiffness(l, Random::uniform(lbK, ubK)); |
| 1977 |
|
| 1978 |
double lbRP = joint->getPositionLowerLimit(l); |
| 1979 |
double ubRP = joint->getPositionUpperLimit(l); |
| 1980 |
if (lbRP < -constantsd::pi()) |
| 1981 |
lbRP = -constantsd::pi(); |
| 1982 |
if (ubRP > constantsd::pi()) |
| 1983 |
ubRP = constantsd::pi(); |
| 1984 |
joint->setRestPosition(l, Random::uniform(lbRP, ubRP)); |
| 1985 |
} |
| 1986 |
} |
| 1987 |
|
| 1988 |
// Set random states |
| 1989 |
VectorXd q = VectorXd(dof); |
| 1990 |
VectorXd dq = VectorXd(dof); |
| 1991 |
VectorXd ddq = VectorXd(dof); |
| 1992 |
for (std::size_t k = 0; k < dof; ++k) |
| 1993 |
{ |
| 1994 |
q[k] = math::Random::uniform(lb, ub); |
| 1995 |
dq[k] = math::Random::uniform(lb, ub); |
| 1996 |
ddq[k] = math::Random::uniform(lb, ub); |
| 1997 |
} |
| 1998 |
skeleton->setPositions(q); |
| 1999 |
skeleton->setVelocities(dq); |
| 2000 |
skeleton->setAccelerations(ddq); |
| 2001 |
|
| 2002 |
randomizeRefFrames(); |
| 2003 |
|
| 2004 |
compareCOMJacobianToFk(skeleton, Frame::World(), 1e-6); |
| 2005 |
|
| 2006 |
for (std::size_t r = 0; r < refFrames.size(); ++r) |
| 2007 |
compareCOMJacobianToFk(skeleton, refFrames[r], 1e-6); |
| 2008 |
} |
| 2009 |
} |
| 2010 |
} |
| 2011 |
|
| 2012 |
//============================================================================== |
| 2013 |
void compareCOMAccelerationToGravity( |
| 2014 |
SkeletonPtr skel, const Eigen::Vector3d& gravity, double tolerance) |
| 2015 |
{ |
| 2016 |
const std::size_t numFrames = 1e+2; |
| 2017 |
skel->setGravity(gravity); |
| 2018 |
|
| 2019 |
for (std::size_t i = 0; i < numFrames; ++i) |
| 2020 |
{ |
| 2021 |
skel->computeForwardDynamics(); |
| 2022 |
|
| 2023 |
Vector3d comLinearAccFk = skel->getCOMLinearAcceleration(); |
| 2024 |
|
| 2025 |
bool comLinearAccFkEqual = equals(gravity, comLinearAccFk, tolerance); |
| 2026 |
EXPECT_TRUE(comLinearAccFkEqual); |
| 2027 |
if (!comLinearAccFkEqual) |
| 2028 |
{ |
| 2029 |
printComparisonError( |
| 2030 |
"COM linear acceleration", |
| 2031 |
skel->getName(), |
| 2032 |
Frame::World()->getName(), |
| 2033 |
gravity, |
| 2034 |
comLinearAccFk); |
| 2035 |
} |
| 2036 |
|
| 2037 |
VectorXd dq = skel->getVelocities(); |
| 2038 |
VectorXd ddq = skel->getAccelerations(); |
| 2039 |
math::LinearJacobian comLinearJac = skel->getCOMLinearJacobian(); |
| 2040 |
math::LinearJacobian comLinearJacDeriv = skel->getCOMLinearJacobianDeriv(); |
| 2041 |
Vector3d comLinearAccJac = comLinearJac * ddq + comLinearJacDeriv * dq; |
| 2042 |
|
| 2043 |
bool comLinearAccJacEqual = equals(gravity, comLinearAccJac, tolerance); |
| 2044 |
EXPECT_TRUE(comLinearAccJacEqual); |
| 2045 |
if (!comLinearAccJacEqual) |
| 2046 |
{ |
| 2047 |
printComparisonError( |
| 2048 |
"COM linear acceleration", |
| 2049 |
skel->getName(), |
| 2050 |
Frame::World()->getName(), |
| 2051 |
gravity, |
| 2052 |
comLinearAccJac); |
| 2053 |
} |
| 2054 |
} |
| 2055 |
} |
| 2056 |
|
| 2057 |
//============================================================================== |
| 2058 |
void DynamicsTest::testCenterOfMassFreeFall(const common::Uri& uri) |
| 2059 |
{ |
| 2060 |
using namespace std; |
| 2061 |
using namespace Eigen; |
| 2062 |
using namespace dart; |
| 2063 |
using namespace math; |
| 2064 |
using namespace dynamics; |
| 2065 |
using namespace simulation; |
| 2066 |
using namespace utils; |
| 2067 |
|
| 2068 |
//---------------------------- Settings -------------------------------------- |
| 2069 |
// Number of random state tests for each skeletons |
| 2070 |
#ifndef NDEBUG // Debug mode |
| 2071 |
std::size_t nRandomItr = 2; |
| 2072 |
#else |
| 2073 |
std::size_t nRandomItr = 10; |
| 2074 |
#endif // ------- Debug mode |
| 2075 |
|
| 2076 |
// Lower and upper bound of configuration for system |
| 2077 |
double lb = -1.5 * constantsd::pi(); |
| 2078 |
double ub = 1.5 * constantsd::pi(); |
| 2079 |
|
| 2080 |
// Lower and upper bound of joint damping and stiffness |
| 2081 |
double lbD = 0.0; |
| 2082 |
double ubD = 10.0; |
| 2083 |
double lbK = 0.0; |
| 2084 |
double ubK = 10.0; |
| 2085 |
|
| 2086 |
simulation::WorldPtr myWorld; |
| 2087 |
std::vector<Vector3d> gravities(4); |
| 2088 |
gravities[0] = Vector3d::Zero(); |
| 2089 |
gravities[1] = Vector3d(-9.81, 0, 0); |
| 2090 |
gravities[2] = Vector3d(0, -9.81, 0); |
| 2091 |
gravities[3] = Vector3d(0, 0, -9.81); |
| 2092 |
|
| 2093 |
//----------------------------- Tests ---------------------------------------- |
| 2094 |
// Check whether multiplication of mass matrix and its inverse is identity |
| 2095 |
// matrix. |
| 2096 |
myWorld = utils::SkelParser::readWorld(uri); |
| 2097 |
EXPECT_TRUE(myWorld != nullptr); |
| 2098 |
|
| 2099 |
for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) |
| 2100 |
{ |
| 2101 |
auto skel = myWorld->getSkeleton(i); |
| 2102 |
auto rootJoint = skel->getJoint(0); |
| 2103 |
auto rootFreeJoint = dynamic_cast<dynamics::FreeJoint*>(rootJoint); |
| 2104 |
|
| 2105 |
auto dof = skel->getNumDofs(); |
| 2106 |
|
| 2107 |
if (nullptr == rootFreeJoint || !skel->isMobile() || 0 == dof) |
| 2108 |
{ |
| 2109 |
#if BUILD_TYPE_DEBUG |
| 2110 |
dtmsg << "Skipping COM free fall test for Skeleton [" << skel->getName() |
| 2111 |
<< "] since the Skeleton doesn't have FreeJoint at the root body " |
| 2112 |
<< " or immobile." << endl; |
| 2113 |
#endif |
| 2114 |
continue; |
| 2115 |
} |
| 2116 |
else |
| 2117 |
{ |
| 2118 |
rootFreeJoint->setActuatorType(dynamics::Joint::PASSIVE); |
| 2119 |
} |
| 2120 |
|
| 2121 |
// Make sure the damping and spring forces are zero for the root FreeJoint. |
| 2122 |
for (std::size_t l = 0; l < rootJoint->getNumDofs(); ++l) |
| 2123 |
{ |
| 2124 |
rootJoint->setDampingCoefficient(l, 0.0); |
| 2125 |
rootJoint->setSpringStiffness(l, 0.0); |
| 2126 |
rootJoint->setRestPosition(l, 0.0); |
| 2127 |
} |
| 2128 |
|
| 2129 |
for (std::size_t j = 0; j < nRandomItr; ++j) |
| 2130 |
{ |
| 2131 |
// Random joint stiffness and damping coefficient |
| 2132 |
for (std::size_t k = 1; k < skel->getNumBodyNodes(); ++k) |
| 2133 |
{ |
| 2134 |
auto body = skel->getBodyNode(k); |
| 2135 |
auto joint = body->getParentJoint(); |
| 2136 |
auto localDof = joint->getNumDofs(); |
| 2137 |
|
| 2138 |
for (std::size_t l = 0; l < localDof; ++l) |
| 2139 |
{ |
| 2140 |
joint->setDampingCoefficient(l, Random::uniform(lbD, ubD)); |
| 2141 |
joint->setSpringStiffness(l, Random::uniform(lbK, ubK)); |
| 2142 |
|
| 2143 |
double lbRP = joint->getPositionLowerLimit(l); |
| 2144 |
double ubRP = joint->getPositionUpperLimit(l); |
| 2145 |
if (lbRP < -constantsd::pi()) |
| 2146 |
lbRP = -constantsd::pi(); |
| 2147 |
if (ubRP > constantsd::pi()) |
| 2148 |
ubRP = constantsd::pi(); |
| 2149 |
joint->setRestPosition(l, Random::uniform(lbRP, ubRP)); |
| 2150 |
} |
| 2151 |
} |
| 2152 |
|
| 2153 |
// Set random states |
| 2154 |
VectorXd q = VectorXd(dof); |
| 2155 |
VectorXd dq = VectorXd(dof); |
| 2156 |
for (std::size_t k = 0; k < dof; ++k) |
| 2157 |
{ |
| 2158 |
q[k] = math::Random::uniform(lb, ub); |
| 2159 |
dq[k] = math::Random::uniform(lb, ub); |
| 2160 |
} |
| 2161 |
VectorXd ddq = VectorXd::Zero(dof); |
| 2162 |
skel->setPositions(q); |
| 2163 |
skel->setVelocities(dq); |
| 2164 |
skel->setAccelerations(ddq); |
| 2165 |
|
| 2166 |
for (const auto& gravity : gravities) |
| 2167 |
compareCOMAccelerationToGravity(skel, gravity, 1e-6); |
| 2168 |
} |
| 2169 |
} |
| 2170 |
} |
| 2171 |
|
| 2172 |
//============================================================================== |
| 2173 |
void DynamicsTest::testConstraintImpulse(const common::Uri& uri) |
| 2174 |
{ |
| 2175 |
using namespace std; |
| 2176 |
using namespace Eigen; |
| 2177 |
using namespace dart; |
| 2178 |
using namespace math; |
| 2179 |
using namespace dynamics; |
| 2180 |
using namespace simulation; |
| 2181 |
using namespace utils; |
| 2182 |
|
| 2183 |
//---------------------------- Settings -------------------------------------- |
| 2184 |
// Number of random state tests for each skeletons |
| 2185 |
#ifndef NDEBUG // Debug mode |
| 2186 |
std::size_t nRandomItr = 1; |
| 2187 |
#else |
| 2188 |
std::size_t nRandomItr = 1; |
| 2189 |
#endif |
| 2190 |
|
| 2191 |
// Lower and upper bound of configuration for system |
| 2192 |
// double lb = -1.5 * constantsd::pi(); |
| 2193 |
// double ub = 1.5 * constantsd::pi(); |
| 2194 |
|
| 2195 |
simulation::WorldPtr myWorld; |
| 2196 |
|
| 2197 |
//----------------------------- Tests ---------------------------------------- |
| 2198 |
// Check whether multiplication of mass matrix and its inverse is identity |
| 2199 |
// matrix. |
| 2200 |
myWorld = utils::SkelParser::readWorld(uri); |
| 2201 |
EXPECT_TRUE(myWorld != nullptr); |
| 2202 |
|
| 2203 |
for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) |
| 2204 |
{ |
| 2205 |
dynamics::SkeletonPtr skel = myWorld->getSkeleton(i); |
| 2206 |
|
| 2207 |
std::size_t dof = skel->getNumDofs(); |
| 2208 |
// int nBodyNodes = skel->getNumBodyNodes(); |
| 2209 |
|
| 2210 |
if (dof == 0 || !skel->isMobile()) |
| 2211 |
{ |
| 2212 |
dtdbg << "Skeleton [" << skel->getName() << "] is skipped since it has " |
| 2213 |
<< "0 DOF or is immobile." << endl; |
| 2214 |
continue; |
| 2215 |
} |
| 2216 |
|
| 2217 |
for (std::size_t j = 0; j < nRandomItr; ++j) |
| 2218 |
{ |
| 2219 |
// Set random configurations |
| 2220 |
for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k) |
| 2221 |
{ |
| 2222 |
BodyNode* body = skel->getBodyNode(k); |
| 2223 |
Joint* joint = body->getParentJoint(); |
| 2224 |
int localDof = joint->getNumDofs(); |
| 2225 |
|
| 2226 |
for (int l = 0; l < localDof; ++l) |
| 2227 |
{ |
| 2228 |
double lbRP = joint->getPositionLowerLimit(l); |
| 2229 |
double ubRP = joint->getPositionUpperLimit(l); |
| 2230 |
if (lbRP < -constantsd::pi()) |
| 2231 |
lbRP = -constantsd::pi(); |
| 2232 |
if (ubRP > constantsd::pi()) |
| 2233 |
ubRP = constantsd::pi(); |
| 2234 |
joint->setPosition(l, Random::uniform(lbRP, ubRP)); |
| 2235 |
} |
| 2236 |
|
| 2237 |
// Set constraint impulse on each body |
| 2238 |
skel->clearConstraintImpulses(); |
| 2239 |
Eigen::Vector6d impulseOnBody = Eigen::Vector6d::Random(); |
| 2240 |
body->setConstraintImpulse(impulseOnBody); |
| 2241 |
|
| 2242 |
// Get constraint force vector |
| 2243 |
Eigen::VectorXd constraintVector1 = skel->getConstraintForces(); |
| 2244 |
|
| 2245 |
// Get constraint force vector by using Jacobian of skeleon |
| 2246 |
Eigen::MatrixXd bodyJacobian = body->getJacobian(); |
| 2247 |
Eigen::VectorXd constraintVector2 |
| 2248 |
= bodyJacobian.transpose() * impulseOnBody / skel->getTimeStep(); |
| 2249 |
|
| 2250 |
std::size_t index = 0; |
| 2251 |
for (std::size_t l = 0; l < dof; ++l) |
| 2252 |
{ |
| 2253 |
if (constraintVector1(l) == 0.0) |
| 2254 |
continue; |
| 2255 |
|
| 2256 |
EXPECT_NEAR(constraintVector1(l), constraintVector2(index), 1e-6); |
| 2257 |
index++; |
| 2258 |
} |
| 2259 |
assert(static_cast<std::size_t>(bodyJacobian.cols()) == index); |
| 2260 |
} |
| 2261 |
} |
| 2262 |
} |
| 2263 |
} |
| 2264 |
|
| 2265 |
//============================================================================== |
| 2266 |
void DynamicsTest::testImpulseBasedDynamics(const common::Uri& uri) |
| 2267 |
{ |
| 2268 |
using namespace std; |
| 2269 |
using namespace Eigen; |
| 2270 |
using namespace dart; |
| 2271 |
using namespace math; |
| 2272 |
using namespace dynamics; |
| 2273 |
using namespace simulation; |
| 2274 |
using namespace utils; |
| 2275 |
|
| 2276 |
//---------------------------- Settings -------------------------------------- |
| 2277 |
// Number of random state tests for each skeletons |
| 2278 |
#ifndef NDEBUG // Debug mode |
| 2279 |
std::size_t nRandomItr = 1; |
| 2280 |
#else |
| 2281 |
std::size_t nRandomItr = 100; |
| 2282 |
#endif |
| 2283 |
|
| 2284 |
double TOLERANCE = 1e-1; |
| 2285 |
|
| 2286 |
// Lower and upper bound of configuration for system |
| 2287 |
double lb = -1.5 * constantsd::pi(); |
| 2288 |
double ub = 1.5 * constantsd::pi(); |
| 2289 |
|
| 2290 |
simulation::WorldPtr myWorld; |
| 2291 |
|
| 2292 |
//----------------------------- Tests ---------------------------------------- |
| 2293 |
// Check whether multiplication of mass matrix and its inverse is identity |
| 2294 |
// matrix. |
| 2295 |
myWorld = utils::SkelParser::readWorld(uri); |
| 2296 |
EXPECT_TRUE(myWorld != nullptr); |
| 2297 |
|
| 2298 |
for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i) |
| 2299 |
{ |
| 2300 |
dynamics::SkeletonPtr skel = myWorld->getSkeleton(i); |
| 2301 |
|
| 2302 |
int dof = skel->getNumDofs(); |
| 2303 |
// int nBodyNodes = skel->getNumBodyNodes(); |
| 2304 |
|
| 2305 |
if (dof == 0 || !skel->isMobile()) |
| 2306 |
{ |
| 2307 |
dtdbg << "Skeleton [" << skel->getName() << "] is skipped since it has " |
| 2308 |
<< "0 DOF or is immobile." << endl; |
| 2309 |
continue; |
| 2310 |
} |
| 2311 |
|
| 2312 |
for (std::size_t j = 0; j < nRandomItr; ++j) |
| 2313 |
{ |
| 2314 |
// Set random configurations |
| 2315 |
for (std::size_t k = 0; k < skel->getNumBodyNodes(); ++k) |
| 2316 |
{ |
| 2317 |
BodyNode* body = skel->getBodyNode(k); |
| 2318 |
Joint* joint = body->getParentJoint(); |
| 2319 |
int localDof = joint->getNumDofs(); |
| 2320 |
|
| 2321 |
for (int l = 0; l < localDof; ++l) |
| 2322 |
{ |
| 2323 |
double lbRP = joint->getPositionLowerLimit(l); |
| 2324 |
double ubRP = joint->getPositionUpperLimit(l); |
| 2325 |
if (lbRP < -constantsd::pi()) |
| 2326 |
lbRP = -constantsd::pi(); |
| 2327 |
if (ubRP > constantsd::pi()) |
| 2328 |
ubRP = constantsd::pi(); |
| 2329 |
joint->setPosition(l, Random::uniform(lbRP, ubRP)); |
| 2330 |
} |
| 2331 |
} |
| 2332 |
// skel->setPositions(VectorXd::Zero(dof)); |
| 2333 |
|
| 2334 |
// TODO(JS): Just clear what should be |
| 2335 |
skel->clearExternalForces(); |
| 2336 |
skel->clearConstraintImpulses(); |
| 2337 |
|
| 2338 |
// Set random impulses |
| 2339 |
VectorXd impulses = VectorXd::Zero(dof); |
| 2340 |
for (int k = 0; k < impulses.size(); ++k) |
| 2341 |
impulses[k] = Random::uniform(lb, ub); |
| 2342 |
skel->setJointConstraintImpulses(impulses); |
| 2343 |
|
| 2344 |
// Compute impulse-based forward dynamics |
| 2345 |
skel->computeImpulseForwardDynamics(); |
| 2346 |
|
| 2347 |
// Compare resultant velocity change and invM * impulses |
| 2348 |
VectorXd deltaVel1 = skel->getVelocityChanges(); |
| 2349 |
MatrixXd invM = skel->getInvMassMatrix(); |
| 2350 |
VectorXd deltaVel2 = invM * impulses; |
| 2351 |
|
| 2352 |
EXPECT_TRUE(equals(deltaVel1, deltaVel2, TOLERANCE)); |
| 2353 |
if (!equals(deltaVel1, deltaVel2, TOLERANCE)) |
| 2354 |
{ |
| 2355 |
cout << "deltaVel1: " << deltaVel1.transpose() << endl; |
| 2356 |
cout << "deltaVel2: " << deltaVel2.transpose() << endl; |
| 2357 |
cout << "error: " << (deltaVel1 - deltaVel2).norm() << endl; |
| 2358 |
} |
| 2359 |
} |
| 2360 |
} |
| 2361 |
} |
| 2362 |
|
| 2363 |
//============================================================================== |
| 2364 |
TEST_F(DynamicsTest, testJacobians) |
| 2365 |
{ |
| 2366 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2367 |
{ |
| 2368 |
#ifndef NDEBUG |
| 2369 |
dtdbg << getList()[i].toString() << std::endl; |
| 2370 |
#endif |
| 2371 |
testJacobians(getList()[i]); |
| 2372 |
} |
| 2373 |
} |
| 2374 |
|
| 2375 |
//============================================================================== |
| 2376 |
TEST_F(DynamicsTest, testFiniteDifference) |
| 2377 |
{ |
| 2378 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2379 |
{ |
| 2380 |
#if BUILD_TYPE_DEBUG |
| 2381 |
dtdbg << getList()[i].toString() << std::endl; |
| 2382 |
#endif |
| 2383 |
testFiniteDifferenceGeneralizedCoordinates(getList()[i]); |
| 2384 |
testFiniteDifferenceBodyNodeVelocity(getList()[i]); |
| 2385 |
testFiniteDifferenceBodyNodeAcceleration(getList()[i]); |
| 2386 |
} |
| 2387 |
} |
| 2388 |
|
| 2389 |
//============================================================================== |
| 2390 |
TEST_F(DynamicsTest, testForwardKinematics) |
| 2391 |
{ |
| 2392 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2393 |
{ |
| 2394 |
#ifndef NDEBUG |
| 2395 |
dtdbg << getList()[i].toString() << std::endl; |
| 2396 |
#endif |
| 2397 |
testForwardKinematics(getList()[i]); |
| 2398 |
} |
| 2399 |
} |
| 2400 |
|
| 2401 |
//============================================================================== |
| 2402 |
TEST_F(DynamicsTest, testInverseDynamics) |
| 2403 |
{ |
| 2404 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2405 |
{ |
| 2406 |
#ifndef NDEBUG |
| 2407 |
dtdbg << getList()[i].toString() << std::endl; |
| 2408 |
#endif |
| 2409 |
testInverseDynamics(getList()[i]); |
| 2410 |
} |
| 2411 |
} |
| 2412 |
|
| 2413 |
//============================================================================== |
| 2414 |
TEST_F(DynamicsTest, compareEquationsOfMotion) |
| 2415 |
{ |
| 2416 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2417 |
{ |
| 2418 |
//////////////////////////////////////////////////////////////////////////// |
| 2419 |
// TODO(JS): Following skel files, which contain euler joints couldn't |
| 2420 |
// pass EQUATIONS_OF_MOTION, are disabled. |
| 2421 |
const auto uri = getList()[i]; |
| 2422 |
if (uri.toString() == "dart://sample/skel/test/double_pendulum_euler_joint.skel" |
| 2423 |
|| uri.toString() == "dart://sample/skel/test/chainwhipa.skel" |
| 2424 |
|| uri.toString() == "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel" |
| 2425 |
|| uri.toString() == "dart://sample/skel/test/simple_tree_structure_euler_joint.skel" |
| 2426 |
|| uri.toString() == "dart://sample/skel/test/tree_structure_euler_joint.skel" |
| 2427 |
|| uri.toString() == "dart://sample/skel/fullbody1.skel") |
| 2428 |
{ |
| 2429 |
continue; |
| 2430 |
} |
| 2431 |
//////////////////////////////////////////////////////////////////////////// |
| 2432 |
|
| 2433 |
#ifndef NDEBUG |
| 2434 |
dtdbg << getList()[i].toString() << std::endl; |
| 2435 |
#endif |
| 2436 |
compareEquationsOfMotion(getList()[i]); |
| 2437 |
} |
| 2438 |
} |
| 2439 |
|
| 2440 |
//============================================================================== |
| 2441 |
TEST_F(DynamicsTest, testCenterOfMass) |
| 2442 |
{ |
| 2443 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2444 |
{ |
| 2445 |
#ifndef NDEBUG |
| 2446 |
dtdbg << getList()[i].toString() << std::endl; |
| 2447 |
#endif |
| 2448 |
testCenterOfMass(getList()[i]); |
| 2449 |
} |
| 2450 |
} |
| 2451 |
|
| 2452 |
//============================================================================== |
| 2453 |
TEST_F(DynamicsTest, testCenterOfMassFreeFall) |
| 2454 |
{ |
| 2455 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2456 |
{ |
| 2457 |
#ifndef NDEBUG |
| 2458 |
dtdbg << getList()[i].toString() << std::endl; |
| 2459 |
#endif |
| 2460 |
testCenterOfMassFreeFall(getList()[i]); |
| 2461 |
} |
| 2462 |
} |
| 2463 |
|
| 2464 |
//============================================================================== |
| 2465 |
TEST_F(DynamicsTest, testConstraintImpulse) |
| 2466 |
{ |
| 2467 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2468 |
{ |
| 2469 |
#ifndef NDEBUG |
| 2470 |
dtdbg << getList()[i].toString() << std::endl; |
| 2471 |
#endif |
| 2472 |
testConstraintImpulse(getList()[i]); |
| 2473 |
} |
| 2474 |
} |
| 2475 |
|
| 2476 |
//============================================================================== |
| 2477 |
TEST_F(DynamicsTest, testImpulseBasedDynamics) |
| 2478 |
{ |
| 2479 |
for (std::size_t i = 0; i < getList().size(); ++i) |
| 2480 |
{ |
| 2481 |
#ifndef NDEBUG |
| 2482 |
dtdbg << getList()[i].toString() << std::endl; |
| 2483 |
#endif |
| 2484 |
testImpulseBasedDynamics(getList()[i]); |
| 2485 |
} |
| 2486 |
} |
| 2487 |
|
| 2488 |
//============================================================================== |
| 2489 |
TEST_F(DynamicsTest, HybridDynamics) |
| 2490 |
{ |
| 2491 |
const double tol = 1e-8; |
| 2492 |
const double timeStep = 1e-3; |
| 2493 |
#ifndef NDEBUG // Debug mode |
| 2494 |
const std::size_t numFrames = 50; // 0.05 secs |
| 2495 |
#else |
| 2496 |
const std::size_t numFrames = 5e+3; // 5 secs |
| 2497 |
#endif // ------- Debug mode |
| 2498 |
|
| 2499 |
// Load world and skeleton |
| 2500 |
WorldPtr world = utils::SkelParser::readWorld( |
| 2501 |
"dart://sample/skel/test/hybrid_dynamics_test.skel"); |
| 2502 |
world->setTimeStep(timeStep); |
| 2503 |
EXPECT_TRUE(world != nullptr); |
| 2504 |
EXPECT_NEAR(world->getTimeStep(), timeStep, tol); |
| 2505 |
|
| 2506 |
SkeletonPtr skel = world->getSkeleton("skeleton 1"); |
| 2507 |
EXPECT_TRUE(skel != nullptr); |
| 2508 |
EXPECT_NEAR(skel->getTimeStep(), timeStep, tol); |
| 2509 |
|
| 2510 |
const std::size_t numDofs = skel->getNumDofs(); |
| 2511 |
|
| 2512 |
// Zero initial states |
| 2513 |
Eigen::VectorXd q0 = Eigen::VectorXd::Zero(numDofs); |
| 2514 |
Eigen::VectorXd dq0 = Eigen::VectorXd::Zero(numDofs); |
| 2515 |
|
| 2516 |
// Initialize the skeleton with the zero initial states |
| 2517 |
skel->setPositions(q0); |
| 2518 |
skel->setVelocities(dq0); |
| 2519 |
EXPECT_TRUE(equals(skel->getPositions(), q0)); |
| 2520 |
EXPECT_TRUE(equals(skel->getVelocities(), dq0)); |
| 2521 |
|
| 2522 |
// Make sure all the joint actuator types |
| 2523 |
EXPECT_EQ(skel->getJoint(0)->getActuatorType(), Joint::FORCE); |
| 2524 |
EXPECT_EQ(skel->getJoint(1)->getActuatorType(), Joint::ACCELERATION); |
| 2525 |
EXPECT_EQ(skel->getJoint(2)->getActuatorType(), Joint::VELOCITY); |
| 2526 |
EXPECT_EQ(skel->getJoint(3)->getActuatorType(), Joint::ACCELERATION); |
| 2527 |
EXPECT_EQ(skel->getJoint(4)->getActuatorType(), Joint::VELOCITY); |
| 2528 |
|
| 2529 |
// Prepare command for each joint types per simulation steps |
| 2530 |
Eigen::MatrixXd command = Eigen::MatrixXd::Zero(numFrames, numDofs); |
| 2531 |
Eigen::VectorXd amp = Eigen::VectorXd::Zero(numDofs); |
| 2532 |
for (std::size_t i = 0; i < numDofs; ++i) |
| 2533 |
amp[i] = math::Random::uniform(-1.5, 1.5); |
| 2534 |
for (std::size_t i = 0; i < numFrames; ++i) |
| 2535 |
{ |
| 2536 |
for (std::size_t j = 0; j < numDofs; ++j) |
| 2537 |
command(i, j) = amp[j] * std::sin(i * timeStep); |
| 2538 |
} |
| 2539 |
|
| 2540 |
// Record joint forces for joint[1~4] |
| 2541 |
Eigen::MatrixXd forces = Eigen::MatrixXd::Zero(numFrames, numDofs); |
| 2542 |
for (std::size_t i = 0; i < numFrames; ++i) |
| 2543 |
{ |
| 2544 |
skel->setCommands(command.row(i)); |
| 2545 |
|
| 2546 |
world->step(false); |
| 2547 |
|
| 2548 |
forces.row(i) = skel->getForces(); |
| 2549 |
|
| 2550 |
EXPECT_NEAR(command(i, 0), skel->getForce(0), tol); |
| 2551 |
EXPECT_NEAR(command(i, 1), skel->getAcceleration(1), tol); |
| 2552 |
EXPECT_NEAR(command(i, 2), skel->getVelocity(2), tol); |
| 2553 |
EXPECT_NEAR(command(i, 3), skel->getAcceleration(3), tol); |
| 2554 |
EXPECT_NEAR(command(i, 4), skel->getVelocity(4), tol); |
| 2555 |
} |
| 2556 |
|
| 2557 |
// Restore the skeleton to the initial state |
| 2558 |
skel->setPositions(q0); |
| 2559 |
skel->setVelocities(dq0); |
| 2560 |
EXPECT_TRUE(equals(skel->getPositions(), q0)); |
| 2561 |
EXPECT_TRUE(equals(skel->getVelocities(), dq0)); |
| 2562 |
|
| 2563 |
// Change all the actuator types to force |
| 2564 |
skel->getJoint(0)->setActuatorType(Joint::FORCE); |
| 2565 |
skel->getJoint(1)->setActuatorType(Joint::FORCE); |
| 2566 |
skel->getJoint(2)->setActuatorType(Joint::FORCE); |
| 2567 |
skel->getJoint(3)->setActuatorType(Joint::FORCE); |
| 2568 |
skel->getJoint(4)->setActuatorType(Joint::FORCE); |
| 2569 |
EXPECT_EQ(skel->getJoint(0)->getActuatorType(), Joint::FORCE); |
| 2570 |
EXPECT_EQ(skel->getJoint(1)->getActuatorType(), Joint::FORCE); |
| 2571 |
EXPECT_EQ(skel->getJoint(2)->getActuatorType(), Joint::FORCE); |
| 2572 |
EXPECT_EQ(skel->getJoint(3)->getActuatorType(), Joint::FORCE); |
| 2573 |
EXPECT_EQ(skel->getJoint(4)->getActuatorType(), Joint::FORCE); |
| 2574 |
|
| 2575 |
// Test if the skeleton moves as the command with the joint forces |
| 2576 |
Eigen::MatrixXd output = Eigen::MatrixXd::Zero(numFrames, numDofs); |
| 2577 |
for (std::size_t i = 0; i < numFrames; ++i) |
| 2578 |
{ |
| 2579 |
skel->setCommands(forces.row(i)); |
| 2580 |
|
| 2581 |
world->step(false); |
| 2582 |
|
| 2583 |
output(i, 0) = skel->getJoint(0)->getForce(0); |
| 2584 |
output(i, 1) = skel->getJoint(1)->getAcceleration(0); |
| 2585 |
output(i, 2) = skel->getJoint(2)->getVelocity(0); |
| 2586 |
output(i, 3) = skel->getJoint(3)->getAcceleration(0); |
| 2587 |
output(i, 4) = skel->getJoint(4)->getVelocity(0); |
| 2588 |
|
| 2589 |
EXPECT_NEAR(command(i, 0), output(i, 0), tol); |
| 2590 |
EXPECT_NEAR(command(i, 1), output(i, 1), tol); |
| 2591 |
EXPECT_NEAR(command(i, 2), output(i, 2), tol); |
| 2592 |
EXPECT_NEAR(command(i, 3), output(i, 3), tol); |
| 2593 |
EXPECT_NEAR(command(i, 4), output(i, 4), tol); |
| 2594 |
} |
| 2595 |
} |
| 2596 |
|
| 2597 |
//============================================================================== |
| 2598 |
TEST_F(DynamicsTest, OffsetCOM) |
| 2599 |
{ |
| 2600 |
WorldPtr world = World::create(); |
| 2601 |
ASSERT_TRUE(world != nullptr); |
| 2602 |
world->setGravity(Eigen::Vector3d(0.0, 0.0, 0.0)); |
| 2603 |
const double dt = 0.001; |
| 2604 |
world->setTimeStep(dt); |
| 2605 |
|
| 2606 |
SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0)); |
| 2607 |
world->addSkeleton(boxSkel); |
| 2608 |
BodyNode* box = boxSkel->getBodyNode(0); |
| 2609 |
dynamics::Inertia inertia; |
| 2610 |
inertia.setMass(60); |
| 2611 |
inertia.setMoment( |
| 2612 |
box->getShapeNode(0)->getShape()->computeInertia(inertia.getMass())); |
| 2613 |
inertia.setLocalCOM({0, 500, 0}); |
| 2614 |
box->setInertia(inertia); |
| 2615 |
Eigen::Isometry3d boxInitialPose = box->getWorldTransform(); |
| 2616 |
Eigen::Vector3d torque(0, 0, 100); |
| 2617 |
box->addExtTorque(torque, false); |
| 2618 |
|
| 2619 |
{ |
| 2620 |
Vector3d comVel = box->getCOMLinearVelocity(); |
| 2621 |
EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel)) |
| 2622 |
<< "comVel: " << comVel.transpose(); |
| 2623 |
} |
| 2624 |
{ |
| 2625 |
Vector3d linVel = box->getLinearVelocity(); |
| 2626 |
EXPECT_TRUE(equals(Vector3d(0, 0, 0), linVel)) |
| 2627 |
<< "linVel: " << linVel.transpose(); |
| 2628 |
} |
| 2629 |
|
| 2630 |
world->step(); |
| 2631 |
// Velocity at COM should be zero since the object is just rotating. |
| 2632 |
{ |
| 2633 |
Vector3d comVel = box->getCOMLinearVelocity(); |
| 2634 |
EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel, 1e-4)) |
| 2635 |
<< "comVel: " << comVel.transpose(); |
| 2636 |
} |
| 2637 |
{ |
| 2638 |
Vector3d angVel = box->getAngularVelocity(); |
| 2639 |
// We can compute expectedAngAccel like so because the initial angular |
| 2640 |
// velocity is zero. |
| 2641 |
Vector3d expectedAngAccel = inertia.getMoment().inverse() * torque; |
| 2642 |
Vector3d expectedAngVel = expectedAngAccel * dt; |
| 2643 |
EXPECT_TRUE(equals(expectedAngVel, angVel)) |
| 2644 |
<< "angVel: " << angVel.transpose(); |
| 2645 |
Vector3d linVel = box->getLinearVelocity(); |
| 2646 |
Vector3d expLinVel |
| 2647 |
= angVel.cross(boxInitialPose.linear() * -box->getLocalCOM()); |
| 2648 |
EXPECT_TRUE(equals(expLinVel, linVel)) |
| 2649 |
<< "Expected: " << expLinVel.transpose() |
| 2650 |
<< "\nActual: " << linVel.transpose(); |
| 2651 |
|
| 2652 |
Vector3d angAccel = box->getAngularAcceleration(); |
| 2653 |
EXPECT_TRUE(equals(expectedAngAccel, angAccel)) |
| 2654 |
<< "angAccel: " << angAccel.transpose(); |
| 2655 |
Vector3d linAccel = box->getLinearAcceleration(); |
| 2656 |
Vector3d expLinAccel |
| 2657 |
= expectedAngAccel.cross(boxInitialPose.linear() * -box->getLocalCOM()); |
| 2658 |
EXPECT_TRUE(equals(expLinAccel, linAccel)) |
| 2659 |
<< "Expected: " << expLinAccel.transpose() |
| 2660 |
<< "\nActual: " << linAccel.transpose(); |
| 2661 |
} |
| 2662 |
} |