From 81b92353d609493009b7dd2080b1427bd4180f94 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 13 Dec 2013 20:05:09 -0500 Subject: [PATCH] Patch for BallJoint/FreeJoint integration bug (See #122) This is just workaround. --- dart/dynamics/BallJoint.cpp | 39 +++++- dart/dynamics/BallJoint.h | 13 ++ dart/dynamics/BodyNode.cpp | 59 +++++++++ dart/dynamics/BodyNode.h | 6 + dart/dynamics/FreeJoint.cpp | 235 +++++++++++++++++++++--------------- dart/dynamics/FreeJoint.h | 13 ++ dart/dynamics/Joint.h | 9 ++ dart/dynamics/Skeleton.cpp | 14 ++- 8 files changed, 283 insertions(+), 105 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 48976f30bf08d..716b3f87e6b35 100644 --- 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 index 51843e355d599..827b9aef22cc8 100644 --- 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 index 477ea0ea3e239..6af2a4e721372 100644 --- 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 index 31b22229a5540..e6eaef72ddfbd 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -388,6 +388,9 @@ 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(); @@ -395,6 +398,9 @@ class BodyNode { /// 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(); diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 77a388b2266a4..38209138a4f6a 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -45,119 +45,158 @@ namespace dart { namespace dynamics { FreeJoint::FreeJoint(const std::string& _name) - : Joint(FREE, _name) { - mGenCoords.push_back(&mCoordinate[0]); - mGenCoords.push_back(&mCoordinate[1]); - mGenCoords.push_back(&mCoordinate[2]); - mGenCoords.push_back(&mCoordinate[3]); - mGenCoords.push_back(&mCoordinate[4]); - mGenCoords.push_back(&mCoordinate[5]); - - mS = Eigen::Matrix::Zero(); - mdS = Eigen::Matrix::Zero(); - - mSpringStiffness.resize(6, 0.0); - mDampingCoefficient.resize(6, 0.0); - mRestPosition.resize(6, 0.0); + : Joint(FREE, _name), + mT_Joint(Eigen::Isometry3d::Identity()) { + mGenCoords.push_back(&mCoordinate[0]); + mGenCoords.push_back(&mCoordinate[1]); + mGenCoords.push_back(&mCoordinate[2]); + mGenCoords.push_back(&mCoordinate[3]); + mGenCoords.push_back(&mCoordinate[4]); + mGenCoords.push_back(&mCoordinate[5]); + + mS = Eigen::Matrix::Zero(); + mdS = Eigen::Matrix::Zero(); + + mSpringStiffness.resize(6, 0.0); + mDampingCoefficient.resize(6, 0.0); + mRestPosition.resize(6, 0.0); } 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()); - - mT = mT_ParentBodyToJoint - * Eigen::Translation3d(q2) - * math::expAngular(q1) - * mT_ChildBodyToJoint.inverse(); - - assert(math::verifyTransform(mT)); + 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 * mT_Joint * mT_ChildBodyToJoint.inverse(); + + assert(math::verifyTransform(mT)); +} + +void FreeJoint::updateTransform_Issue122(double _timeStep) { + mT_Joint = mT_Joint * math::expMap(_timeStep * get_dq()); + + mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse(); + + assert(math::verifyTransform(mT)); } void FreeJoint::updateJacobian() { - Eigen::Vector3d q(mCoordinate[0].get_q(), - mCoordinate[1].get_q(), - mCoordinate[2].get_q()); - - Eigen::Matrix3d J = math::expMapJac(q); - - Eigen::Vector6d J0; - Eigen::Vector6d J1; - Eigen::Vector6d J2; - Eigen::Vector6d J3; - Eigen::Vector6d J4; - Eigen::Vector6d J5; - - J0 << J(0, 0), J(0, 1), J(0, 2), 0, 0, 0; - J1 << J(1, 0), J(1, 1), J(1, 2), 0, 0, 0; - J2 << J(2, 0), J(2, 1), J(2, 2), 0, 0, 0; - J3 << 0, 0, 0, 1, 0, 0; - J4 << 0, 0, 0, 0, 1, 0; - J5 << 0, 0, 0, 0, 0, 1; - - mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0); - mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1); - mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2); - mS.col(3) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3); - mS.col(4) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4); - mS.col(5) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5); - - assert(!math::isNan(mS)); + Eigen::Vector3d q(mCoordinate[0].get_q(), + mCoordinate[1].get_q(), + mCoordinate[2].get_q()); + + Eigen::Matrix3d J = math::expMapJac(q); + + Eigen::Vector6d J0; + Eigen::Vector6d J1; + Eigen::Vector6d J2; + Eigen::Vector6d J3; + Eigen::Vector6d J4; + Eigen::Vector6d J5; + + J0 << J(0, 0), J(0, 1), J(0, 2), 0, 0, 0; + J1 << J(1, 0), J(1, 1), J(1, 2), 0, 0, 0; + J2 << J(2, 0), J(2, 1), J(2, 2), 0, 0, 0; + J3 << 0, 0, 0, 1, 0, 0; + J4 << 0, 0, 0, 0, 1, 0; + J5 << 0, 0, 0, 0, 0, 1; + + mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0); + mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1); + mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2); + mS.col(3) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3); + mS.col(4) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4); + mS.col(5) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5); + + assert(!math::isNan(mS)); +} + +void FreeJoint::updateJacobian_Issue122() { + Eigen::Vector6d J0 = Eigen::Vector6d::Zero(); + Eigen::Vector6d J1 = Eigen::Vector6d::Zero(); + Eigen::Vector6d J2 = Eigen::Vector6d::Zero(); + Eigen::Vector6d J3 = Eigen::Vector6d::Zero(); + Eigen::Vector6d J4 = Eigen::Vector6d::Zero(); + Eigen::Vector6d J5 = Eigen::Vector6d::Zero(); + + J0[0] = 1.0; + J1[1] = 1.0; + J2[2] = 1.0; + J3[3] = 1.0; + J4[4] = 1.0; + J5[5] = 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); + mS.col(3) = math::AdT(mT_ChildBodyToJoint, J3); + mS.col(4) = math::AdT(mT_ChildBodyToJoint, J4); + mS.col(5) = math::AdT(mT_ChildBodyToJoint, J5); + + assert(!math::isNan(mS)); } void FreeJoint::updateJacobianTimeDeriv() { - Eigen::Vector3d q(mCoordinate[0].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()); - - Eigen::Matrix3d dJ = math::expMapJacDot(q, dq); - - Eigen::Vector6d dJ0; - Eigen::Vector6d dJ1; - Eigen::Vector6d dJ2; - Eigen::Vector6d J3; - Eigen::Vector6d J4; - Eigen::Vector6d J5; - - dJ0 << dJ(0, 0), dJ(0, 1), dJ(0, 2), 0, 0, 0; - dJ1 << dJ(1, 0), dJ(1, 1), dJ(1, 2), 0, 0, 0; - dJ2 << dJ(2, 0), dJ(2, 1), dJ(2, 2), 0, 0, 0; - J3 << 0, 0, 0, 1, 0, 0; - J4 << 0, 0, 0, 0, 1, 0; - J5 << 0, 0, 0, 0, 0, 1; - - mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0); - mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1); - mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2); - mdS.col(3) = - -math::ad(mS.leftCols<3>() * get_dq().head<3>(), - math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3)); - mdS.col(4) = - -math::ad(mS.leftCols<3>() * get_dq().head<3>(), - math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4)); - mdS.col(5) = - -math::ad(mS.leftCols<3>() * get_dq().head<3>(), - math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5)); - - assert(!math::isNan(mdS)); + Eigen::Vector3d q(mCoordinate[0].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()); + + Eigen::Matrix3d dJ = math::expMapJacDot(q, dq); + + Eigen::Vector6d dJ0; + Eigen::Vector6d dJ1; + Eigen::Vector6d dJ2; + Eigen::Vector6d J3; + Eigen::Vector6d J4; + Eigen::Vector6d J5; + + dJ0 << dJ(0, 0), dJ(0, 1), dJ(0, 2), 0, 0, 0; + dJ1 << dJ(1, 0), dJ(1, 1), dJ(1, 2), 0, 0, 0; + dJ2 << dJ(2, 0), dJ(2, 1), dJ(2, 2), 0, 0, 0; + J3 << 0, 0, 0, 1, 0, 0; + J4 << 0, 0, 0, 0, 1, 0; + J5 << 0, 0, 0, 0, 0, 1; + + mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0); + mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1); + mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2); + mdS.col(3) = + -math::ad(mS.leftCols<3>() * get_dq().head<3>(), + math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3)); + mdS.col(4) = + -math::ad(mS.leftCols<3>() * get_dq().head<3>(), + math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4)); + mdS.col(5) = + -math::ad(mS.leftCols<3>() * get_dq().head<3>(), + math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5)); + + assert(!math::isNan(mdS)); +} + +void FreeJoint::updateJacobianTimeDeriv_Issue122() { + // mdS == 0 + assert(mdS == Eigen::MatrixXd::Zero(6, 6)); } void FreeJoint::clampRotation() { - for (int i = 0; i < 3; i++) { - if (mCoordinate[i].get_q() > M_PI) - mCoordinate[i].set_q(mCoordinate[i].get_q() - 2*M_PI); - if (mCoordinate[i].get_q() < -M_PI) - mCoordinate[i].set_q(mCoordinate[i].get_q() + 2*M_PI); - } + for (int i = 0; i < 3; i++) { + if (mCoordinate[i].get_q() > M_PI) + mCoordinate[i].set_q(mCoordinate[i].get_q() - 2*M_PI); + if (mCoordinate[i].get_q() < -M_PI) + mCoordinate[i].set_q(mCoordinate[i].get_q() + 2*M_PI); + } } } // namespace dynamics diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index beec62cf70b07..f8ecf86fb4e33 100644 --- a/dart/dynamics/FreeJoint.h +++ b/dart/dynamics/FreeJoint.h @@ -56,12 +56,21 @@ class FreeJoint : 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(); + // Documentation inherited. virtual void clampRotation(); @@ -69,6 +78,10 @@ class FreeJoint : public Joint { /// \brief GenCoord mCoordinate[6]; + /// @brief + // TODO(JS): This is workaround for Issue #122. + Eigen::Isometry3d mT_Joint; + public: // EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index bbf96c5b16d81..0251d4345e86b 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -204,16 +204,25 @@ class Joint : public GenCoordSystem { /// q --> T(q) virtual void updateTransform() = 0; + /// @brief TODO(JS): This is workaround for Issue #122. + virtual void updateTransform_Issue122(double _timeStep) {} + /// \brief /// q, dq --> S(q), V(q, dq) /// V(q, dq) = S(q) * dq virtual void updateJacobian() = 0; + /// @brief TODO(JS): This is workaround for Issue #122. + virtual void updateJacobian_Issue122() {} + /// \brief /// dq, ddq, S(q) --> dS(q), dV(q, dq, ddq) /// dV(q, dq, ddq) = dS(q) * dq + S(q) * ddq virtual void updateJacobianTimeDeriv() = 0; + /// @brief TODO(JS): This is workaround for Issue #122. + virtual void updateJacobianTimeDeriv_Issue122() {} + //-------------------------------------------------------------------------- // //-------------------------------------------------------------------------- diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 54c8d00cade9f..7ecab8bef57f4 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -300,9 +300,17 @@ void Skeleton::setState(const Eigen::VectorXd& _state) { for (std::vector::iterator it = mBodyNodes.begin(); it != mBodyNodes.end(); ++it) { - (*it)->updateTransform(); - (*it)->updateVelocity(); - (*it)->updateEta(); + // TODO(JS): This is workaround for Issue #122. + if ((*it)->getParentJoint()->getJointType() == Joint::BALL + || (*it)->getParentJoint()->getJointType() == Joint::FREE) { + (*it)->updateTransform_Issue122(mTimeStep); + (*it)->updateVelocity(); + (*it)->updateEta_Issue122(); + } else { + (*it)->updateTransform(); + (*it)->updateVelocity(); + (*it)->updateEta(); + } } for (std::vector::reverse_iterator it = mBodyNodes.rbegin();