Add relative Jacobian functions to MetaSkeleton

CHANGELOG.md

@@ -8,6 +8,7 @@
 
 * Kinematics/Dynamics
 
+  * Added relative Jacobian functions to MetaSkeleton: [#997](https://github.com/dartsim/dart/pull/997)
   * Added vectorized joint limit functions: [#996](https://github.com/dartsim/dart/pull/996)
   * Added lazy evaluation for shape's volume and bounding-box computation: [#959](https://github.com/dartsim/dart/pull/959)
   * Added IkFast support as analytic IK solver: [#887](https://github.com/dartsim/dart/pull/887)

dart/dynamics/MetaSkeleton.cpp

@@ -31,8 +31,9 @@
  */
 
 #include "dart/common/Console.hpp"
-#include "dart/dynamics/MetaSkeleton.hpp"
 #include "dart/dynamics/DegreeOfFreedom.hpp"
+#include "dart/dynamics/JacobianNode.hpp"
+#include "dart/dynamics/MetaSkeleton.hpp"
 
 namespace dart {
 namespace dynamics {
@@ -938,6 +939,129 @@ Eigen::VectorXd MetaSkeleton::getJointConstraintImpulses() const
         this, "getJointConstraintImpulses");
 }
 
+//==============================================================================
+math::Jacobian MetaSkeleton::getJacobian(
+    const JacobianNode* _node,
+    const JacobianNode* _relativeTo,
+    const Frame* _inCoordinatesOf) const
+{
+  if (_node == _relativeTo)
+    return math::Jacobian::Zero(6, getNumDofs());
+
+  const math::Jacobian J = getJacobian(_node);
+  const math::Jacobian JRelTo = getJacobian(_relativeTo);
+  const Eigen::Isometry3d T = _relativeTo->getTransform(_node);
+
+  const math::Jacobian result = (J - math::AdTJac(T, JRelTo)).eval();
+
+  if (_node == _inCoordinatesOf)
+    return result;
+
+  return math::AdRJac(_node->getTransform(_inCoordinatesOf), result);
+}
+
+//==============================================================================
+math::Jacobian MetaSkeleton::getJacobian(
+    const JacobianNode* _node,
+    const Eigen::Vector3d& _localOffset,
+    const JacobianNode* _relativeTo,
+    const Frame* _inCoordinatesOf) const
+{
+  if (_node == _relativeTo)
+    return math::Jacobian::Zero(6, getNumDofs());
+
+  const math::Jacobian J = getJacobian(_node);
+  const math::Jacobian JRelTo = getJacobian(_relativeTo);
+  const Eigen::Isometry3d T = _relativeTo->getTransform(_node);
+
+  math::Jacobian result = (J - math::AdTJac(T, JRelTo)).eval();
+  result.bottomRows<3>() += result.topRows<3>().colwise().cross(_localOffset);
+
+  if (_node == _inCoordinatesOf)
+    return result;
+
+  return math::AdRJac(_node->getTransform(_inCoordinatesOf), result);
+}
+
+//==============================================================================
+math::LinearJacobian MetaSkeleton::getLinearJacobian(
+    const JacobianNode* _node,
+    const JacobianNode* _relativeTo,
+    const Frame* _inCoordinatesOf) const
+{
+  return getJacobian(_node, _relativeTo, _inCoordinatesOf).bottomRows<3>();
+}
+
+//==============================================================================
+math::LinearJacobian MetaSkeleton::getLinearJacobian(
+    const JacobianNode* _node,
+    const Eigen::Vector3d& _localOffset,
+    const JacobianNode* _relativeTo,
+    const Frame* _inCoordinatesOf) const
+{
+  return getJacobian(
+        _node, _localOffset, _relativeTo, _inCoordinatesOf).bottomRows<3>();
+}
+
+//==============================================================================
+math::AngularJacobian MetaSkeleton::getAngularJacobian(
+    const JacobianNode* _node,
+    const JacobianNode* _relativeTo,
+    const Frame* _inCoordinatesOf) const
+{
+  return getJacobian(_node, _relativeTo, _inCoordinatesOf).topRows<3>();
+}
+
+//==============================================================================
+math::Jacobian MetaSkeleton::getJacobianSpatialDeriv(
+    const JacobianNode* _node,
+    const JacobianNode* _relativeTo,
+    const Frame* _inCoordinatesOf) const
+{
+  if (_node == _relativeTo)
+    return math::Jacobian::Zero(6, getNumDofs());
+
+  const math::Jacobian dJ = getJacobianSpatialDeriv(_node);
+  const math::Jacobian JRelTo = getJacobian(_relativeTo);
+  const math::Jacobian dJRelTo = getJacobianSpatialDeriv(_relativeTo);
+  const Eigen::Isometry3d T = _relativeTo->getTransform(_node);
+  const Eigen::Vector6d V = _relativeTo->getSpatialVelocity(_node, _relativeTo);
+  const math::Jacobian adJRelTo = math::adJac(V, JRelTo);
+
+  const math::Jacobian result = dJ - math::AdTJac(T, dJRelTo + adJRelTo);
+
+  if (_node == _inCoordinatesOf)
+    return result;
+
+  return math::AdRJac(_node->getTransform(_inCoordinatesOf), result);
+}
+
+//==============================================================================
+math::Jacobian MetaSkeleton::getJacobianSpatialDeriv(
+    const JacobianNode* _node,
+    const Eigen::Vector3d& _localOffset,
+    const JacobianNode* _relativeTo,
+    const Frame* _inCoordinatesOf) const
+{
+  if (_node == _relativeTo)
+    return math::Jacobian::Zero(6, getNumDofs());
+
+  const math::Jacobian dJ = getJacobianSpatialDeriv(_node);
+  const math::Jacobian JRelTo = getJacobian(_relativeTo);
+  const math::Jacobian dJRelTo = getJacobianSpatialDeriv(_relativeTo);
+  const Eigen::Isometry3d T = _relativeTo->getTransform(_node);
+  const Eigen::Vector6d V = _relativeTo->getSpatialVelocity(_node, _relativeTo);
+  const math::Jacobian adJRelTo = math::adJac(V, JRelTo);
+
+  math::Jacobian result = dJ - math::AdTJac(T, dJRelTo + adJRelTo);
+  result.bottomRows<3>().noalias() += result.topRows<3>().colwise().cross(_localOffset);
+
+  if (_node == _inCoordinatesOf)
+    return result;
+
+  return math::AdRJac(_node->getTransform(_inCoordinatesOf), result);
+}
+
 //==============================================================================
 double MetaSkeleton::computeLagrangian() const
 {

dart/dynamics/MetaSkeleton.hpp

@@ -540,11 +540,19 @@ class MetaSkeleton : public common::Subject
   virtual math::Jacobian getJacobian(const JacobianNode* _node) const = 0;
 
   /// Get the spatial Jacobian targeting the origin of a BodyNode. You can
-  /// specify a coordinate Frame to express the Jabocian in.
+  /// specify a coordinate Frame to express the Jacobian in.
   virtual math::Jacobian getJacobian(
       const JacobianNode* _node,
       const Frame* _inCoordinatesOf) const = 0;
 
+  /// Get the spatial Jacobian targeting the origin of a BodyNode relative to
+  /// another BodyNode in the same Skeleton. You can specify a coordinate Frame
+  /// to express the Jacobian in.
+  math::Jacobian getJacobian(
+      const JacobianNode* _node,
+      const JacobianNode* _relativeTo,
+      const Frame* _inCoordinatesOf) const;
+
   /// Get the spatial Jacobian targeting an offset in a BodyNode. The _offset is
   /// expected in coordinates of the BodyNode Frame. The Jacobian is expressed
   /// in the Frame of the BodyNode.
@@ -554,12 +562,22 @@ class MetaSkeleton : public common::Subject
 
   /// Get the spatial Jacobian targeting an offset in a BodyNode. The _offset is
   /// expected in coordinates of the BodyNode Frame. You can specify a
-  /// coordinate Frame to express the Jabocian in.
+  /// coordinate Frame to express the Jacobian in.
   virtual math::Jacobian getJacobian(
       const JacobianNode* _node,
       const Eigen::Vector3d& _localOffset,
       const Frame* _inCoordinatesOf) const = 0;
 
+  /// Get the spatial Jacobian targeting an offset in a BodyNode relative to
+  /// another BodyNode in the same Skeleton. The _offset is expected in
+  /// coordinates of the BodyNode Frame. You can specify a coordinate Frame to
+  /// express the Jacobian in.
+  math::Jacobian getJacobian(
+      const JacobianNode* _node,
+      const Eigen::Vector3d& _localOffset,
+      const JacobianNode* _relativeTo,
+      const Frame* _inCoordinatesOf) const;
+
   /// Get the spatial Jacobian targeting the origin of a BodyNode. The Jacobian
   /// is expressed in the World Frame.
   virtual math::Jacobian getWorldJacobian(const JacobianNode* _node) const = 0;
@@ -572,32 +590,64 @@ class MetaSkeleton : public common::Subject
       const Eigen::Vector3d& _localOffset) const = 0;
 
   /// Get the linear Jacobian targeting the origin of a BodyNode. You can
-  /// specify a coordinate Frame to express the Jabocian in.
+  /// specify a coordinate Frame to express the Jacobian in.
   virtual math::LinearJacobian getLinearJacobian(
       const JacobianNode* _node,
       const Frame* _inCoordinatesOf = Frame::World()) const = 0;
 
   /// Get the linear Jacobian targeting an offset in a BodyNode. The _offset is
   /// expected in coordinates of the BodyNode Frame. You can specify a
-  /// coordinate Frame to express the Jabocian in.
+  /// coordinate Frame to express the Jacobian in.
   virtual math::LinearJacobian getLinearJacobian(
       const JacobianNode* _node,
       const Eigen::Vector3d& _localOffset,
       const Frame* _inCoordinatesOf = Frame::World()) const = 0;
 
+  /// Get the linear Jacobian targeting the origin of a BodyNode relative to
+  /// another BodyNode in the same Skeleton. You can specify a coordinate Frame
+  /// to express the Jacobian in.
+  math::LinearJacobian getLinearJacobian(
+      const JacobianNode* _node,
+      const JacobianNode* _relativeTo,
+      const Frame* _inCoordinatesOf = Frame::World()) const;
+
+  /// Get the linear Jacobian targeting an offset in a BodyNode relative to
+  /// another BodyNode in the same Skeleton. The _offset is expected in
+  /// coordinates of the BodyNode Frame. You can specify a coordinate Frame to
+  /// express the Jacobian in.
+  math::LinearJacobian getLinearJacobian(
+      const JacobianNode* _node,
+      const Eigen::Vector3d& _localOffset,
+      const JacobianNode* _relativeTo,
+      const Frame* _inCoordinatesOf = Frame::World()) const;
+
+  /// \}
+
+  //----------------------------------------------------------------------------
+  /// \{ \name Jacobian derivatives
+  //----------------------------------------------------------------------------
+
   /// Get the angular Jacobian of a BodyNode. You can specify a coordinate Frame
-  /// to express the Jabocian in.
+  /// to express the Jacobian in.
   virtual math::AngularJacobian getAngularJacobian(
       const JacobianNode* _node,
       const Frame* _inCoordinatesOf = Frame::World()) const = 0;
 
+  /// Get the angular Jacobian of a BodyNode relative to another BodyNode in the
+  /// same Skeleton. You can specify a coordinate Frame to express the Jacobian
+  /// in.
+  math::AngularJacobian getAngularJacobian(
+      const JacobianNode* _node,
+      const JacobianNode* _relativeTo,
+      const Frame* _inCoordinatesOf = Frame::World()) const;
+
   /// Get the spatial Jacobian time derivative targeting the origin of a
   /// BodyNode. The Jacobian is expressed in the Frame of the BodyNode.
   virtual math::Jacobian getJacobianSpatialDeriv(
       const JacobianNode* _node) const = 0;
 
   /// Get the spatial Jacobian time derivative targeting the origin of a
-  /// BodyNode. You can specify a coordinate Frame to express the Jabocian in.
+  /// BodyNode. You can specify a coordinate Frame to express the Jacobian in.
   virtual math::Jacobian getJacobianSpatialDeriv(
       const JacobianNode* _node,
       const Frame* _inCoordinatesOf) const = 0;
@@ -611,49 +661,66 @@ class MetaSkeleton : public common::Subject
 
   /// Get the spatial Jacobian time derivative targeting an offset in a
   /// BodyNode. The _offset is expected in coordinates of the BodyNode Frame.
-  /// You can specify a coordinate Frame to express the Jabocian in.
+  /// You can specify a coordinate Frame to express the Jacobian in.
   virtual math::Jacobian getJacobianSpatialDeriv(
       const JacobianNode* _node,
       const Eigen::Vector3d& _localOffset,
       const Frame* _inCoordinatesOf) const = 0;
 
   /// Get the spatial Jacobian time derivative targeting the origin of a
-  /// BodyNode. The Jacobian is expressed in the World Frame.
+  /// BodyNode relative to another BodyNode in the same Skeleton. You can
+  /// specify a coordinate Frame to express the Jacobian in.
+  math::Jacobian getJacobianSpatialDeriv(
+      const JacobianNode* _node,
+      const JacobianNode* _relativeTo,
+      const Frame* _inCoordinatesOf) const;
+
+  /// Get the spatial Jacobian time derivative targeting an offset in a
+  /// BodyNode relative to another Bodynode in the same Skeleton. The _offset is
+  /// expected in coordinates of the BodyNode Frame. You can specify a
+  /// coordinate Frame to express the Jacobian in.
+  math::Jacobian getJacobianSpatialDeriv(
+      const JacobianNode* _node,
+      const Eigen::Vector3d& _localOffset,
+      const JacobianNode* _relativeTo,
+      const Frame* _inCoordinatesOf) const;
+
+  /// Get the spatial Jacobian (classical) time derivative targeting the origin
+  /// of a BodyNode. The Jacobian is expressed in the World Frame.
   virtual math::Jacobian getJacobianClassicDeriv(
       const JacobianNode* _node) const = 0;
 
-  /// Get the spatial Jacobian time derivative targeting the origin a
-  /// BodyNode. The _offset is expected in coordinates of the BodyNode Frame.
-  /// You can specify a coordinate Frame to express the Jabocian in.
+  /// Get the spatial Jacobian (classical) time derivative targeting the origin
+  /// a BodyNode. The _offset is expected in coordinates of the BodyNode Frame.
+  /// You can specify a coordinate Frame to express the Jacobian in.
   virtual math::Jacobian getJacobianClassicDeriv(
       const JacobianNode* _node,
       const Frame* _inCoordinatesOf) const = 0;
 
-  /// Get the spatial Jacobian time derivative targeting an offset in a
-  /// BodyNode. The _offset is expected in coordinates of the BodyNode Frame.
-  /// You can specify a coordinate Frame to express the Jabocian in.
+  /// Get the spatial Jacobian (classical) time derivative targeting an offset
+  /// in a BodyNode. The _offset is expected in coordinates of the BodyNode
+  /// Frame. You can specify a coordinate Frame to express the Jacobian in.
   virtual math::Jacobian getJacobianClassicDeriv(
       const JacobianNode* _node,
       const Eigen::Vector3d& _localOffset,
       const Frame* _inCoordinatesOf = Frame::World()) const = 0;
 
-  /// Get the linear Jacobian (classical) time derivative targeting the origin
   /// of a BodyNode. The _offset is expected in coordinates of the BodyNode
-  /// Frame. You can specify a coordinate Frame to express the Jabocian in.
+  /// Frame. You can specify a coordinate Frame to express the Jacobian in.
   virtual math::LinearJacobian getLinearJacobianDeriv(
       const JacobianNode* _node,
       const Frame* _inCoordinatesOf = Frame::World()) const = 0;
 
   /// Get the linear Jacobian (classical) time derivative targeting an offset in
   /// a BodyNode. The _offset is expected in coordinates of the BodyNode Frame.
-  /// You can specify a coordinate Frame to express the Jabocian in.
+  /// You can specify a coordinate Frame to express the Jacobian in.
   virtual math::LinearJacobian getLinearJacobianDeriv(
       const JacobianNode* _node,
       const Eigen::Vector3d& _localOffset,
       const Frame* _inCoordinatesOf = Frame::World()) const = 0;
 
   /// Get the angular Jacobian time derivative of a BodyNode. You can specify a
-  /// coordinate Frame to express the Jabocian in.
+  /// coordinate Frame to express the Jacobian in.
   virtual math::AngularJacobian getAngularJacobianDeriv(
       const JacobianNode* _node,
       const Frame* _inCoordinatesOf = Frame::World()) const = 0;

dart/dynamics/Skeleton.hpp

@@ -58,6 +58,16 @@ class Skeleton :
     public detail::SkeletonAspectBase
 {
 public:
+  // Some of non-virtual functions of MetaSkeleton are hidden because of the
+  // functions of the same name in this class. We expose those functions as
+  // follows.
+  using MetaSkeleton::getJacobian;
+  using MetaSkeleton::getLinearJacobian;
+  using MetaSkeleton::getAngularJacobian;
+  using MetaSkeleton::getJacobianSpatialDeriv;
+  using MetaSkeleton::getJacobianClassicDeriv;
+  using MetaSkeleton::getLinearJacobianDeriv;
+  using MetaSkeleton::getAngularJacobianDeriv;
 
   using AspectPropertiesData = detail::SkeletonAspectProperties;
   using AspectProperties = common::Aspect::MakeProperties<AspectPropertiesData>;