Added convenience functions to help with setting FreeJoint, EulerJoint, and BallJoint positions

dart/dynamics/BallJoint.cpp

@@ -57,13 +57,26 @@ BallJoint::~BallJoint()
 {
 }
 
+//==============================================================================
+Eigen::Isometry3d BallJoint::convertToTransform(
+    const Eigen::Vector3d& _positions)
+{
+  return Eigen::Isometry3d(convertToRotation(_positions));
+}
+
+//==============================================================================
+Eigen::Matrix3d BallJoint::convertToRotation(const Eigen::Vector3d& _positions)
+{
+  return math::expMapRot(_positions);
+}
+
 //==============================================================================
 void BallJoint::integratePositions(double _dt)
 {
-  mR.linear() = mR.linear() * math::expMapRot(mJacobian.topRows<3>()
-                                              * mVelocities * _dt);
+  mR.linear() = mR.linear() * convertToRotation(mJacobian.topRows<3>()
+                                                * mVelocities * _dt);
 
-  mPositions = math::logMap(mR.linear());
+  mPositions = convertToPositions(mR.linear());
 }
 
 //==============================================================================
@@ -80,7 +93,7 @@ void BallJoint::updateDegreeOfFreedomNames()
 //==============================================================================
 void BallJoint::updateLocalTransform()
 {
-  mR.linear() = math::expMapRot(mPositions);
+  mR.linear() = convertToRotation(mPositions);
 
   mT = mT_ParentBodyToJoint * mR * mT_ChildBodyToJoint.inverse();
 

dart/dynamics/BallJoint.h

@@ -54,6 +54,24 @@ class BallJoint : public MultiDofJoint<3>
   /// Destructor
   virtual ~BallJoint();
 
+  /// Convert a rotation into a 3D vector that can be used to set the positions
+  /// of a BallJoint. The positions returned by this function will result in a
+  /// relative transform of
+  /// getTransformFromParentBodyNode() * _rotation * getTransformFromChildBodyNode().inverse()
+  /// between the parent BodyNode and the child BodyNode frames when applied to
+  /// a BallJoint.
+  template <typename RotationType>
+  static Eigen::Vector3d convertToPositions(const RotationType& _rotation)
+  {
+    return math::logMap(_rotation);
+  }
+
+  /// Convert a BallJoint-style position vector into a transform
+  static Eigen::Isometry3d convertToTransform(const Eigen::Vector3d& _positions);
+
+  /// Convert a BallJoint-style position vector into a rotation matrix
+  static Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions);
+
 protected:
   // Documentation inherited
   virtual void integratePositions(double _dt);

dart/dynamics/EulerJoint.cpp

@@ -73,6 +73,46 @@ EulerJoint::AxisOrder EulerJoint::getAxisOrder() const
   return mAxisOrder;
 }
 
