From e9c5b3d1a05868284bad14434f13ca3cbf2f7caf Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 20 Sep 2021 17:29:09 -0500 Subject: [PATCH 1/3] And RelativeWrench, rename to JointTransmittedWrench Signed-off-by: Addisu Z. Taddese --- dartsim/src/JointFeatures.cc | 27 ++ dartsim/src/JointFeatures.hh | 7 +- dartsim/src/JointFeatures_TEST.cc | 336 ++++++++++++++++++ dartsim/worlds/pendulum_joint_wrench.sdf | 262 ++++++++++++++ include/ignition/physics/Geometry.hh | 8 + include/ignition/physics/Joint.hh | 51 +++ include/ignition/physics/RelativeQuantity.hh | 7 + include/ignition/physics/detail/Joint.hh | 25 ++ .../physics/detail/RelativeQuantity.hh | 78 ++++ test/Utils.hh | 13 + 10 files changed, 813 insertions(+), 1 deletion(-) create mode 100644 dartsim/worlds/pendulum_joint_wrench.sdf diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 6f410ef7c..8f5ec221d 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -645,6 +645,33 @@ Identity JointFeatures::AttachPrismaticJoint( return this->GenerateIdentity(jointID, this->joints.at(jointID)); } +///////////////////////////////////////////////// +Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( + const Identity &_id) const +{ + auto &joint = this->ReferenceInterface(_id)->joint; + auto *childBn = joint->getChildBodyNode(); + if (nullptr == childBn) + { + ignerr + << "Joint [" << joint->getName() + << "] does not have a child link. Unable to get transmitted wrench.\n"; + return {}; + } + + const Eigen::Vector6d transmittedWrenchInBody = childBn->getBodyForce(); + // C - Child body frame + // J - Joint frame + // X_CJ - Pose of joint in child body frame + const Eigen::Isometry3d X_CJ = joint->getTransformFromChildBodyNode(); + + const Eigen::Vector6d transmittedWrenchInJoint = + dart::math::dAdT(X_CJ, transmittedWrenchInBody); + Wrench3d wrenchOut; + wrenchOut.torque = transmittedWrenchInJoint.head<3>(); + wrenchOut.force = transmittedWrenchInJoint.tail<3>(); + return wrenchOut; +} } } } diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh index 3cdb783fc..876c548fc 100644 --- a/dartsim/src/JointFeatures.hh +++ b/dartsim/src/JointFeatures.hh @@ -55,7 +55,8 @@ struct JointFeatureList : FeatureList< SetJointVelocityCommandFeature, SetJointPositionLimitsFeature, SetJointVelocityLimitsFeature, - SetJointEffortLimitsFeature + SetJointEffortLimitsFeature, + GetJointTransmittedWrench > { }; class JointFeatures : @@ -198,6 +199,10 @@ class JointFeatures : public: void SetJointMaxEffort( const Identity &_id, const std::size_t _dof, const double _value) override; + + // ----- Transmitted wrench ----- + public: Wrench3d GetJointTransmittedWrenchInJointFrame( + const Identity &_id) const override; }; } diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 86b9920ab..7ec00bae4 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -27,10 +27,13 @@ #include #include +#include #include // Features +#include #include +#include #include #include #include @@ -45,6 +48,7 @@ #include #include +#include "ignition/physics/Geometry.hh" #include "test/Utils.hh" using namespace ignition; @@ -56,8 +60,12 @@ using TestFeatureList = ignition::physics::FeatureList< physics::SetJointTransformFromParentFeature, physics::ForwardStep, physics::FreeJointCast, + physics::SetFreeGroupWorldPose, physics::GetBasicJointState, physics::GetEntities, + physics::GetJointTransmittedWrench, + physics::JointFrameSemantics, + physics::LinkFrameSemantics, physics::RevoluteJointCast, physics::SetBasicJointState, physics::SetJointVelocityCommandFeature, @@ -1056,6 +1064,334 @@ TEST_F(JointFeaturesFixture, JointAttachDetachSpawnedModel) EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); } +class JointTransmittedWrenchFixture : public JointFeaturesFixture +{ + public: using WorldPtr = physics::World3dPtr; + public: using ModelPtr = physics::Model3dPtr; + public: using JointPtr = physics::Joint3dPtr; + public: using LinkPtr = physics::Link3dPtr; + public: using Vector3d = physics::Vector3d; + public: using Wrench3d = physics::Wrench3d; + + protected: void SetUp() override + { + JointFeaturesFixture::SetUp(); + sdf::Root root; + const sdf::Errors errors = + root.Load(TEST_WORLD_DIR "pendulum_joint_wrench.sdf"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + this->world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, this->world); + + this->model = this->world->GetModel("pendulum"); + ASSERT_NE(nullptr, this->model); + this->motorJoint = this->model->GetJoint("motor_joint"); + ASSERT_NE(nullptr, this->motorJoint); + this->sensorJoint = this->model->GetJoint("sensor_joint"); + ASSERT_NE(nullptr, this->sensorJoint); + this->armLink = this->model->GetLink("arm"); + ASSERT_NE(nullptr, this->armLink); + } + + public: void Step(int _iters) + { + for (int i = 0; i < _iters; ++i) + { + this->world->Step(this->output, this->state, this->input); + } + } + + public: physics::ForwardStep::Output output; + public: physics::ForwardStep::State state; + public: physics::ForwardStep::Input input; + public: WorldPtr world; + public: ModelPtr model; + public: JointPtr motorJoint; + public: JointPtr sensorJoint; + public: LinkPtr armLink; + + // From SDFormat file + static constexpr double kGravity = 9.8; + static constexpr double kArmLinkMass = 6.0; + static constexpr double kSensorLinkMass = 0.4; + // MOI in the z-axis + static constexpr double kSensorLinkMOI = 0.02; + static constexpr double kArmLength = 1.0; +}; + +TEST_F(JointTransmittedWrenchFixture , PendulumAtZeroAngle) +{ + namespace test = physics::test; + + // Run a few steps for the constraint forces to stabilize + this->Step(10); + + // Test wrench expressed in different frames + { + auto wrenchAtMotorJoint = this->motorJoint->GetTransmittedWrench(); + Wrench3d expectedWrenchAtMotorJoint{ + Vector3d::Zero(), {-kGravity * (kArmLinkMass + kSensorLinkMass), 0, 0}}; + + EXPECT_TRUE( + test::Equal(expectedWrenchAtMotorJoint, wrenchAtMotorJoint, 1e-4)); + } + { + auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( + this->motorJoint->GetFrameID(), physics::FrameID::World()); + Wrench3d expectedWrenchAtMotorJointInWorld{ + Vector3d::Zero(), {0, 0, kGravity * (kArmLinkMass + kSensorLinkMass)}}; + + EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInWorld, + wrenchAtMotorJointInWorld, 1e-4)); + } + { + auto wrenchAtMotorJointInArm = this->motorJoint->GetTransmittedWrench( + this->armLink->GetFrameID(), this->armLink->GetFrameID()); + // The arm frame is rotated by 90° in the Y-axis of the joint frame. + Wrench3d expectedWrenchAtMotorJointInArm{ + Vector3d::Zero(), {0, 0, kGravity * (kArmLinkMass + kSensorLinkMass)}}; + + EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInArm, + wrenchAtMotorJointInArm, 1e-4)); + } +} + +TEST_F(JointTransmittedWrenchFixture, PendulumInMotion) +{ + namespace test = physics::test; + // Start pendulum at 90° (parallel to the ground) and stop when its around 40° + // so that we have non-trivial test expectations. + this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); + this->Step(350); + + // Given the position (θ), velocity (ω), and acceleration (α) of the joint + // and distance from the joint to the COM (r), we can the reaction forces in + // the tangent direction (Ft) and normal direction (Fn) are given by: + // + // Ft = m * α * r - (m * g * sin(θ)) = m * (α * r - g * sin(θ)) + // Fn = m * ω² * r + (m * g * cos(θ)) = m * (ω² * r + g * cos(θ)) + { + const double theta = this->motorJoint->GetPosition(0); + const double omega = this->motorJoint->GetVelocity(0); + // In order to get the math to work out, we need to use the joint + // acceleration and transmitted wrench from the current time step with the + // joint position and velocity from the previous time step. That is, we need + // the position and velocity before they are integrated. + this->Step(1); + const double alpha = this->motorJoint->GetAcceleration(0); + + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + + const double armTangentForce = + kArmLinkMass * ((-alpha * kArmLength / 2.0) - (kGravity * sin(theta))); + + const double motorLinkTangentForce = + -kSensorLinkMass * kGravity * sin(theta); + + const double armNormalForce = + kArmLinkMass * + ((std::pow(omega, 2) * kArmLength / 2.0) + (kGravity * cos(theta))); + + const double motorLinkNormalForce = kSensorLinkMass * kGravity * cos(theta); + + const double tangentForce = armTangentForce + motorLinkTangentForce; + const double normalForce = armNormalForce + motorLinkNormalForce; + + // The orientation of the joint frame is such that the normal force is + // parallel to the x-axis and the tangent force is parallel to the y-axis. + Wrench3d expectedWrenchAtMotorJointInJoint{ + Vector3d::Zero(), {-normalForce, -tangentForce, 0}}; + + EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInJoint, + wrenchAtMotorJointInJoint, 1e-4)); + } + + // Test Wrench expressed in different frames + { + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + // This is just a rotation of the wrench to be expressed in the world's + // coordinate frame + auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( + this->motorJoint->GetFrameID(), physics::FrameID::World()); + // The joint frame is rotated by 90° along the world's y-axis + Eigen::Quaterniond R_WJ = + Eigen::AngleAxisd(IGN_PI_2, Eigen::Vector3d(0, 1, 0)) * + Eigen::AngleAxisd(this->motorJoint->GetPosition(0), + Eigen::Vector3d(0, 0, 1)); + + Wrench3d expectedWrenchAtMotorJointInWorld{ + Vector3d::Zero(), R_WJ * wrenchAtMotorJointInJoint.force}; + EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInWorld, + wrenchAtMotorJointInWorld, 1e-4)); + + // This moves the point of application as well change the coordinate frame + Wrench3d wrenchAtArmInArm = this->motorJoint->GetTransmittedWrench( + armLink->GetFrameID(), armLink->GetFrameID()); + + // Notation: arm link (A), joint (J) + Eigen::Isometry3d X_AJ; + // Pose of joint (J) in arm link (A) as specified in the SDFormat file. + X_AJ = Eigen::AngleAxisd(IGN_PI_2, Eigen::Vector3d(0, 1, 0)); + X_AJ.translation() = Vector3d(0, 0, kArmLength / 2.0); + Wrench3d expectedWrenchAtArmInArm; + + expectedWrenchAtArmInArm.force = + X_AJ.linear() * wrenchAtMotorJointInJoint.force; + + expectedWrenchAtArmInArm.torque = + X_AJ.linear() * wrenchAtMotorJointInJoint.torque + + X_AJ.translation().cross(expectedWrenchAtArmInArm.force); + + EXPECT_TRUE(test::Equal(expectedWrenchAtArmInArm, wrenchAtArmInArm, 1e-4)); + } +} + +// Compare wrench at the motor joint with wrench from the sensor joint (a +// fixed joint measuring only constraint forces). +TEST_F(JointTransmittedWrenchFixture, ValidateWrenchWithSecondaryJoint) +{ + namespace test = physics::test; + // Start pendulum at 90° (parallel to the ground) and stop when its around 40° + // so that we have non-trivial test expectations. + this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); + this->Step(350); + const double theta = this->motorJoint->GetPosition(0); + // In order to get the math to work out, we need to use the joint + // acceleration and transmitted wrench from the current time step with the + // joint position and velocity from the previous time step. That is, we need + // the position and velocity before they are integrated. + this->Step(1); + const double alpha = this->motorJoint->GetAcceleration(0); + + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + auto wrenchAtSensorInSensor = this->sensorJoint->GetTransmittedWrench(); + + // Since sensor_link has moment of inertia, the fixed joint will transmit a + // torque necessary to rotate the sensor. This is not detected by the motor + // joint because no force is transmitted along the revolute axis. On the + // other hand, the mass of sensor_link will contribute to the constraint + // forces on the motor joint, but these won't be detected by the sensor + // joint. + Vector3d expectedTorqueDiff{0, 0, kSensorLinkMOI * alpha}; + Vector3d expectedForceDiff{-kSensorLinkMass * kGravity * cos(theta), + kSensorLinkMass * kGravity * sin(theta), 0}; + + Vector3d torqueDiff = + wrenchAtMotorJointInJoint.torque - wrenchAtSensorInSensor.torque; + Vector3d forceDiff = + wrenchAtMotorJointInJoint.force - wrenchAtSensorInSensor.force; + EXPECT_TRUE(test::Equal(expectedTorqueDiff, torqueDiff, 1e-4)); + EXPECT_TRUE(test::Equal(expectedForceDiff, forceDiff, 1e-4)); +} + +// Check that the transmitted wrench is affected by joint friction, stiffness +// and damping +TEST_F(JointTransmittedWrenchFixture, JointLosses) +{ + // Get DART joint pointer to set joint friction, damping, etc. + auto dartWorld = this->world->GetDartsimWorld(); + ASSERT_NE(nullptr, dartWorld); + auto dartModel = dartWorld->getSkeleton(this->model->GetIndex()); + ASSERT_NE(nullptr, dartModel); + auto dartJoint = dartModel->getJoint(this->motorJoint->GetIndex()); + ASSERT_NE(nullptr, dartJoint); + + // Joint friction + { + this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); + this->motorJoint->SetVelocity(0, 0); + const double kFrictionCoef = 0.5; + dartJoint->setCoulombFriction(0, kFrictionCoef); + this->Step(10); + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + EXPECT_NEAR(kFrictionCoef, wrenchAtMotorJointInJoint.torque.z(), 1e-4); + dartJoint->setCoulombFriction(0, 0.0); + } + + // Joint damping + { + this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); + this->motorJoint->SetVelocity(0, 0); + const double kDampingCoef = 0.2; + dartJoint->setDampingCoefficient(0, kDampingCoef); + this->Step(100); + const double omega = this->motorJoint->GetVelocity(0); + this->Step(1); + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + EXPECT_NEAR(-omega * kDampingCoef, wrenchAtMotorJointInJoint.torque.z(), + 1e-3); + dartJoint->setDampingCoefficient(0, 0.0); + } + + // Joint stiffness + { + // Note: By default, the spring reference position is 0. + this->motorJoint->SetPosition(0, IGN_DTOR(30.0)); + this->motorJoint->SetVelocity(0, 0); + const double kSpringStiffness = 0.7; + dartJoint->setSpringStiffness(0, kSpringStiffness); + this->Step(1); + const double theta = this->motorJoint->GetPosition(0); + this->Step(1); + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + EXPECT_NEAR(-theta * kSpringStiffness, wrenchAtMotorJointInJoint.torque.z(), + 1e-3); + dartJoint->setSpringStiffness(0, 0.0); + } +} + +// Check that the transmitted wrench is affected contact forces +TEST_F(JointTransmittedWrenchFixture, ContactForces) +{ + auto box = this->world->GetModel("box"); + ASSERT_NE(nullptr, box); + auto boxFreeGroup = box->FindFreeGroup(); + ASSERT_NE(nullptr, boxFreeGroup); + physics::Pose3d X_WB(Eigen::Translation3d(0, 1, 1)); + boxFreeGroup->SetWorldPose(X_WB); + + this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); + // After this many steps, the pendulum is in contact with the box + this->Step(1000); + const double theta = this->motorJoint->GetPosition(0); + // Sanity check that the pendulum is at rest + EXPECT_NEAR(0.0, this->motorJoint->GetVelocity(0), 1e-3); + + auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); + + // To compute the reaction forces, we consider the pivot on the contact point + // between the pendulum and the box and the fact that the sum of moments about + // the pivot is zero. We also note that all forces, including the reaction + // forces, are in the vertical (world's z-axis) direction. + // + // Notation: + // Fp_z: Reaction force at pendulum joint (pin) in the world's z-axis + // M_b: Moment about the contact point between box and pendulum + // + // Fp_z = √(Fn² + Ft²) // Since all of the reaction force is in the world's + // z-axis + // + // ∑M_b = 0 = Fp * sin(θ) * (2*r) - m₁*g*sin(θ)*r - m₂*g*sin(θ)*(2*r) + // + // Fp_z = 0.5 * g * ((m₁ + 2*m₂)) + // + // We can then compute the tangential (Ft) and normal (Fn) components as + // + // Ft = Fp_z * sin(θ) + // Fn = Fp_z * cos(θ) + + const double reactionForceAtP = + 0.5 * kGravity * (kArmLinkMass + 2 * kSensorLinkMass); + + Wrench3d expectedWrenchAtMotorJointInJoint{ + Vector3d::Zero(), + {-reactionForceAtP * cos(theta), reactionForceAtP * sin(theta), 0}}; + + EXPECT_TRUE(physics::test::Equal(expectedWrenchAtMotorJointInJoint, + wrenchAtMotorJointInJoint, 1e-4)); +} + ///////////////////////////////////////////////// int main(int argc, char *argv[]) { diff --git a/dartsim/worlds/pendulum_joint_wrench.sdf b/dartsim/worlds/pendulum_joint_wrench.sdf new file mode 100644 index 000000000..20f168f8d --- /dev/null +++ b/dartsim/worlds/pendulum_joint_wrench.sdf @@ -0,0 +1,262 @@ + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + true + 0.0 1 11.48 0 0 0 + + + + 1 1 1 + + + + + 0.0 + 0.0 + + + + + + + 1 1 1 + + + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + + + + + + + + + 1000 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.1 0 1.115 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0.8 0 0.05 0 0 0 + + + 0.05 + + + + + -0.8 0 0.05 0 0 0 + + + 0.05 + + + + + 0 0.8 0.05 0 0 0 + + + 0.05 + + + + + 0 -0.8 0.05 0 0 0 + + + 0.05 + + + + + + + base + sensor_link + + 0 0 1 + + + + + 0.125 0.0 2.1 0 1.5707963267948966 0 + + 0.4 + + 0.01333 + 0.01333 + 0.02 + + + + + + 0.08 + 0.25 + + + + 0.0 0.8 0.0 1 + 0.0 0.8 0.0 1 + + + + + + + sensor_link + arm + + + + 0.125 0.0 1.6 0 0 0 + + 6.0 + + .515 + .515 + .03 + + + + + 0 0 0.5 0 1.5707963267948966 0 + + + 0.1 + 0.2 + + + + 0.0 0.0 0.8 1 + 0.0 0.0 0.8 1 + + + + 0 0 0.5 0 1.5707963267948966 0 + + + 0.1 + 0.3 + + + + + + 0 0 -0.5 0 0 0 + + + 0.11 + + + + 0.8 0.8 0.8 1 + 0.5 0.5 0.0 1 + + + + 0 0 -0.5 0 0 0 + + + 0.11 + + + + + + + + 0.1 + 1.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0.1 + 1.0 + + + + + + + + diff --git a/include/ignition/physics/Geometry.hh b/include/ignition/physics/Geometry.hh index b008d9a09..248feb2e5 100644 --- a/include/ignition/physics/Geometry.hh +++ b/include/ignition/physics/Geometry.hh @@ -56,6 +56,14 @@ namespace ignition using AngularVector = Vector; IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(AngularVector) + template + struct Wrench + { + AngularVector torque; + LinearVector force; + }; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Wrench) + template using AlignedBox = Eigen::AlignedBox; IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(AlignedBox) diff --git a/include/ignition/physics/Joint.hh b/include/ignition/physics/Joint.hh index 005640ce9..a1a01b621 100644 --- a/include/ignition/physics/Joint.hh +++ b/include/ignition/physics/Joint.hh @@ -19,6 +19,7 @@ #define IGNITION_PHYSICS_JOINT_HH_ #include +#include #include namespace ignition @@ -533,6 +534,56 @@ namespace ignition public: virtual void DetachJoint(const Identity &_jointID) = 0; }; }; + + class IGNITION_PHYSICS_VISIBLE GetJointTransmittedWrench + : public virtual FeatureWithRequirements + { + public: template + class Joint + : public virtual JointFrameSemantics::Joint + { + public: using Wrench = typename FromPolicy< + PolicyT>::template Use; + + /// \brief Get the transmitted wrench at the Joint frame. + /// + /// The transmitted wrench is the force and torque + /// applied by the parent link on the child link, transmitted through + /// the joint. It is the sum of constraint forces from the joint, + /// applied joint force (set by the user using the Joint::SetForce API) + /// as well as forces due to joint friction, damping, and spring + /// stiffness. + public: Wrench GetTransmittedWrench() const; + + /// \brief Get the transmitted wrench of this joint at the specified + /// reference frame and expressed in the specified coordinate frame. + /// + /// The transmitted wrench is the force and torque applied by the parent + /// link on the child link, transmitted through the joint. It is the sum + /// of constraint forces from the joint, applied joint force (set by the + /// user using the Joint::SetForce API) as well as forces due to joint + /// friction, damping, and spring stiffness. + /// \param[in] _relativeTo Reference frame whose origin specifies the + /// location where the linear force of the wrench is applied. + /// \param[in] _inCoordinatesOf Coordinate frame in which the wrench is + /// expressed. Unlike _relativeTo, the coordinate frame is only used + /// to apply a rotation to the individual vectors in the wrench. It does + /// not move the point where the force is applied. + public: Wrench GetTransmittedWrench( + const FrameID &_relativeTo, + const FrameID &_inCoordinatesOf) const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Wrench = typename FromPolicy< + PolicyT>::template Use; + public: virtual Wrench GetJointTransmittedWrenchInJointFrame( + const Identity &_jointID) const = 0; + }; + }; + } } diff --git a/include/ignition/physics/RelativeQuantity.hh b/include/ignition/physics/RelativeQuantity.hh index cd4ca2e2a..e8546f815 100644 --- a/include/ignition/physics/RelativeQuantity.hh +++ b/include/ignition/physics/RelativeQuantity.hh @@ -142,6 +142,7 @@ namespace ignition template struct VectorSpace; template struct FrameSpace; template struct AABBSpace; + template struct WrenchSpace; // TODO(MXG): We can add more spaces to support other types like Moments // of Inertia, Jacobians, Spatial Velocities/Accelerations, Wrench+Point // pairs, and so on. @@ -204,6 +205,12 @@ namespace ignition using RelativeFrameData = RelativeQuantity< FrameData, Dim, detail::FrameSpace>; IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeFrameData) + + ///////////////////////////////////////////////// + template + using RelativeWrench = RelativeQuantity< + Wrench, Dim, detail::WrenchSpace>; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeWrench) } } diff --git a/include/ignition/physics/detail/Joint.hh b/include/ignition/physics/detail/Joint.hh index e8ed197f3..8e05c974e 100644 --- a/include/ignition/physics/detail/Joint.hh +++ b/include/ignition/physics/detail/Joint.hh @@ -19,6 +19,7 @@ #define IGNITION_PHYSICS_DETAIL_JOINT_HH_ #include +#include "ignition/physics/detail/FrameSemantics.hh" namespace ignition { @@ -220,6 +221,30 @@ namespace ignition this->template Interface() ->DetachJoint(this->identity); } + + ///////////////////////////////////////////////// + template + auto GetJointTransmittedWrench::Joint:: + GetTransmittedWrench() const -> Wrench + { + return this->template Interface() + ->GetJointTransmittedWrenchInJointFrame(this->identity); + } + + ///////////////////////////////////////////////// + template + auto GetJointTransmittedWrench::Joint:: + GetTransmittedWrench(const FrameID &_relativeTo, + const FrameID &_inCoordinatesOf) const -> Wrench + { + using RelativeWrench = + physics::RelativeWrench; + + return detail::Resolve( + *this->template Interface(), + RelativeWrench(this->GetFrameID(), this->GetTransmittedWrench()), + _relativeTo, _inCoordinatesOf); + } } } diff --git a/include/ignition/physics/detail/RelativeQuantity.hh b/include/ignition/physics/detail/RelativeQuantity.hh index 8f484648f..4aec1642d 100644 --- a/include/ignition/physics/detail/RelativeQuantity.hh +++ b/include/ignition/physics/detail/RelativeQuantity.hh @@ -22,6 +22,7 @@ #include #include +#include "ignition/physics/Geometry.hh" namespace ignition { @@ -828,6 +829,83 @@ namespace ignition return resultFrameData; } }; + + ///////////////////////////////////////////////// + /// \brief WrenchSpace + template + struct WrenchSpace + { + IGNITION_PHYSICS_DEFINE_COORDINATE_SPACE(Wrench<_Scalar, _Dim>) + using AngularVectorSpace = VectorSpace<_Scalar, (_Dim*(_Dim-1))/2>; + using LinearVectorSpace = VectorSpace<_Scalar, _Dim>; + using Op = Operator<_Scalar, _Dim>; + + public: static Quantity ResolveToWorldFrame( + const Quantity &_wrench, + const FrameDataType &_parentFrame) + { + Quantity result; + + result.torque = AngularVectorSpace::ResolveToWorldFrame( + _wrench.torque, _parentFrame) + + Op::Cross(_parentFrame.pose.translation(), + LinearVectorSpace::ResolveToWorldFrame( + _wrench.force, _parentFrame)); + + result.force = LinearVectorSpace::ResolveToWorldFrame(_wrench.force, + _parentFrame); + + return result; + } + + public: static Quantity ResolveToTargetFrame( + const Quantity &_wrench, + const FrameDataType &_parentFrame, + const FrameDataType &_targetFrame) + { + Quantity result; + + LinearVector3d momentArmInTarget = + (_targetFrame.pose.inverse() * _parentFrame.pose).translation(); + + result.torque = + AngularVectorSpace::ResolveToTargetFrame( + _wrench.torque, _parentFrame, _targetFrame) + + Op::Cross(momentArmInTarget, + LinearVectorSpace::ResolveToTargetFrame( + _wrench.force, _parentFrame, _targetFrame)); + + result.force = LinearVectorSpace::ResolveToTargetFrame( + _wrench.force, _parentFrame, _targetFrame); + return result; + } + + public: static Quantity ResolveToWorldCoordinates( + const Quantity &_wrench, + const RotationType &_currentCoordinates) + { + Quantity result; + result.torque = AngularVectorSpace::ResolveToWorldCoordinates( + _wrench.torque, _currentCoordinates); + result.force = LinearVectorSpace::ResolveToWorldCoordinates( + _wrench.force, _currentCoordinates); + return result; + } + + public: static Quantity ResolveToTargetCoordinates( + const Quantity &_wrench, + const RotationType &_currentCoordinates, + const RotationType &_targetCoordinates) + { + Quantity result; + result.torque = AngularVectorSpace::ResolveToTargetCoordinates( + _wrench.torque, _currentCoordinates, _targetCoordinates); + result.force = LinearVectorSpace::ResolveToTargetCoordinates( + _wrench.force, _currentCoordinates, _targetCoordinates); + return result; + } + }; + } } } diff --git a/test/Utils.hh b/test/Utils.hh index 6abbb2915..78d8b55cd 100644 --- a/test/Utils.hh +++ b/test/Utils.hh @@ -217,6 +217,19 @@ namespace ignition return result; } + + ///////////////////////////////////////////////// + template + bool Equal(const Wrench &_data1, + const Wrench &_data2, + const double _tolerance) + { + bool result = true; + result &= Equal(_data1.torque, _data2.torque, _tolerance, "torque"); + result &= Equal(_data1.force, _data2.force, _tolerance, "force"); + + return result; + } } } } From 7bd1556c3993a722e4cc6cc8dc04ffa6ec9ef371 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 21 Sep 2021 00:27:57 -0700 Subject: [PATCH 2/3] Fix sign convention and comments in test Signed-off-by: Steve Peters --- dartsim/src/JointFeatures_TEST.cc | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 7ec00bae4..4d53f5e40 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -1160,17 +1160,17 @@ TEST_F(JointTransmittedWrenchFixture , PendulumAtZeroAngle) TEST_F(JointTransmittedWrenchFixture, PendulumInMotion) { namespace test = physics::test; - // Start pendulum at 90° (parallel to the ground) and stop when its around 40° + // Start pendulum at 90° (parallel to the ground) and stop at about 40° // so that we have non-trivial test expectations. this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); this->Step(350); // Given the position (θ), velocity (ω), and acceleration (α) of the joint - // and distance from the joint to the COM (r), we can the reaction forces in + // and distance from the joint to the COM (r), the reaction forces in // the tangent direction (Ft) and normal direction (Fn) are given by: // - // Ft = m * α * r - (m * g * sin(θ)) = m * (α * r - g * sin(θ)) - // Fn = m * ω² * r + (m * g * cos(θ)) = m * (ω² * r + g * cos(θ)) + // Ft = m * α * r + (m * g * sin(θ)) = m * (α * r + g * sin(θ)) + // Fn = -m * ω² * r - (m * g * cos(θ)) = -m * (ω² * r + g * cos(θ)) { const double theta = this->motorJoint->GetPosition(0); const double omega = this->motorJoint->GetVelocity(0); @@ -1184,16 +1184,17 @@ TEST_F(JointTransmittedWrenchFixture, PendulumInMotion) auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); const double armTangentForce = - kArmLinkMass * ((-alpha * kArmLength / 2.0) - (kGravity * sin(theta))); + kArmLinkMass * ((alpha * kArmLength / 2.0) + (kGravity * sin(theta))); const double motorLinkTangentForce = - -kSensorLinkMass * kGravity * sin(theta); + kSensorLinkMass * kGravity * sin(theta); const double armNormalForce = - kArmLinkMass * + -kArmLinkMass * ((std::pow(omega, 2) * kArmLength / 2.0) + (kGravity * cos(theta))); - const double motorLinkNormalForce = kSensorLinkMass * kGravity * cos(theta); + const double motorLinkNormalForce = + -kSensorLinkMass * kGravity * cos(theta); const double tangentForce = armTangentForce + motorLinkTangentForce; const double normalForce = armNormalForce + motorLinkNormalForce; @@ -1201,7 +1202,7 @@ TEST_F(JointTransmittedWrenchFixture, PendulumInMotion) // The orientation of the joint frame is such that the normal force is // parallel to the x-axis and the tangent force is parallel to the y-axis. Wrench3d expectedWrenchAtMotorJointInJoint{ - Vector3d::Zero(), {-normalForce, -tangentForce, 0}}; + Vector3d::Zero(), {normalForce, tangentForce, 0}}; EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInJoint, wrenchAtMotorJointInJoint, 1e-4)); @@ -1225,7 +1226,7 @@ TEST_F(JointTransmittedWrenchFixture, PendulumInMotion) EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInWorld, wrenchAtMotorJointInWorld, 1e-4)); - // This moves the point of application as well change the coordinate frame + // This moves the point of application and changes the coordinate frame Wrench3d wrenchAtArmInArm = this->motorJoint->GetTransmittedWrench( armLink->GetFrameID(), armLink->GetFrameID()); @@ -1252,7 +1253,7 @@ TEST_F(JointTransmittedWrenchFixture, PendulumInMotion) TEST_F(JointTransmittedWrenchFixture, ValidateWrenchWithSecondaryJoint) { namespace test = physics::test; - // Start pendulum at 90° (parallel to the ground) and stop when its around 40° + // Start pendulum at 90° (parallel to the ground) and stop at about 40° // so that we have non-trivial test expectations. this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); this->Step(350); From 42a5613c85c306896f37b032dc3243689fa9d7cf Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 21 Sep 2021 00:53:06 -0700 Subject: [PATCH 3/3] Fix more signs and comments in test Signed-off-by: Steve Peters --- dartsim/src/JointFeatures_TEST.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 4d53f5e40..82502c9a2 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -1342,7 +1342,7 @@ TEST_F(JointTransmittedWrenchFixture, JointLosses) } } -// Check that the transmitted wrench is affected contact forces +// Check that the transmitted wrench is affected by contact forces TEST_F(JointTransmittedWrenchFixture, ContactForces) { auto box = this->world->GetModel("box"); @@ -1373,14 +1373,14 @@ TEST_F(JointTransmittedWrenchFixture, ContactForces) // Fp_z = √(Fn² + Ft²) // Since all of the reaction force is in the world's // z-axis // - // ∑M_b = 0 = Fp * sin(θ) * (2*r) - m₁*g*sin(θ)*r - m₂*g*sin(θ)*(2*r) + // ∑M_b = 0 = -Fp_z * sin(θ) * (2*r) + m₁*g*sin(θ)*r + m₂*g*sin(θ)*(2*r) // - // Fp_z = 0.5 * g * ((m₁ + 2*m₂)) + // Fp_z = 0.5 * g * (m₁ + 2*m₂) // // We can then compute the tangential (Ft) and normal (Fn) components as // - // Ft = Fp_z * sin(θ) - // Fn = Fp_z * cos(θ) + // Ft = Fp_z * sin(θ) + // Fn = -Fp_z * cos(θ) const double reactionForceAtP = 0.5 * kGravity * (kArmLinkMass + 2 * kSensorLinkMass);