From 4389b698374d57f3b299b0d7a6ed3aa245dfa6e9 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 1 Nov 2021 07:33:36 -0700 Subject: [PATCH] Add joint force/torque getter (#1616) --- CHANGELOG.md | 4 + dart/dynamics/Joint.cpp | 46 +++ dart/dynamics/Joint.hpp | 25 ++ data/sdf/test/force_torque_test.world | 173 +++++++++++ unittests/comprehensive/CMakeLists.txt | 3 + .../comprehensive/test_JointForceTorque.cpp | 275 ++++++++++++++++++ 6 files changed, 526 insertions(+) create mode 100644 data/sdf/test/force_torque_test.world create mode 100644 unittests/comprehensive/test_JointForceTorque.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index fe63f710ab715..c17fde5d8a0f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,10 @@ * Remove DART_BUILD_DARTPY option: [#1600](https://github.com/dartsim/dart/pull/1600) +* Dynamics + + * Added joint force/torque getter: [#1616](https://github.com/dartsim/dart/pull/1616) + * Parsers * Added default options to DartLoader for missing properties in URDF files: : [#1605](https://github.com/dartsim/dart/pull/1605) diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 998aa11546b75..d3c6abd5ae389 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -387,6 +387,52 @@ const Eigen::Vector6d& Joint::getRelativePrimaryAcceleration() const return mPrimaryAcceleration; } +//============================================================================== +Eigen::Vector6d Joint::getWrenchToChildBodyNode( + const Frame* withRespectTo) const +{ + const BodyNode* childBodyNode = getChildBodyNode(); + if (!childBodyNode) + { + return Eigen::Vector6d::Zero(); + } + + const Eigen::Vector6d& F2 = childBodyNode->getBodyForce(); + const BodyNode* parentBodyNode = getParentBodyNode(); + + if (withRespectTo == nullptr) + { + // (Default) Wrench applying to the child body node, where the reference + // frame is the joint frame + return math::dAdT(getTransformFromChildBodyNode(), -F2); + } + else if (withRespectTo == childBodyNode) + { + // Wrench applying to the child body node, where the reference frame is the + // child body frame + return -F2; + } + else if (withRespectTo == parentBodyNode) + { + // Wrench applying to the child body node, where the reference frame is the + // parent body frame + return math::dAdInvT(getRelativeTransform(), -F2); + } + else + { + // Wrench applying to the child body node, where the reference frame is an + // arbitrary frame + return math::dAdT(withRespectTo->getTransform(childBodyNode), -F2); + } +} + +//============================================================================== +Eigen::Vector6d Joint::getWrenchToParentBodyNode( + const Frame* withRespectTo) const +{ + return -getWrenchToChildBodyNode(withRespectTo); +} + //============================================================================== void Joint::setPositionLimitEnforced(bool enforced) { diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp index 56b8619ebb841..f50bb38686f31 100644 --- a/dart/dynamics/Joint.hpp +++ b/dart/dynamics/Joint.hpp @@ -41,6 +41,7 @@ #include "dart/common/EmbeddedAspect.hpp" #include "dart/common/Subject.hpp" #include "dart/common/VersionCounter.hpp" +#include "dart/dynamics/Frame.hpp" #include "dart/dynamics/SmartPointer.hpp" #include "dart/dynamics/detail/JointAspect.hpp" #include "dart/math/MathTypes.hpp" @@ -725,6 +726,30 @@ class Joint : public virtual common::Subject, /// \sa BodyNode::updateArticulatedInertia(double). // Eigen::VectorXd getDampingForces() const; + /// Returns wrench exerted to the child body node to satisfy the joint + /// constraint. + /// + /// \param[in] withRespectTo: The reference frame where the wrench is + /// measured. The default (i.e., nullptr) is to get the wrench measured in the + /// joint frame. + /// \return Wrench where the first three elements represent torque and the + /// last three elements represent force. Zero vector if this joint has no + /// child body node defined. + Eigen::Vector6d getWrenchToChildBodyNode( + const Frame* withRespectTo = nullptr) const; + + /// Returns wrench exerted to the parent body node to satisfy the joint + /// constraint. + /// + /// \param[in] withRespectTo: The reference frame where the wrench is + /// measured. The default (i.e., nullptr) is to get the wrench measured in the + /// joint frame. + /// \return Wrench where the first three elements represent torque and the + /// last three elements represent force. Zero vector if this joint has no + /// child body node defined. + Eigen::Vector6d getWrenchToParentBodyNode( + const Frame* withRespectTo = nullptr) const; + //---------------------------------------------------------------------------- /// \{ \name Update Notifiers //---------------------------------------------------------------------------- diff --git a/data/sdf/test/force_torque_test.world b/data/sdf/test/force_torque_test.world new file mode 100644 index 0000000000000..9dc16de1e51d5 --- /dev/null +++ b/data/sdf/test/force_torque_test.world @@ -0,0 +1,173 @@ + + + + + 0.000000 0.000000 -9.810000 + + + quick + 1000 + 0 + 1.000000 + + + 0.000000 + 0.200000 + 100.000000 + 0.01000 + + + + + sequential_impulse + 1000 + 1.000000 + + + 0.000000 + 0.200000 + true + -0.01 + 0.00000 + + + 0.000000 + 0.001 + + + model://ground_plane + + + model://sun + + + + + + + 0 0 0.5 0 0 0 + + 0.100000 + 0.000000 + 0.000000 + 0.100000 + 0.000000 + 0.100000 + + 10.000000 + + + 0 0 0.5 0 0 0 + + + 0.100000 + + + + + 0 0 0.5 0 0 0 + 250 + + + 0.100000 + + + + + + + 0 0 1.5 0 0 0 + + 0 0 0.5 0 0 0 + + 0.100000 + 0.000000 + 0.000000 + 0.100000 + 0.000000 + 0.100000 + + 10.000000 + + + 0 0 0.5 0 0 0 + + + 0.1 0.2 0.4 + + + + + 0 0 0.5 0 0 0 + + + 0.1 0.2 0.4 + + + + + + + world + link_1 + + + 0 0 0.5 0 0 0 + + + -1.57079 + 1.57079 + 1000.000000 + 1000.000000 + + + 0.000000 + 0.000000 + + 1.000000 0.000000 0.000000 + + + true + true + 30 + + + true + + + + + link_1 + link_2 + + + + 0 0 0 0 0 0 + + + -0.000001 + 0.000001 + 1000.000000 + 1000.000000 + + + 0.000000 + 0.000000 + + 0.000000 0.000000 1.000000 + + + true + true + 30 + + + true + + + + + diff --git a/unittests/comprehensive/CMakeLists.txt b/unittests/comprehensive/CMakeLists.txt index b068dce7b0ab1..e4b5e06f966a2 100644 --- a/unittests/comprehensive/CMakeLists.txt +++ b/unittests/comprehensive/CMakeLists.txt @@ -46,6 +46,9 @@ if(TARGET dart-utils) dart_add_test("comprehensive" test_Joints) target_link_libraries(test_Joints dart-utils) + dart_add_test("comprehensive" test_JointForceTorque) + target_link_libraries(test_JointForceTorque dart-utils) + dart_add_test("comprehensive" test_Skeleton) target_link_libraries(test_Skeleton dart-utils) diff --git a/unittests/comprehensive/test_JointForceTorque.cpp b/unittests/comprehensive/test_JointForceTorque.cpp new file mode 100644 index 0000000000000..916779226a167 --- /dev/null +++ b/unittests/comprehensive/test_JointForceTorque.cpp @@ -0,0 +1,275 @@ +/* + * Copyright (c) 2011-2021, 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. + */ + +// The tests in this file are inspired by the tests in: +// https://github.com/osrf/gazebo/blob/01b395a5fa92eb054c72f9a2027cdcfd35f287f4/test/integration/joint_force_torque.cc + +#include + +#include "dart/dart.hpp" +#include "dart/utils/sdf/sdf.hpp" + +#include "TestHelpers.hpp" + +using namespace dart; +using namespace dart::math; +using namespace dart::dynamics; +using namespace dart::simulation; +using namespace dart::utils; + +//============================================================================== +WorldPtr readWorld(const common::Uri& uri) +{ + WorldPtr world = SdfParser::readWorld(uri); + for (auto i = 0u; i < world->getNumSkeletons(); ++i) + { + auto skel = world->getSkeleton(i); + for (auto j = 0u; j < skel->getNumJoints(); ++j) + { + auto joint = skel->getJoint(j); + joint->setLimitEnforcement(true); + } + } + return world; +} + +//============================================================================== +TEST(JointForceTorqueTest, Static) +{ + // Load world + WorldPtr world = readWorld("dart://sample/sdf/test/force_torque_test.world"); + ASSERT_NE(world, nullptr); + + // Check if the world is correct loaded + ASSERT_EQ(world->getNumSkeletons(), 1); + SkeletonPtr model_1 = world->getSkeleton(0); + ASSERT_NE(model_1, nullptr); + EXPECT_EQ(model_1->getName(), "model_1"); + EXPECT_EQ(model_1->getNumBodyNodes(), 2); + EXPECT_EQ(model_1->getNumJoints(), 2); + + world->setGravity(Eigen::Vector3d(0, 0, -50)); + + // Simulate 1 step + world->step(); + double t = world->getTime(); + + // Get time step size + double dt = world->getTimeStep(); + EXPECT_GT(dt, 0); + + // Check that time moves forward + EXPECT_DOUBLE_EQ(t, dt); + + // Get joint and get force torque + auto link_1 = model_1->getBodyNode("link_1"); + ASSERT_NE(link_1, nullptr); + auto link_2 = model_1->getBodyNode("link_2"); + ASSERT_NE(link_2, nullptr); + auto joint_01 = model_1->getJoint("joint_01"); + ASSERT_NE(joint_01, nullptr); + auto joint_12 = model_1->getJoint("joint_12"); + ASSERT_NE(joint_12, nullptr); + + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + + // Run 10 steps + for (auto i = 0u; i < 10; ++i) + { + world->step(); + + //---------------------- + // Test joint_01 wrench + //---------------------- + + // Reference adjustment for different joint frame conventions between Gazebo and DART + tf.setIdentity(); + tf.translation() = joint_01->getTransformFromParentBodyNode().translation(); + SimpleFramePtr parentFrame01 + = std::make_shared(Frame::World(), "parentFrame01", tf); + tf.setIdentity(); + tf.translation() = joint_01->getTransformFromChildBodyNode().translation(); + SimpleFramePtr childFrame01 + = std::make_shared(link_1, "childFrame01", tf); + + const Eigen::Vector6d parentF01 = joint_01->getWrenchToParentBodyNode(parentFrame01.get()); + EXPECT_DOUBLE_EQ(parentF01[0], 0); // torque + EXPECT_DOUBLE_EQ(parentF01[1], 0); + EXPECT_DOUBLE_EQ(parentF01[2], 0); + EXPECT_DOUBLE_EQ(parentF01[3], 0); // force + EXPECT_DOUBLE_EQ(parentF01[4], 0); + EXPECT_DOUBLE_EQ(parentF01[5], 1000); + + const Eigen::Vector6d childF01 = joint_01->getWrenchToChildBodyNode(childFrame01.get()); + EXPECT_EQ(childF01, -parentF01); + + //---------------------- + // Test joint_12 wrench + //---------------------- + + // Reference adjustment for different joint frame conventions between Gazebo and DART + tf.setIdentity(); + tf.translation() = joint_12->getTransformFromParentBodyNode().translation(); + SimpleFramePtr parentFrame12 + = std::make_shared(link_1, "parentFrame12", tf); + tf.setIdentity(); + tf.translation() = joint_12->getTransformFromChildBodyNode().translation(); + SimpleFramePtr childFrame12 + = std::make_shared(link_2, "childFrame12", tf); + + const Eigen::Vector6d parentF12 = joint_12->getWrenchToParentBodyNode(parentFrame12.get()); + EXPECT_DOUBLE_EQ(parentF12[0], 0); // torque + EXPECT_DOUBLE_EQ(parentF12[1], 0); + EXPECT_DOUBLE_EQ(parentF12[2], 0); + EXPECT_DOUBLE_EQ(parentF12[3], 0); // force + EXPECT_DOUBLE_EQ(parentF12[4], 0); + EXPECT_DOUBLE_EQ(parentF12[5], 500); + + const Eigen::Vector6d childF12 = joint_12->getWrenchToChildBodyNode(childFrame01.get()); + EXPECT_EQ(childF12, -parentF12); + } +} + +//============================================================================== +TEST(JointForceTorqueTest, ForceTorqeAtJointLimits) +{ + // Load world + WorldPtr world = readWorld("dart://sample/sdf/test/force_torque_test.world"); + ASSERT_NE(world, nullptr); + + // Check if the world is correct loaded + ASSERT_EQ(world->getNumSkeletons(), 1); + SkeletonPtr model_1 = world->getSkeleton(0); + ASSERT_NE(model_1, nullptr); + EXPECT_EQ(model_1->getName(), "model_1"); + EXPECT_EQ(model_1->getNumBodyNodes(), 2); + EXPECT_EQ(model_1->getNumJoints(), 2); + + world->setGravity(Eigen::Vector3d(0, 0, -50)); + + // Simulate 1 step + world->step(); + double t = world->getTime(); + + // Get time step size + double dt = world->getTimeStep(); + EXPECT_GT(dt, 0); + + // Check that time moves forward + EXPECT_DOUBLE_EQ(t, dt); + + // Get joint and get force torque + auto link_1 = model_1->getBodyNode("link_1"); + ASSERT_NE(link_1, nullptr); + auto link_2 = model_1->getBodyNode("link_2"); + ASSERT_NE(link_2, nullptr); + auto joint_01 = model_1->getJoint("joint_01"); + ASSERT_NE(joint_01, nullptr); + auto joint_12 = model_1->getJoint("joint_12"); + ASSERT_NE(joint_12, nullptr); + + // Change gravity so that the top link topples over, then remeasure + world->setGravity(Eigen::Vector3d(-30, 10, -50)); + + // Wait for dynamics to be stabilized + for (auto i = 0; i < 2000; ++i) + { + world->step(); + } + + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + + // Run 5 steps + for (auto i = 0u; i < 5; ++i) + { + world->step(); + + //---------------------- + // Test joint_01 wrench + //---------------------- + + // Reference adjustment for different joint frame conventions between Gazebo and DART + tf.setIdentity(); + tf.translation() = joint_01->getTransformFromParentBodyNode().translation(); + SimpleFramePtr parentFrame01 + = std::make_shared(Frame::World(), "parentFrame01", tf); + tf.setIdentity(); + tf.translation() = joint_01->getTransformFromChildBodyNode().translation(); + SimpleFramePtr childFrame01 + = std::make_shared(link_1, "childFrame01", tf); + + const Eigen::Vector6d parentF01 + = joint_01->getWrenchToParentBodyNode(parentFrame01.get()); + EXPECT_NEAR(parentF01[0], 750, 7.5); // torque + EXPECT_NEAR(parentF01[1], 0, 4.5); + EXPECT_NEAR(parentF01[2], -450, 0.1); + EXPECT_NEAR(parentF01[3], 600, 6); // force + EXPECT_NEAR(parentF01[4], -200, 10); + EXPECT_NEAR(parentF01[5], 1000, 2); + + const Eigen::Vector6d childF01 + = joint_01->getWrenchToChildBodyNode(childFrame01.get()); + EXPECT_NEAR(childF01[0], -750, 7.5); // torque + EXPECT_NEAR(childF01[1], -450, 4.5); + EXPECT_NEAR(childF01[2], 0, 0.1); + EXPECT_NEAR(childF01[3], -600, 6); // force + EXPECT_NEAR(childF01[4], 1000, 10); + EXPECT_NEAR(childF01[5], 200, 2); + + //---------------------- + // Test joint_12 wrench + //---------------------- + + // Reference adjustment for different joint frame conventions between Gazebo and DART + tf.setIdentity(); + tf.translation() = joint_12->getTransformFromParentBodyNode().translation(); + SimpleFramePtr parentFrame12 + = std::make_shared(link_1, "parentFrame12", tf); + tf.setIdentity(); + tf.translation() = joint_12->getTransformFromChildBodyNode().translation(); + SimpleFramePtr childFrame12 + = std::make_shared(link_2, "childFrame12", tf); + + const Eigen::Vector6d parentF12 + = joint_12->getWrenchToParentBodyNode(parentFrame12.get()); + EXPECT_NEAR(parentF12[0], 250, 2.5); // torque + EXPECT_NEAR(parentF12[1], 150, 1.5); + EXPECT_NEAR(parentF12[2], 0, 0.1); + EXPECT_NEAR(parentF12[3], 300, 3); // force + EXPECT_NEAR(parentF12[4], -500, 5); + EXPECT_NEAR(parentF12[5], -100, 1); + + const Eigen::Vector6d childF12 + = joint_12->getWrenchToChildBodyNode(childFrame12.get()); + EXPECT_TRUE(childF12.isApprox(-parentF12)); + } +}