Fix BallJoint/FreeJoint in updateTransform_Issue122()

dartsim · Dec 20, 2013 · fddf941 · fddf941
1 parent 8a9b7c4
commit fddf941
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 19 deletions.
diff --git a/src/dynamics/BallJoint.cpp b/src/dynamics/BallJoint.cpp
@@ -64,8 +64,8 @@ BallJoint::~BallJoint()
 void BallJoint::updateTransform()
 {
     Eigen::Vector3d q(mCoordinate[0].get_q(),
-            mCoordinate[1].get_q(),
-            mCoordinate[2].get_q());
+                      mCoordinate[1].get_q(),
+                      mCoordinate[2].get_q());
 
     // TODO(JS): This is workaround for Issue #122.
     mT_Joint = math::expAngular(q);
@@ -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));
@@ -88,8 +90,8 @@ void BallJoint::updateTransform_Issue122(double _timeStep)
 void BallJoint::updateJacobian()
 {
     Eigen::Vector3d q(mCoordinate[0].get_q(),
-            mCoordinate[1].get_q(),
-            mCoordinate[2].get_q());
+                      mCoordinate[1].get_q(),
+                      mCoordinate[2].get_q());
 
     Eigen::Matrix3d J = math::expMapJac(q);
 
@@ -126,11 +128,11 @@ void BallJoint::updateJacobian_Issue122()
 void BallJoint::updateJacobianTimeDeriv()
 {
     Eigen::Vector3d q(mCoordinate[0].get_q(),
-            mCoordinate[1].get_q(),
-            mCoordinate[2].get_q());
+                      mCoordinate[1].get_q(),
+                      mCoordinate[2].get_q());
     Eigen::Vector3d dq(mCoordinate[0].get_dq(),
-            mCoordinate[1].get_dq(),
-            mCoordinate[2].get_dq());
+                       mCoordinate[1].get_dq(),
+                       mCoordinate[2].get_dq());
 
     Eigen::Matrix3d dJ = math::expMapJacDot(q, dq);
 

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));