Fix free joint and ball joints bug in workaround for Issue #122

dartsim · Jan 10, 2014 · e739477 · e739477
1 parent ed15686
commit e739477
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 11 deletions.
diff --git a/src/dynamics/BallJoint.cpp b/src/dynamics/BallJoint.cpp
@@ -80,6 +80,8 @@ void BallJoint::updateTransform_Issue122(double _timeStep)
 {
     mT_Joint = mT_Joint * math::expAngular(_timeStep * get_dq());
 
+    set_q(math::logMap(mT_Joint).head<3>());
+
     mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
 
     assert(math::verifyTransform(mT));

diff --git a/src/dynamics/FreeJoint.cpp b/src/dynamics/FreeJoint.cpp
@@ -66,20 +66,10 @@ FreeJoint::~FreeJoint()
 
 void FreeJoint::updateTransform()
 {
-    Eigen::Vector3d q1(mCoordinate[0].get_q(),
-                       mCoordinate[1].get_q(),
-                       mCoordinate[2].get_q());
-    Eigen::Vector3d q2(mCoordinate[3].get_q(),
-                  mCoordinate[4].get_q(),
-                  mCoordinate[5].get_q());
-
     // TODO(JS): This is workaround for Issue #122.
     mT_Joint = math::expMap(get_q());
 
-    mT = mT_ParentBodyToJoint *
-            Eigen::Translation3d(q2) *
-            math::expAngular(q1) *
-            mT_ChildBodyToJoint.inverse();
+    mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
 
     assert(math::verifyTransform(mT));
 }
@@ -89,6 +79,8 @@ void FreeJoint::updateTransform_Issue122(double _timeStep)
 {
     mT_Joint = mT_Joint * math::expMap(_timeStep * get_dq());
 
+    set_q(math::logMap(mT_Joint));
+
     mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
 
     assert(math::verifyTransform(mT));