diff --git a/src/dynamics/BallJoint.cpp b/src/dynamics/BallJoint.cpp
index fb03ef7a6fa35..744b0f6464837 100644
--- 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
index e7d9f1321993e..71d71ba1988c4 100644
--- 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));