Patch for BallJoint/FreeJoint integration bug (See #122)

This is just workaround.
dartsim · Dec 14, 2013 · 81b9235 · 81b9235
1 parent dc3bdeb
commit 81b9235
Show file tree

Hide file tree

Showing 8 changed files with 283 additions and 105 deletions.
diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp
@@ -45,7 +45,8 @@ namespace dart {
 namespace dynamics {
 
 BallJoint::BallJoint(const std::string& _name)
-  : Joint(BALL, _name) {
+  : Joint(BALL, _name),
+    mT_Joint(Eigen::Isometry3d::Identity()) {
   mGenCoords.push_back(&mCoordinate[0]);
   mGenCoords.push_back(&mCoordinate[1]);
   mGenCoords.push_back(&mCoordinate[2]);
@@ -66,9 +67,18 @@ inline void BallJoint::updateTransform() {
                     mCoordinate[1].get_q(),
                     mCoordinate[2].get_q());
 
-  mT = mT_ParentBodyToJoint
-       * math::expAngular(q)
-       * mT_ChildBodyToJoint.inverse();
+  // TODO(JS): This is workaround for Issue #122.
+  mT_Joint = math::expAngular(q);
+
+  mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
+
+  assert(math::verifyTransform(mT));
+}
+
+void BallJoint::updateTransform_Issue122(double _timeStep) {
+  mT_Joint = mT_Joint * math::expAngular(_timeStep * get_dq());
+
+  mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
 
   assert(math::verifyTransform(mT));
 }
@@ -95,6 +105,22 @@ inline void BallJoint::updateJacobian() {
   assert(!math::isNan(mS));
 }
 
+void BallJoint::updateJacobian_Issue122() {
+  Eigen::Vector6d J0 = Eigen::Vector6d::Zero();
+  Eigen::Vector6d J1 = Eigen::Vector6d::Zero();
+  Eigen::Vector6d J2 = Eigen::Vector6d::Zero();
+
+  J0[0] = 1.0;
+  J1[1] = 1.0;
+  J2[2] = 1.0;
+
+  mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
+  mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
+  mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
+
+  assert(!math::isNan(mS));
+}
+
 inline void BallJoint::updateJacobianTimeDeriv() {
   Eigen::Vector3d q(mCoordinate[0].get_q(),
       mCoordinate[1].get_q(),
@@ -120,5 +146,10 @@ inline void BallJoint::updateJacobianTimeDeriv() {
   assert(!math::isNan(mdS));
 }
 
+void BallJoint::updateJacobianTimeDeriv_Issue122() {
+  // mdS == 0
+  assert(mdS == Eigen::MatrixXd::Zero(6, 3));
+}
+
 }  // namespace dynamics
 }  // namespace dart
diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h
@@ -58,16 +58,29 @@ class BallJoint : public Joint {
   // Documentation inherited.
   virtual void updateTransform();
 
+  /// @brief TODO(JS): This is workaround for Issue #122.
+  virtual void updateTransform_Issue122(double _timeStep);
+
   // Documentation inherited.
   virtual void updateJacobian();
 
+  /// @brief TODO(JS): This is workaround for Issue #122.
+  virtual void updateJacobian_Issue122();
+
   // Documentation inherited.
   virtual void updateJacobianTimeDeriv();
 
+  /// @brief TODO(JS): This is workaround for Issue #122.
+  virtual void updateJacobianTimeDeriv_Issue122();
+
 protected:
   /// \brief
   GenCoord mCoordinate[3];
 
+  /// @brief
+  // TODO(JS): This is workaround for Issue #122.
+  Eigen::Isometry3d mT_Joint;
+
 public:
   //
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp
@@ -359,6 +359,19 @@ void BodyNode::updateTransform() {
   mParentJoint->updateJacobian();
 }
 
+void BodyNode::updateTransform_Issue122(double _timeStep) {
+  mParentJoint->updateTransform_Issue122(_timeStep);
+  if (mParentBodyNode) {
+    mW = mParentBodyNode->getWorldTransform()
+         * mParentJoint->getLocalTransform();
+  } else {
+    mW = mParentJoint->getLocalTransform();
+  }
+  assert(math::verifyTransform(mW));
+
+  mParentJoint->updateJacobian_Issue122();
+}
+
 void BodyNode::updateVelocity() {
   //--------------------------------------------------------------------------
   // Body velocity update
@@ -423,6 +436,52 @@ void BodyNode::updateEta(bool _updateJacobianDeriv) {
       mParentJoint->getLocalJacobianTimeDeriv();
 }
 
+void BodyNode::updateEta_Issue122(bool _updateJacobianDeriv) {
+  mParentJoint->updateJacobianTimeDeriv_Issue122();
+
+  if (mParentJoint->getNumGenCoords() > 0) {
+    mEta = math::ad(mV, mParentJoint->getLocalJacobian() *
+                    mParentJoint->get_dq());
+    mEta.noalias() += mParentJoint->getLocalJacobianTimeDeriv() *
+                      mParentJoint->get_dq();
+
+    assert(!math::isNan(mEta));
+  }
+
+  if (_updateJacobianDeriv == false)
+    return;
+
+  //--------------------------------------------------------------------------
+  // Jacobian first derivative update
+  //
+  // dJ = | dJ1 dJ2 ... dJn |
+  //   = | Ad(T(i,i-1), dJ_parent) dJ_local |
+  //
+  //   dJ_parent: (6 x parentDOF)
+  //    dJ_local: (6 x localDOF)
+  //         dJi: (6 x 1) se3
+  //          n: number of dependent coordinates
+  //--------------------------------------------------------------------------
+
+  const int numLocalDOFs = mParentJoint->getNumGenCoords();
+  const int numParentDOFs = getNumDependentGenCoords() - numLocalDOFs;
+
+  // Parent Jacobian
+  if (mParentBodyNode) {
+    assert(mParentBodyNode->mBodyJacobianTimeDeriv.cols()
+           + mParentJoint->getNumGenCoords() == mBodyJacobianTimeDeriv.cols());
+
+    assert(mParentJoint);
+    mBodyJacobianTimeDeriv.leftCols(numParentDOFs) =
+        math::AdInvTJac(mParentJoint->getLocalTransform(),
+                        mParentBodyNode->mBodyJacobianTimeDeriv);
+  }
+
+  // Local Jacobian
+  mBodyJacobianTimeDeriv.rightCols(numLocalDOFs) =
+      mParentJoint->getLocalJacobianTimeDeriv();
+}
+
 void BodyNode::updateAcceleration() {
   // dV(i) = Ad(T(i, i-1), dV(i-1))
   //         + ad(V(i), S * dq) + dS * dq

diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h
@@ -388,13 +388,19 @@ class BodyNode {
   /// \brief Update local transformations and world transformations.
   virtual void updateTransform();
 
+  /// @brief // TODO(JS): This is workaround for Issue #122.
+  virtual void updateTransform_Issue122(double _timeStep);
+
   /// \brief
   virtual void updateVelocity();
 
   /// \brief
   /// parentJoint.dS --> dJ
   virtual void updateEta(bool _updateJacobianDeriv = false);
 
+  /// @brief // TODO(JS): This is workaround for Issue #122.
+  virtual void updateEta_Issue122(bool _updateJacobianDeriv = false);
+
   /// \brief
   /// parentJoint.V, parentJoint.dV, parentBody.dV, V --> dV
   virtual void updateAcceleration();