From 2ba401c9baf87fad5098a3c047a92cc192ad63b1 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 11 Oct 2024 16:42:03 -0700 Subject: [PATCH] bullet-featherstone - add applied constraint to joint transmitted wrench (#668) * add test world file Signed-off-by: Ian Chen --- bullet-featherstone/src/JointFeatures.cc | 31 ++++ test/common_test/Worlds.hh | 2 + test/common_test/joint_features.cc | 138 ++++++++++++++++++ .../worlds/joint_offset_empty_links.sdf | 29 ++++ 4 files changed, 200 insertions(+) create mode 100644 test/common_test/worlds/joint_offset_empty_links.sdf diff --git a/bullet-featherstone/src/JointFeatures.cc b/bullet-featherstone/src/JointFeatures.cc index 59d5c65a8..44328e4fb 100644 --- a/bullet-featherstone/src/JointFeatures.cc +++ b/bullet-featherstone/src/JointFeatures.cc @@ -742,6 +742,37 @@ Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( jointInfo->jointFeedback->m_reactionForces.getLinear()); wrenchOut.torque = jointInfo->tf_to_child.rotation() * convert( jointInfo->jointFeedback->m_reactionForces.getAngular()); + + // If a constraint is used to move the joint, e.g motor constraint, + // account for the applied constraint forces and torques. + // \todo(iche033) Check whether this is also needed for gearbox constraint + if (jointInfo->motor) + { + auto linkInfo = this->ReferenceInterface( + jointInfo->childLinkID); + + // link index in model should be >=0 because we expect the base link in + // btMultibody to always be a parent link of a joint + // (except when it's fixed to world) + int linkIndexInModel = -1; + if (linkInfo->indexInModel.has_value()) + linkIndexInModel = *linkInfo->indexInModel; + if (linkIndexInModel >= 0) + { + auto *modelInfo = this->ReferenceInterface(linkInfo->model); + btMultibodyLink &link = modelInfo->body->getLink(linkIndexInModel); + + wrenchOut.force += + jointInfo->tf_to_child.rotation().inverse() * + convert(link.m_cachedWorldTransform.getBasis().inverse() * + link.m_appliedConstraintForce); + wrenchOut.torque += + jointInfo->tf_to_child.rotation().inverse() * + convert(link.m_cachedWorldTransform.getBasis().inverse() * + link.m_appliedConstraintTorque); + } + } + return wrenchOut; } diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index 37d77efd7..212f9c35a 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -41,6 +41,8 @@ const auto kJointAcrossModelsSdf = CommonTestWorld("joint_across_models.sdf"); const auto kJointAcrossModelsFixedSdf = CommonTestWorld("joint_across_models_fixed.sdf"); const auto kJointConstraintSdf = CommonTestWorld("joint_constraint.sdf"); +const auto kJointOffsetEmptyLinksSdf = + CommonTestWorld("joint_offset_empty_links.sdf"); const auto kMimicFastSlowPendulumsWorld = CommonTestWorld("mimic_fast_slow_pendulums_world.sdf"); const auto kMimicPendulumWorld = CommonTestWorld("mimic_pendulum_world.sdf"); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 435fdabcb..129465d95 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -300,6 +300,7 @@ struct JointFeaturePositionLimitsForceControlList : gz::physics::FeatureList< gz::physics::GetBasicJointState, gz::physics::GetEngineInfo, gz::physics::GetJointFromModel, + gz::physics::GetJointTransmittedWrench, gz::physics::GetModelFromWorld, gz::physics::SetBasicJointState, gz::physics::SetJointEffortLimitsFeature, @@ -681,6 +682,7 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetVelocityLimitsWi joint->SetVelocityCommand(0, 1); world->Step(output, state, input); } + EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 10; ++i) @@ -818,6 +820,142 @@ TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, JointSetCombinedLimitsWi EXPECT_NEAR(-0.5, joint->GetVelocity(0), 1e-6); } } + +TYPED_TEST(JointFeaturesPositionLimitsForceControlTest, + JointTransmittedWrenchWithVelocityControl) +{ + for (const std::string &name : this->pluginNames) + { + // This test requires https://github.com/bulletphysics/bullet3/pull/4462 +#ifdef BT_BULLET_VERSION_LE_325 + if (this->PhysicsEngineName(name) == "bullet-featherstone") + GTEST_SKIP(); +#endif + + std::cout << "Testing plugin: " << name << std::endl; + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = + gz::physics::RequestEngine3d:: + From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + const sdf::Errors errors = + root.Load(common_test::worlds::kJointOffsetEmptyLinksSdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + auto model = world->GetModel("model"); + auto joint = model->GetJoint("J0"); + + // default step size: 1ms + double dt = 1e-3; + // velocity limit set in SDF + double velocityLimit = 4; + const double positionGoal = 0.1; + // Calculate number of time steps expected to reach the position goal at + // the maximum joint velocity. + const int expectedSteps = + static_cast(positionGoal / velocityLimit / dt); + // Take the expected number of steps. + gzdbg << "Taking " << expectedSteps << " steps " + << "to reach the goal." << std::endl; + + gz::physics::ForwardStep::Output output; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Input input; + + for (int i = 0; i < expectedSteps; ++i) + { + joint->SetVelocityCommand(0, velocityLimit); + world->Step(output, state, input); + } + + // Read wrench from force_torque_status and expect it to be consistent with + // the dynamic state of the model. + // + // Summary of dynamic state at this time in the test: + // - The base link is fixed to the world. + // - The child link is attached to the base link via the revolute joint J0. + // - The child link has a mass of 1 kg and its center of mass is located + // 0.5 m from joint J0. + // m = 1 kg + // L = 0.5 m + // - The joint velocity command moves the joint at its maximum velocity of + // 4 rad/s about the X-axis of the joint to reach its goal position + // of 0.1 rad. With a constant angular velocity, the angular acceleration + // is zero. + // pos_J0 = 0.1 rad (approximately) + // vel_J0 = 4 rad/s + // acc_J0 = 0 rad/s^2 + // - The child link is rotating about the joint like a pendulum with a + // linear acceleration consisting of centripetal acceleration since its + // angular acceleration is zero. When expressed in coordinates of the + // joint frame, the linear acceleration is in the y direction. + // a_child = {0, m * L * vel_J0^2, 0} + // - Gravity is acting in the negative Z direction of the world frame with a + // magnitude of 9.8 m/s^2. + // g = 9.8 m/s^2 + // - The force of gravity is expressed in coordinates of the joint frame as + // F_gravity = {0, - m * g * sin(pos_J0), - m * g * cos(pos_J0)}. + // - The joint transmitted wrench is the wrench applied from the base link + // to the child link at joint J0 and expressed in coordinates of + // the frame, which happens to coincide with the joint frame. + // - First apply conservation of linear momentum: + // - The sum of forces acting on the child link is equal to the product of + // mass and the linear acceleration of its center of mass. + // F_child = m * a_child + // - The forces acting on the child link include the force of gravity and + // the reaction force from the joint: + // F_child = F_gravity + F_joint + // - The joint reaction force is thus equal to: + // F_joint = m * a_child - F_gravity + // - which can be expressed in coordinates of the joint frame as: + // F_joint = m * {0, L * vel_J0^2 + g * sin(pos_J0), g * cos(pos_J0)} + // - Now apply conservation of angular momentum with respect to the origin + // of the joint frame J0: + // - Since the origin of the joint frame J0 is fixed with respect to an + // inertial frame, conservation of angular momentum about that point + // implies that the sum of torques acting on the child link at the joint + // origin (T_child_J0) is equal to the product of its moment of inertia + // with respect to the joint origin (I_J0) and its angular acceleration + // (which is zero). + // Thus the sum of torques acting on the child link is zero. + // T_child_J0 = I_J0 * alpha_child = 0 + // - The torques acting on the child link with respect to the origin of + // the joint frame include the torque due to gravity and the reaction + // torque at the joint: + // T_child_J0 = T_gravity_J0 + T_joint_J0 + // T_gravity_J0 = {m * g * L * cos(pos_J0), 0, 0} + // T_joint_J0 = {-m * g * L cos(pos_J0), 0, 0} + // + // After substitution of known constants, the expected wrench is therefore: + const double expectedForceX = 0; + // expectedForceY = m * L * vel_J0^2 + g * sin(pos_J0) + // expectedForceY = 1 * 0.5 * 4^2 + 9.8 * sin(0.1) + const double expectedForceY = 8 + 9.8 * sin(positionGoal); + // expectedForceZ = m * g * cos(pos_J0) + // expectedForceZ = 1 * 9.8 * cos(0.1) + const double expectedForceZ = 9.8 * cos(positionGoal); + // expectedTorqueX = -m * g * L cos(pos_J0) + // expectedTorqueX = -1 * 9.8 * 0.5 cos(0.1) + const double expectedTorqueX = -4.9 * cos(positionGoal); + const double expectedTorqueY = 0; + const double expectedTorqueZ = 0; + gzdbg << "Checking that wrench values match the dynamic state." + << std::endl; + auto wrench = joint->GetTransmittedWrench(); + EXPECT_NEAR(expectedForceX, wrench.force.x(), 1e-6); + // Looser tolerances are needed for the nonzero terms + EXPECT_NEAR(expectedForceY, wrench.force.y(), 1e-1); + EXPECT_NEAR(expectedForceZ, wrench.force.z(), 1e-2); + EXPECT_NEAR(expectedTorqueX, wrench.torque.x(), 1e-2); + EXPECT_NEAR(expectedTorqueY, wrench.torque.y(), 1e-6); + EXPECT_NEAR(expectedTorqueZ, wrench.torque.z(), 1e-6); + } +} + ///////////// DARTSIM > 6.10 end diff --git a/test/common_test/worlds/joint_offset_empty_links.sdf b/test/common_test/worlds/joint_offset_empty_links.sdf new file mode 100644 index 000000000..fbf255b09 --- /dev/null +++ b/test/common_test/worlds/joint_offset_empty_links.sdf @@ -0,0 +1,29 @@ + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + + 0 0 0.5 0 0 0 + + + base + link_1 + 0 0.5 0 0 0 0 + + 1 0 0 + + 4 + + + + + world + base + + + +