New Bullet bindings for teleoperation. (#2548)

* A set of minor bug fixes and exposing some bullet variables used in teleop application

* Remove commented early exit.

---------

Co-authored-by: Alexander Clegg <[email protected]>

Author: 0mdc

src/esp/bindings/PhysicsObjectBindings.cpp

@@ -565,6 +565,19 @@ void declareArticulatedObjectWrapper(py::module& m,
             "'s link specified by the given link_id.")
                .c_str(),
            "link_id"_a)
+      .def("get_link_joint_axes", &ManagedArticulatedObject::getLinkJointAxes,
+           ("Get the axes (see btMultiBody top and bottom axis) of the parent "
+            "joint for this " +
+            objType + "'s link specified by the given link_id.")
+               .c_str(),
+           "link_id"_a)
+      .def("get_link_joint_to_com",
+           &ManagedArticulatedObject::getLinkJointToCoM,
+           ("Get the local vector pointing from " + objType +
+            "'s link' the parent joint to the CoM of the link specified by the "
+            "given link_id.")
+               .c_str(),
+           "link_id"_a)
       .def("get_link_joint_name", &ManagedArticulatedObject::getLinkJointName,
            ("Get the name of the parent joint for this " + objType +
             "'s link specified by the given link_id.")
@@ -622,7 +635,8 @@ void declareArticulatedObjectWrapper(py::module& m,
       .def("create_all_motors",
            &ManagedArticulatedObject::createMotorsForAllDofs,
            ("Make motors for all of this " + objType +
-            "'s links which support motors (Revolute, Prismatic, Spherical).")
+            "'s links which support motors (Revolute, Prismatic, Spherical). "
+            "Returns a dict mapping new motor ids to link ids.")
                .c_str(),
            "settings"_a)
       .def("update_all_motor_targets",
@@ -638,7 +652,8 @@ void declareArticulatedObjectWrapper(py::module& m,
            "state_targets"_a, "velocities"_a = false)
       .def("create_joint_motor", &ManagedArticulatedObject::createJointMotor,
            ("Create a joint motor for the specified DOF on this " + objType +
-            " using the provided JointMotorSettings")
+            " using the provided JointMotorSettings. Returns the newly created "
+            "motor's id.")
                .c_str(),
            "link"_a, "settings"_a)
       .def(

src/esp/physics/ArticulatedObject.h

@@ -589,6 +589,28 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
     return JointType::Invalid;
   }
 
+  /**
+   * @brief Get the top and bottom axis vectors for the link's parent joint.
+   *
+   * @param linkId The link's index.
+   * @return The link's parent joint's type.
+   */
+  virtual std::pair<Mn::Vector3, Mn::Vector3> getLinkJointAxes(
+      CORRADE_UNUSED int linkId) const {
+    return std::pair<Mn::Vector3, Mn::Vector3>();
+  }
+
+  /**
+   * @brief gets a local vector pointing from the parent joint to the CoM of the
+   * link.
+   *
+   * @param linkId The link's index.
+   * @return The a local vector from joint to link CoM.
+   */
+  virtual Mn::Vector3 getLinkJointToCoM(CORRADE_UNUSED int linkId) const {
+    return Mn::Vector3();
+  }
+
   /**
    * @brief Get the name of the link's parent joint.
    *

src/esp/physics/bullet/BulletArticulatedObject.cpp

@@ -535,6 +535,29 @@ JointType BulletArticulatedObject::getLinkJointType(int linkId) const {
   return JointType(int(btMultiBody_->getLink(linkId).m_jointType));
 }
 
+std::pair<Mn::Vector3, Mn::Vector3> BulletArticulatedObject::getLinkJointAxes(
+    int linkId) const {
+  // Notes from btMultiBodyLink:
+  //  "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local
+  //  frame.) constant. for prismatic: m_axesTop[0] = zero;
+  //                 m_axesBottom[0] = unit vector along the joint axis.
+  //  for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
+  //                m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion
+  //                due to the rotation at the joint)
+  // NOTE: this implementation will not return the additional components for
+  // multi-dof joints
+  auto axt = btMultiBody_->getLink(linkId).m_axes->m_topVec;
+  auto axb = btMultiBody_->getLink(linkId).m_axes->m_bottomVec;
+  return std::pair<Mn::Vector3, Mn::Vector3>(Mn::Vector3{axt},
+                                             Mn::Vector3{axb});
+}
+
+Mn::Vector3 BulletArticulatedObject::getLinkJointToCoM(int linkId) const {
+  // gets a local vector pointing from the parent joint to the CoM of the link
+  // use this to solve for the joint position from the link
+  return Mn::Vector3{btMultiBody_->getLink(linkId).m_dVector};
+}
+
 int BulletArticulatedObject::getLinkDoFOffset(int linkId) const {
   CORRADE_INTERNAL_ASSERT(getNumLinks() > linkId && linkId >= 0);
   return btMultiBody_->getLink(linkId).m_dofOffset;

src/esp/physics/bullet/BulletArticulatedObject.h

@@ -238,6 +238,25 @@ class BulletArticulatedObject : public ArticulatedObject {
    */
   JointType getLinkJointType(CORRADE_UNUSED int linkId) const override;
 
+  /**
+   * @brief Get the top and bottom axis vectors for the link's parent joint. See
+   * btMultiBody for details.
+   *
+   * @param linkId The link's index.
+   * @return The link's parent joint's axes.
+   */
+  std::pair<Mn::Vector3, Mn::Vector3> getLinkJointAxes(
+      CORRADE_UNUSED int linkId) const override;
+
+  /**
+   * @brief gets a local vector pointing from the parent joint to the CoM of the
+   * link.
+   *
+   * @param linkId The link's index.
+   * @return The a local vector from joint to link CoM.
+   */
+  Mn::Vector3 getLinkJointToCoM(CORRADE_UNUSED int linkId) const override;
+
   /**
    * @brief Get the starting position for this link's parent joint in the global
    * DoFs array.

src/esp/physics/bullet/BulletPhysicsManager.cpp

@@ -371,12 +371,15 @@ bool BulletPhysicsManager::attachLinkGeometry(
         // cache the visual component for later query
         linkObject->visualAttachments_.emplace_back(
             &visualGeomComponent, visual.m_geometry.m_meshFileName);
-        return true;
+      } else {
+        return false;
       }
+    } else {
+      return false;
     }
   }
 
-  return false;
+  return true;
 }
 
 void BulletPhysicsManager::setGravity(const Magnum::Vector3& gravity) {

src/esp/physics/objectWrappers/ManagedArticulatedObject.h

@@ -219,6 +219,20 @@ class ManagedArticulatedObject
     return JointType::Invalid;
   }
 
+  std::pair<Mn::Vector3, Mn::Vector3> getLinkJointAxes(int linkId) const {
+    if (auto sp = getObjectReference()) {
+      return sp->getLinkJointAxes(linkId);
+    }
+    return std::pair<Mn::Vector3, Mn::Vector3>();
+  }
+
+  Mn::Vector3 getLinkJointToCoM(CORRADE_UNUSED int linkId) const {
+    if (auto sp = getObjectReference()) {
+      return sp->getLinkJointToCoM(linkId);
+    }
+    return Mn::Vector3();
+  }
+
   std::string getLinkJointName(int linkId) const {
     if (auto sp = getObjectReference()) {
       return sp->getLinkJointName(linkId);