+//==============================================================================
+Eigen::Isometry3d EulerJoint::convertToTransform(
+    const Eigen::Vector3d& _positions, AxisOrder _ordering)
+{
+  return Eigen::Isometry3d(convertToRotation(_positions, _ordering));
+}
+
+//==============================================================================
+Eigen::Isometry3d EulerJoint::convertToTransform(
+    const Eigen::Vector3d &_positions) const
+{
+  return convertToTransform(_positions, mAxisOrder);
+}
+
+//==============================================================================
+Eigen::Matrix3d EulerJoint::convertToRotation(
+    const Eigen::Vector3d& _positions, AxisOrder _ordering)
+{
+  switch (_ordering)
+  {
+    case AO_XYZ:
+      return math::eulerXYZToMatrix(_positions);
+    case AO_ZYX:
+      return math::eulerZYXToMatrix(_positions);
+    default:
+    {
+      dterr << "[EulerJoint::convertToRotation] Invalid AxisOrder specified ("
+            << _ordering << ")\n";
+      return Eigen::Matrix3d::Identity();
+    }
+  }
+}
+
+//==============================================================================
+Eigen::Matrix3d EulerJoint::convertToRotation(const Eigen::Vector3d& _positions)
+                                                                           const
+{
+  return convertToRotation(_positions, mAxisOrder);
+}
+
 //==============================================================================
 void EulerJoint::updateDegreeOfFreedomNames()
 {
@@ -117,28 +157,8 @@ void EulerJoint::updateDegreeOfFreedomNames()
 //==============================================================================
 void EulerJoint::updateLocalTransform()
 {
-  switch (mAxisOrder)
-  {
-    case AO_XYZ:
-    {
-      mT = mT_ParentBodyToJoint
-           * Eigen::Isometry3d(math::eulerXYZToMatrix(mPositions))
-           * mT_ChildBodyToJoint.inverse();
-      break;
-    }
-    case AO_ZYX:
-    {
-      mT = mT_ParentBodyToJoint
-           * Eigen::Isometry3d(math::eulerZYXToMatrix(mPositions))
-           * mT_ChildBodyToJoint.inverse();
-      break;
-    }
-    default:
-    {
-      dterr << "Undefined Euler axis order\n";
-      break;
-    }
-  }
+  mT = mT_ParentBodyToJoint * convertToTransform(mPositions)
+       * mT_ChildBodyToJoint.inverse();
 
   assert(math::verifyTransform(mT));
 }

dart/dynamics/EulerJoint.h

@@ -71,6 +71,53 @@ class EulerJoint : public MultiDofJoint<3>
   ///
   AxisOrder getAxisOrder() const;
 
+  /// Convert a rotation into a 3D vector that can be used to set the positions
+  /// of an EulerJoint with the specified AxisOrder. The positions returned by
+  /// this function will result in a relative transform of
+  /// getTransformFromParentBodyNode() * _rotation * getTransformFromChildBodyNode().inverse()
+  /// between the parent BodyNode and the child BodyNode frames when applied to
+  /// an EulerJoint with the correct axis ordering.
+  template <typename RotationType>
+  static Eigen::Vector3d convertToPositions(
+      const RotationType& _rotation, AxisOrder _ordering)
+  {
+    switch(_ordering)
+    {
+      case AO_XYZ:
+        return math::matrixToEulerXYZ(_rotation);
+      case AO_ZYX:
+        return math::matrixToEulerZYX(_rotation);
+      default:
+        dtwarn << "[EulerJoint::convertToPositions] Unsupported AxisOrder ("
+               << _ordering << "), returning a zero vector\n";
+        return Eigen::Vector3d::Zero();
+    }
+  }
+
+  /// This is a version of EulerJoint::convertToPositions(const RotationType&,
+  /// AxisOrder) which will use the AxisOrder belonging to the joint instance
+  /// that it gets called on.
+  template <typename RotationType>
+  Eigen::Vector3d convertToPositions(const RotationType& _rotation) const
+  {
+    return convertToPositions(_rotation, mAxisOrder);
+  }
+
+  /// Convert a set of Euler angle positions into a transform
+  static Eigen::Isometry3d convertToTransform(const Eigen::Vector3d& _positions,
+                                           AxisOrder _ordering);
+
+  /// This is a version of EulerJoint::convertToRotation(const Eigen::Vector3d&,
+  /// AxisOrder) which will use the AxisOrder belonging to the joint instance
+  /// that it gets called on.
+  Eigen::Isometry3d convertToTransform(const Eigen::Vector3d& _positions) const;
+
+  /// Convert a set of Euler angle positions into a rotation matrix
+  static Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions,
+                                           AxisOrder _ordering);
+
+  Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions) const;
+
 protected:
   /// Set the names of this joint's DegreesOfFreedom. Used during construction
   /// and when axis order is changed.

dart/dynamics/FreeJoint.cpp

@@ -57,15 +57,33 @@ FreeJoint::~FreeJoint()
 {
 }
 
