/* * Copyright (c) 2011-2022, The DART development contributors * All rights reserved. * * The list of contributors can be found at: * https://github.com/dartsim/dart/blob/master/LICENSE * * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following * conditions are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include "dart/dynamics/SimpleFrame.hpp" #include "dart/math/Helpers.hpp" #include "dart/math/Random.hpp" #include "GTestUtils.hpp" #include "TestHelpers.hpp" using namespace dart; using namespace dynamics; template Eigen::Matrix random_vec(double limit = 100) { return Random::uniform>(-limit, limit); } void randomize_transform( Eigen::Isometry3d& tf, double translation_limit = 100, double rotation_limit = 100) { Eigen::Vector3d r = random_vec<3>(translation_limit); Eigen::Vector3d theta = random_vec<3>(rotation_limit); tf.setIdentity(); tf.translate(r); if (theta.norm() > 0) tf.rotate(Eigen::AngleAxisd(theta.norm(), theta.normalized())); } void randomize_transforms(common::aligned_vector& tfs) { for (std::size_t i = 0; i < tfs.size(); ++i) { randomize_transform(tfs[i]); } } void compute_spatial_velocity( const Eigen::Vector6d& v_parent, const Eigen::Vector6d& v_relative, const Eigen::Isometry3d& tf_rel, Eigen::Vector6d& v_child) { v_child = math::AdInvT(tf_rel, v_parent) + v_relative; } void compute_spatial_acceleration( const Eigen::Vector6d& a_parent, const Eigen::Vector6d& a_relative, const Eigen::Vector6d& v_child, const Eigen::Vector6d& v_rel, const Eigen::Isometry3d& tf_rel, Eigen::Vector6d& a_child) { a_child = math::AdInvT(tf_rel, a_parent) + a_relative + math::ad(v_child, v_rel); } void compute_velocity( const Eigen::Vector3d& v_parent, const Eigen::Vector3d& w_parent, const Eigen::Vector3d& v_rel, const Eigen::Vector3d& w_rel, const Eigen::Vector3d& offset, const Eigen::Isometry3d& tf_parent, Eigen::Vector3d& v_child, Eigen::Vector3d& w_child) { const Eigen::Matrix3d& R = tf_parent.rotation(); v_child = v_parent + R * v_rel + w_parent.cross(R * offset); w_child = w_parent + R * w_rel; } void compute_acceleration( const Eigen::Vector3d& a_parent, const Eigen::Vector3d& alpha_parent, const Eigen::Vector3d& w_parent, const Eigen::Vector3d& a_rel, const Eigen::Vector3d& alpha_rel, const Eigen::Vector3d& v_rel, const Eigen::Vector3d& w_rel, const Eigen::Vector3d& offset, const Eigen::Isometry3d& tf_parent, Eigen::Vector3d& a_child, Eigen::Vector3d& alpha_child) { const Eigen::Matrix3d& R = tf_parent.rotation(); a_child = a_parent + R * a_rel + alpha_parent.cross(R * offset) + 2 * w_parent.cross(R * v_rel) + w_parent.cross(w_parent.cross(R * offset)); alpha_child = alpha_parent + R * alpha_rel + w_parent.cross(R * w_rel); } TEST(FRAMES, FORWARD_KINEMATICS_CHAIN) { std::vector frames; double tolerance = 1e-6; SimpleFrame A(Frame::World(), "A"); frames.push_back(&A); SimpleFrame B(&A, "B"); frames.push_back(&B); SimpleFrame C(&B, "C"); frames.push_back(&C); SimpleFrame D(&C, "D"); frames.push_back(&D); // -- Test Position -------------------------------------------------------- EXPECT_TRANSFORM_NEAR( D.getTransform(), Eigen::Isometry3d::Identity(), tolerance); common::aligned_vector tfs; tfs.resize(frames.size(), Eigen::Isometry3d::Identity()); randomize_transforms(tfs); for (std::size_t i = 0; i < frames.size(); ++i) { SimpleFrame* F = frames[i]; F->setRelativeTransform(tfs[i]); } for (std::size_t i = 0; i < frames.size(); ++i) { Frame* F = frames[i]; Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity()); for (std::size_t j = 0; j <= i; ++j) { expectation = expectation * tfs[j]; } Eigen::Isometry3d actual = F->getTransform(); EXPECT_TRANSFORM_NEAR(actual, expectation, tolerance); } randomize_transforms(tfs); for (std::size_t i = 0; i < frames.size(); ++i) { SimpleFrame* F = frames[i]; F->setRelativeTransform(tfs[i]); } Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity()); for (std::size_t j = 0; j < frames.size(); ++j) expectation = expectation * tfs[j]; EXPECT_TRANSFORM_NEAR(frames.back()->getTransform(), expectation, tolerance); // -- Test Velocity -------------------------------------------------------- // Basic forward spatial velocity propagation { // The brackets are to allow reusing variable names common::aligned_vector v_rels(frames.size()); common::aligned_vector v_total(frames.size()); for (std::size_t i = 0; i < frames.size(); ++i) { v_rels[i] = random_vec<6>(); SimpleFrame* F = frames[i]; F->setRelativeSpatialVelocity(v_rels[i]); if (i > 0) compute_spatial_velocity( v_total[i - 1], v_rels[i], F->getRelativeTransform(), v_total[i]); else compute_spatial_velocity( Eigen::Vector6d::Zero(), v_rels[i], F->getRelativeTransform(), v_total[i]); } for (std::size_t i = 0; i < frames.size(); ++i) { SimpleFrame* F = frames[i]; Eigen::Vector6d v_actual = F->getSpatialVelocity(); EXPECT_VECTOR_DOUBLE_EQ(v_total[i], v_actual); } } // Testing conversion beteween spatial and classical velocities { std::vector v_rels(frames.size()); std::vector w_rels(frames.size()); std::vector v_total(frames.size()); std::vector w_total(frames.size()); for (std::size_t i = 0; i < frames.size(); ++i) { v_rels[i] = random_vec<3>(); w_rels[i] = random_vec<3>(); SimpleFrame* F = frames[i]; F->setClassicDerivatives(v_rels[i], w_rels[i]); Eigen::Vector3d offset = F->getRelativeTransform().translation(); Eigen::Isometry3d tf = i > 0 ? frames[i - 1]->getTransform() : Eigen::Isometry3d::Identity(); if (i > 0) compute_velocity( v_total[i - 1], w_total[i - 1], v_rels[i], w_rels[i], offset, tf, v_total[i], w_total[i]); else compute_velocity( Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), v_rels[i], w_rels[i], offset, tf, v_total[i], w_total[i]); } for (std::size_t i = 0; i < frames.size(); ++i) { SimpleFrame* F = frames[i]; Eigen::Vector3d v_actual = F->getLinearVelocity(); Eigen::Vector3d w_actual = F->getAngularVelocity(); EXPECT_VECTOR_NEAR(v_total[i], v_actual, tolerance); EXPECT_VECTOR_NEAR(w_total[i], w_actual, tolerance); } } // -- Acceleration --------------------------------------------------------- // Basic forward spatial acceleration propagation { common::aligned_vector v_rels(frames.size()); common::aligned_vector a_rels(frames.size()); common::aligned_vector v_total(frames.size()); common::aligned_vector a_total(frames.size()); for (std::size_t i = 0; i < frames.size(); ++i) { v_rels[i] = random_vec<6>(); a_rels[i] = random_vec<6>(); SimpleFrame* F = frames[i]; F->setRelativeSpatialVelocity(v_rels[i]); F->setRelativeSpatialAcceleration(a_rels[i]); Eigen::Isometry3d tf = F->getRelativeTransform(); if (i > 0) { compute_spatial_velocity(v_total[i - 1], v_rels[i], tf, v_total[i]); compute_spatial_acceleration( a_total[i - 1], a_rels[i], v_total[i], v_rels[i], tf, a_total[i]); } else { compute_spatial_velocity( Eigen::Vector6d::Zero(), v_rels[i], tf, v_total[i]); compute_spatial_acceleration( Eigen::Vector6d::Zero(), a_rels[i], v_total[i], v_rels[i], tf, a_total[i]); } } for (std::size_t i = 0; i < frames.size(); ++i) { SimpleFrame* F = frames[i]; Eigen::Vector6d a_actual = F->getSpatialAcceleration(); EXPECT_VECTOR_DOUBLE_EQ(a_total[i], a_actual); } // Test relative computations for (std::size_t i = 0; i < frames.size(); ++i) { Frame* F = frames[i]; Frame* P = F->getParentFrame(); Eigen::Vector6d v_rel = F->getSpatialVelocity(P, F); Eigen::Vector6d a_rel = F->getSpatialAcceleration(P, F); EXPECT_VECTOR_NEAR(v_rels[i], v_rel, tolerance); EXPECT_VECTOR_NEAR(a_rels[i], a_rel, tolerance); } // Test offset computations for (std::size_t i = 0; i < frames.size(); ++i) { Eigen::Vector3d offset = random_vec<3>(); Frame* F = frames[i]; Eigen::Vector3d v_actual = F->getLinearVelocity(offset); Eigen::Vector3d w_actual = F->getAngularVelocity(); Eigen::Vector3d v_expect, w_expect; compute_velocity( F->getLinearVelocity(), F->getAngularVelocity(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), offset, F->getWorldTransform(), v_expect, w_expect); EXPECT_VECTOR_DOUBLE_EQ(v_expect, v_actual); EXPECT_VECTOR_DOUBLE_EQ(w_expect, w_actual); Eigen::Vector3d a_actual = F->getLinearAcceleration(offset); Eigen::Vector3d alpha_actual = F->getAngularAcceleration(); Eigen::Vector3d a_expect, alpha_expect; compute_acceleration( F->getLinearAcceleration(), F->getAngularAcceleration(), F->getAngularVelocity(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), offset, F->getWorldTransform(), a_expect, alpha_expect); EXPECT_VECTOR_DOUBLE_EQ(a_expect, a_actual); EXPECT_VECTOR_DOUBLE_EQ(alpha_expect, alpha_actual); } } // Testing conversion between spatial and classical accelerations { std::vector v_rels(frames.size()); std::vector w_rels(frames.size()); std::vector a_rels(frames.size()); std::vector alpha_rels(frames.size()); std::vector v_total(frames.size()); std::vector w_total(frames.size()); std::vector a_total(frames.size()); std::vector alpha_total(frames.size()); for (std::size_t i = 0; i < frames.size(); ++i) { v_rels[i] = random_vec<3>(); w_rels[i] = random_vec<3>(); a_rels[i] = random_vec<3>(); alpha_rels[i] = random_vec<3>(); SimpleFrame* F = frames[i]; Eigen::Isometry3d rel_tf; randomize_transform(rel_tf); F->setRelativeTransform(rel_tf); F->setClassicDerivatives(v_rels[i], w_rels[i], a_rels[i], alpha_rels[i]); Eigen::Vector3d offset = F->getRelativeTransform().translation(); Eigen::Isometry3d tf = i > 0 ? frames[i - 1]->getTransform() : Eigen::Isometry3d::Identity(); if (i > 0) { compute_velocity( v_total[i - 1], w_total[i - 1], v_rels[i], w_rels[i], offset, tf, v_total[i], w_total[i]); compute_acceleration( a_total[i - 1], alpha_total[i - 1], w_total[i - 1], a_rels[i], alpha_rels[i], v_rels[i], w_rels[i], offset, tf, a_total[i], alpha_total[i]); } else { compute_velocity( Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), v_rels[i], w_rels[i], offset, tf, v_total[i], w_total[i]); compute_acceleration( Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), a_rels[i], alpha_rels[i], v_rels[i], w_rels[i], offset, tf, a_total[i], alpha_total[i]); } } for (std::size_t i = 0; i < frames.size(); ++i) { SimpleFrame* F = frames[i]; Eigen::Vector3d v_actual = F->getLinearVelocity(); Eigen::Vector3d w_actual = F->getAngularVelocity(); Eigen::Vector3d a_actual = F->getLinearAcceleration(); Eigen::Vector3d alpha_actual = F->getAngularAcceleration(); EXPECT_VECTOR_NEAR(v_total[i], v_actual, tolerance); EXPECT_VECTOR_NEAR(w_total[i], w_actual, tolerance); EXPECT_VECTOR_NEAR(a_total[i], a_actual, tolerance); EXPECT_VECTOR_NEAR(alpha_total[i], alpha_actual, tolerance); } // Test relative computations for (std::size_t i = 0; i < frames.size(); ++i) { SimpleFrame* F = frames[i]; Frame* P = F->getParentFrame(); Eigen::Vector3d v_rel = F->getLinearVelocity(P, P); Eigen::Vector3d w_rel = F->getAngularVelocity(P, P); Eigen::Vector3d a_rel = F->getLinearAcceleration(P, P); Eigen::Vector3d alpha_rel = F->getAngularAcceleration(P, P); EXPECT_VECTOR_NEAR(v_rels[i], v_rel, tolerance); EXPECT_VECTOR_NEAR(w_rels[i], w_rel, tolerance); EXPECT_VECTOR_NEAR(a_rels[i], a_rel, tolerance); EXPECT_VECTOR_NEAR(alpha_rels[i], alpha_rel, tolerance); } } } void randomize_target_values( const std::vector& targets, bool spatial) { for (std::size_t i = 0; i < targets.size(); ++i) { SimpleFrame* T = targets[i]; Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); randomize_transform(tf, 1, 2 * dart::math::constantsd::pi()); if (spatial) { T->setRelativeTransform(tf); T->setRelativeSpatialVelocity(random_vec<6>(1)); T->setRelativeSpatialAcceleration(random_vec<6>(1)); } else { T->setRelativeTransform(tf); T->setClassicDerivatives( random_vec<3>(1), random_vec<3>(1), random_vec<3>(1), random_vec<3>(1)); } } } void set_relative_values( const std::vector& targets, const std::vector& followers, bool spatial) { for (std::size_t i = 0; i < targets.size(); ++i) { Frame* T = targets[i]; SimpleFrame* F = followers[i]; Frame* P = F->getParentFrame(); F->setRelativeTransform(T->getTransform(P)); if (spatial) { F->setRelativeSpatialVelocity(T->getSpatialVelocity(P, F)); F->setRelativeSpatialAcceleration(T->getSpatialAcceleration(P, F)); } else { F->setClassicDerivatives( T->getLinearVelocity(P, P), T->getAngularVelocity(P, P), T->getLinearAcceleration(P, P), T->getAngularAcceleration(P, P)); } } } void check_world_values( const std::vector& targets, const std::vector& followers, double tolerance) { for (std::size_t i = 0; i < targets.size(); ++i) { Frame* T = targets[i]; Frame* F = followers[i]; const Eigen::Isometry3d& tf_error = T->getWorldTransform() * F->getWorldTransform().inverse(); Eigen::Vector6d error; Eigen::AngleAxisd rot_error(tf_error.rotation()); error.block<3, 1>(0, 0) = rot_error.angle() * rot_error.axis(); error.block<3, 1>(3, 0) = tf_error.translation(); EXPECT_TRUE(error.norm() < tolerance); error.block<3, 1>(0, 0) = rot_error.angle() * rot_error.axis(); error.block<3, 1>(3, 0) = tf_error.translation(); EXPECT_TRUE(error.norm() < tolerance); EXPECT_VECTOR_NEAR( T->getSpatialVelocity(), F->getSpatialVelocity(), tolerance); EXPECT_VECTOR_NEAR( T->getLinearVelocity(), F->getLinearVelocity(), tolerance); EXPECT_VECTOR_NEAR( T->getAngularVelocity(), F->getAngularVelocity(), tolerance); EXPECT_VECTOR_NEAR( T->getSpatialAcceleration(), F->getSpatialAcceleration(), tolerance); EXPECT_VECTOR_NEAR( T->getLinearAcceleration(), F->getLinearAcceleration(), tolerance); } } void check_values( const std::vector& targets, const std::vector& followers, const Frame* relativeTo, const Frame* inCoordinatesOf, double tolerance) { for (std::size_t i = 0; i < targets.size(); ++i) { Frame* T = targets[i]; Frame* F = followers[i]; const Eigen::Isometry3d& tf_error = T->getTransform(relativeTo) * F->getTransform(relativeTo).inverse(); Eigen::Vector6d error; Eigen::AngleAxisd rot_error(tf_error.rotation()); error.block<3, 1>(0, 0) = rot_error.angle() * rot_error.axis(); error.block<3, 1>(3, 0) = tf_error.translation(); EXPECT_TRUE(error.norm() < tolerance); EXPECT_VECTOR_NEAR( T->getSpatialVelocity(relativeTo, inCoordinatesOf), F->getSpatialVelocity(relativeTo, inCoordinatesOf), tolerance); EXPECT_VECTOR_NEAR( T->getLinearVelocity(relativeTo, inCoordinatesOf), F->getLinearVelocity(relativeTo, inCoordinatesOf), tolerance); EXPECT_VECTOR_NEAR( T->getAngularVelocity(relativeTo, inCoordinatesOf), F->getAngularVelocity(relativeTo, inCoordinatesOf), tolerance); EXPECT_VECTOR_NEAR( T->getSpatialAcceleration(relativeTo, inCoordinatesOf), F->getSpatialAcceleration(relativeTo, inCoordinatesOf), tolerance); EXPECT_VECTOR_NEAR( T->getLinearAcceleration(relativeTo, inCoordinatesOf), F->getLinearAcceleration(relativeTo, inCoordinatesOf), tolerance); EXPECT_VECTOR_NEAR( T->getAngularAcceleration(relativeTo, inCoordinatesOf), F->getAngularAcceleration(relativeTo, inCoordinatesOf), tolerance); } } void check_offset_computations( const std::vector& targets, const std::vector& followers, const Frame* relativeTo, const Frame* inCoordinatesOf, double tolerance) { for (std::size_t i = 0; i < targets.size(); ++i) { Frame* T = targets[i]; Frame* F = followers[i]; Eigen::Isometry3d coordTf = relativeTo->getTransform(inCoordinatesOf); Vector3d offset_T = random_vec<3>(); Vector3d offset_F = T->getTransform(F) * offset_T; Vector3d v_TO, w_TO, v_FO, w_FO, a_TO, alpha_TO, a_FO, alpha_FO; // Compute velocity of the offfset in the relative frame compute_velocity( T->getLinearVelocity(relativeTo, relativeTo), T->getAngularVelocity(relativeTo, relativeTo), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), offset_T, T->getTransform(relativeTo), v_TO, w_TO); // Convert to the desired coordinate system v_TO = coordTf.linear() * v_TO; w_TO = coordTf.linear() * w_TO; // Compute acceleration of the offset in the relative frame compute_acceleration( T->getLinearAcceleration(relativeTo, relativeTo), T->getAngularAcceleration(relativeTo, relativeTo), T->getAngularVelocity(relativeTo, relativeTo), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), offset_T, T->getTransform(relativeTo), a_TO, alpha_TO); // Convert to the desired coordinate system a_TO = coordTf.linear() * a_TO; alpha_TO = coordTf.linear() * alpha_TO; compute_velocity( F->getLinearVelocity(relativeTo, relativeTo), F->getAngularVelocity(relativeTo, relativeTo), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), offset_F, F->getTransform(relativeTo), v_FO, w_FO); v_FO = coordTf.linear() * v_FO; w_FO = coordTf.linear() * w_FO; compute_acceleration( F->getLinearAcceleration(relativeTo, relativeTo), F->getAngularAcceleration(relativeTo, relativeTo), F->getAngularVelocity(relativeTo, relativeTo), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), offset_F, F->getTransform(relativeTo), a_FO, alpha_FO); a_FO = coordTf.linear() * a_FO; alpha_FO = coordTf.linear() * alpha_FO; Eigen::Vector3d v_TO_actual = T->getLinearVelocity(offset_T, relativeTo, inCoordinatesOf); Eigen::Vector3d v_FO_actual = F->getLinearVelocity(offset_F, relativeTo, inCoordinatesOf); EXPECT_VECTOR_NEAR(v_TO, v_FO, tolerance); EXPECT_VECTOR_NEAR(v_TO, v_TO_actual, tolerance); EXPECT_VECTOR_NEAR(v_FO, v_FO_actual, tolerance); Eigen::Vector3d a_TO_actual = T->getLinearAcceleration(offset_T, relativeTo, inCoordinatesOf); Eigen::Vector3d a_FO_actual = F->getLinearAcceleration(offset_F, relativeTo, inCoordinatesOf); EXPECT_VECTOR_NEAR(a_TO, a_FO, tolerance); EXPECT_VECTOR_NEAR(a_TO, a_TO_actual, tolerance); EXPECT_VECTOR_NEAR(a_FO, a_FO_actual, tolerance); } } void test_relative_values(bool spatial_targets, bool spatial_followers) { const double tolerance = 1e-8; // These frames will form a chain SimpleFrame A(Frame::World(), "A"); SimpleFrame B(&A, "B"); SimpleFrame C(&B, "C"); SimpleFrame D(&C, "D"); std::vector targets; targets.push_back(&A); targets.push_back(&B); targets.push_back(&C); targets.push_back(&D); // R will be an arbitrary reference frame SimpleFrame R(Frame::World(), "R"); targets.push_back(&R); // Each of these frames will attempt to track one of the frames in the chain // with respect to the frame R. SimpleFrame AR(&R, "AR"); SimpleFrame BR(&R, "BR"); SimpleFrame CR(&R, "CR"); SimpleFrame DR(&R, "DR"); SimpleFrame RR(&R, "RR"); std::vector followers; followers.push_back(&AR); followers.push_back(&BR); followers.push_back(&CR); followers.push_back(&DR); followers.push_back(&RR); assert(targets.size() == followers.size()); randomize_target_values(targets, spatial_targets); set_relative_values(targets, followers, spatial_followers); check_world_values(targets, followers, tolerance); // Check every combination of relative values for (std::size_t i = 0; i < targets.size(); ++i) { Frame* T = targets[i]; for (std::size_t j = 0; j < followers.size(); ++j) { Frame* F = followers[j]; check_values(targets, followers, T, F, tolerance); check_values(targets, followers, F, T, tolerance); check_offset_computations(targets, followers, T, F, tolerance); } } // Test SimpleFrame::setTransform() for (std::size_t i = 0; i < followers.size(); ++i) { for (std::size_t j = 0; j < followers.size(); ++j) { SimpleFrame* F = followers[i]; SimpleFrame* T = followers[j]; Eigen::Isometry3d tf; randomize_transform(tf, 1, 2 * dart::math::constantsd::pi()); T->setTransform(tf, F); if (i != j) { EXPECT_TRANSFORM_NEAR(T->getTransform(F), tf, 1e-10); } } } } // Test different combinations of using spatial and classical derivative terms TEST(RELATIVE_FRAMES, SPATIAL_SPATIAL) { test_relative_values(true, true); } TEST(RELATIVE_FRAMES, CLASSIC_CLASSIC) { test_relative_values(false, false); } TEST(RELATIVE_FRAMES, SPATIAL_CLASSIC) { test_relative_values(true, false); } TEST(RELATIVE_FRAMES, CLASSIC_SPATIAL) { test_relative_values(false, true); } TEST(FRAMES, CHILDHOOD) { SimpleFrame F1(Frame::World(), "F1"); SimpleFrame F2(&F1, "F2"); EXPECT_TRUE(F1.getNumChildFrames() == 1); }