bullet-featherstone - add applied constraint to joint transmitted wre…

…nch (#668)

* add test world file

Signed-off-by: Ian Chen <ichen@openrobotics.org>

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<LinkInfo>(
+        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<ModelInfo>(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;
 }
 

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");

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<JointFeaturePositionLimitsForceControlList>::
+      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<int>(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
 
 

test/common_test/worlds/joint_offset_empty_links.sdf

@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <world name="joint_empty_links">
+    <model name="model">
+      <pose>0 0 0 0 0 0</pose>
+      <link name="base">
+        <pose>0 0 0 0 0 0</pose>
+      </link>
+      <link name="link_1">
+        <pose relative_to="base">0 0 0.5 0 0 0</pose>
+      </link>
+      <joint name="J0" type="revolute">
+        <parent>base</parent>
+        <child>link_1</child>
+        <pose>0 0.5 0 0 0 0</pose>
+        <axis>
+          <xyz>1 0 0</xyz>
+          <limit>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name="fixed_joint" type="fixed">
+        <parent>world</parent>
+        <child>base</child>
+      </joint>
+    </model>
+  </world>
+</sdf>