+//==============================================================================
+Eigen::Vector6d FreeJoint::convertToPositions(const Eigen::Isometry3d& _tf)
+{
+  Eigen::Vector6d x;
+  x.head<3>() = math::logMap(_tf.linear());
+  x.tail<3>() = _tf.translation();
+  return x;
+}
+
+//==============================================================================
+Eigen::Isometry3d FreeJoint::convertToTransform(
+    const Eigen::Vector6d& _positions)
+{
+  Eigen::Isometry3d tf;
+  tf.linear() = math::expMapRot(_positions.head<3>());
+  tf.translation() = _positions.tail<3>();
+  return tf;
+}
+
 //==============================================================================
 void FreeJoint::integratePositions(double _dt)
 {
   mQ.linear()      = mQ.linear() * math::expMapRot(mJacobian.topRows<3>()
                                                    * mVelocities * _dt);
   mQ.translation() = mQ.translation() + mVelocities.tail<3>() * _dt;
 
-  mPositions.head<3>() = math::logMap(mQ.linear());
-  mPositions.tail<3>() = mQ.translation();
+  mPositions = convertToPositions(mQ);
 }
 
 //==============================================================================
@@ -88,8 +106,7 @@ void FreeJoint::updateDegreeOfFreedomNames()
 //==============================================================================
 void FreeJoint::updateLocalTransform()
 {
-  mQ.linear()      = math::expMapRot(mPositions.head<3>());
-  mQ.translation() = mPositions.tail<3>();
+  mQ = convertToTransform(mPositions);
 
   mT = mT_ParentBodyToJoint * mQ * mT_ChildBodyToJoint.inverse();
 

dart/dynamics/FreeJoint.h

@@ -56,6 +56,17 @@ class FreeJoint : public MultiDofJoint<6>
   /// Destructor
   virtual ~FreeJoint();
 
+  /// Convert a transform into a 6D vector that can be used to set the positions
+  /// of a FreeJoint. The positions returned by this function will result in a
+  /// relative transform of
+  /// getTransformFromParentBodyNode() * _tf * getTransformFromChildBodyNode().inverse()
+  /// between the parent BodyNode and the child BodyNode frames when applied to
+  /// a FreeJoint.
+  static Eigen::Vector6d convertToPositions(const Eigen::Isometry3d& _tf);
+
+  /// Convert a FreeJoint-style 6D vector into a transform
+  static Eigen::Isometry3d convertToTransform(const Eigen::Vector6d& _positions);
+
 protected:
   // Documentation inherited
   virtual void integratePositions(double _dt);

unittests/testJoints.cpp

@@ -502,6 +502,138 @@ TEST_F(JOINTS, JOINT_COULOMB_FRICTION)
     testJointCoulombFrictionForce(timeStep);
 }
 
+//==============================================================================
+template<int N>
+Eigen::Matrix<double,N,1> random_vec(double limit=100)
+{
+  Eigen::Matrix<double,N,1> v;
+  for(size_t i=0; i<N; ++i)
+    v[i] = math::random(-fabs(limit), fabs(limit));
+  return v;
+}
+
+//==============================================================================
+Eigen::Isometry3d random_transform(double translation_limit=100,
+                                   double rotation_limit=2*M_PI)
+{
+  Eigen::Vector3d r = random_vec<3>(translation_limit);
+  Eigen::Vector3d theta = random_vec<3>(rotation_limit);
+
+  Eigen::Isometry3d tf;
+  tf.setIdentity();
+  tf.translate(r);
+
+  if(theta.norm()>0)
+    tf.rotate(Eigen::AngleAxisd(theta.norm(), theta.normalized()));
+
+  return tf;
+}
+
+Eigen::Isometry3d predict_joint_transform(Joint* joint,
+                                          const Eigen::Isometry3d& joint_tf)
+{
+  return joint->getTransformFromParentBodyNode() * joint_tf
+          * joint->getTransformFromChildBodyNode().inverse();
+}
+
+Eigen::Isometry3d get_relative_transform(BodyNode* bn, BodyNode* relativeTo)
+{
+  return relativeTo->getTransform().inverse() * bn->getTransform();
+}
+
+//==============================================================================
+TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)
+{
+  // -- set up the root BodyNode
+  BodyNode* root = new BodyNode("root");
+  WeldJoint* rootjoint = new WeldJoint("base");
+  root->setParentJoint(rootjoint);
+
+  // -- set up the FreeJoint
+  BodyNode* freejoint_bn = new BodyNode("freejoint_bn");
+  FreeJoint* freejoint = new FreeJoint("freejoint");
+
+  freejoint_bn->setParentJoint(freejoint);
+  root->addChildBodyNode(freejoint_bn);
+
+  freejoint->setTransformFromParentBodyNode(random_transform());
+  freejoint->setTransformFromChildBodyNode(random_transform());
+  Eigen::Isometry3d freejoint_tf = random_transform();
+  freejoint->setPositions(FreeJoint::convertToPositions(freejoint_tf));
+
+  // -- set up the EulerJoint
+  BodyNode* eulerjoint_bn = new BodyNode("eulerjoint_bn");
+  EulerJoint* eulerjoint = new EulerJoint("eulerjoint");
+
+  eulerjoint_bn->setParentJoint(eulerjoint);
+  root->addChildBodyNode(eulerjoint_bn);
+
+  eulerjoint->setTransformFromParentBodyNode(random_transform());
+  eulerjoint->setTransformFromChildBodyNode(random_transform());
+  Eigen::Isometry3d eulerjoint_tf = random_transform();
+  eulerjoint_tf.translation() = Eigen::Vector3d::Zero();
+  eulerjoint->setPositions(
+        eulerjoint->convertToPositions(eulerjoint_tf.linear()));
+
+  // -- set up the BallJoint
+  BodyNode* balljoint_bn = new BodyNode("balljoint_bn");
+  BallJoint* balljoint = new BallJoint("balljoint");
+
+  balljoint_bn->setParentJoint(balljoint);
+  root->addChildBodyNode(balljoint_bn);
+
+  balljoint->setTransformFromParentBodyNode(random_transform());
+  balljoint->setTransformFromChildBodyNode(random_transform());
+  Eigen::Isometry3d balljoint_tf = random_transform();
+  balljoint_tf.translation() = Eigen::Vector3d::Zero();
+  balljoint->setPositions(
+        BallJoint::convertToPositions(balljoint_tf.linear()));
+
+  // -- set up Skeleton and compute forward kinematics
+  Skeleton* skel = new Skeleton;
+  skel->addBodyNode(root);
+  skel->addBodyNode(freejoint_bn);
+  skel->addBodyNode(eulerjoint_bn);
+  skel->addBodyNode(balljoint_bn);
+  skel->init();
+  skel->computeForwardKinematics(true, false, false);
+
+  std::vector<Joint*> joints;
+  std::vector<BodyNode*> bns;
+  std::vector<Eigen::Isometry3d> tfs;
+
+  joints.push_back(freejoint);
+  bns.push_back(freejoint_bn);
+  tfs.push_back(freejoint_tf);
+
+  joints.push_back(eulerjoint);
+  bns.push_back(eulerjoint_bn);
+  tfs.push_back(eulerjoint_tf);
+
+  joints.push_back(balljoint);
+  bns.push_back(balljoint_bn);
+  tfs.push_back(balljoint_tf);
+
+  for(size_t i=0; i<joints.size(); ++i)
+  {
+    Joint* joint = joints[i];
+    BodyNode* bn = bns[i];
+    Eigen::Isometry3d tf = tfs[i];
+    EXPECT_TRUE(equals(predict_joint_transform(joint, tf).matrix(),
+            get_relative_transform(bn, bn->getParentBodyNode()).matrix()));
+
+    if(!equals(predict_joint_transform(joint, tf).matrix(),
+          get_relative_transform(bn, bn->getParentBodyNode()).matrix()))
+    {
+      std::cout << "[" << joint->getName() << " Failed]\n";
+      std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix()
+                << "\n\nActual:\n"
+                << get_relative_transform(bn, bn->getParentBodyNode()).matrix()
+                << "\n\n";
+    }
+  }
+}
+
 //==============================================================================
 int main(int argc, char* argv[])
 {