From 400ea38db41f36c56b49981b471bb7d78e1c1acb Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 15 Feb 2021 15:53:45 -0800 Subject: [PATCH 1/2] Compute actual Jacobian of BallJoint instead of returning identity --- dart/dynamics/BallJoint.cpp | 65 +++++++++++++------------ dart/dynamics/BallJoint.hpp | 10 ---- dart/math/Geometry.cpp | 65 +++++++++++++++++++++++++ dart/math/Geometry.hpp | 19 ++++++++ unittests/comprehensive/test_Joints.cpp | 12 ++--- 5 files changed, 123 insertions(+), 48 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index ebf22401e..e0a5ad654 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -95,8 +95,7 @@ Eigen::Matrix3d BallJoint::convertToRotation(const Eigen::Vector3d& _positions) //============================================================================== BallJoint::BallJoint(const Properties& properties) - : Base(properties), - mR(Eigen::Isometry3d::Identity()) + : Base(properties) { mJacobianDeriv = Eigen::Matrix::Zero(); @@ -114,9 +113,18 @@ Joint* BallJoint::clone() const //============================================================================== Eigen::Matrix BallJoint::getRelativeJacobianStatic( - const Eigen::Vector3d& /*positions*/) const + const Eigen::Vector3d& positions) const { - return mJacobian; + Eigen::Matrix J; + + const Eigen::Vector3d& q = positions; + const Eigen::Isometry3d& T = Joint::mAspectProperties.mT_ChildBodyToJoint; + + J.topRows(3).noalias() = T.rotation() * math::so3RightJacobian(q); + J.bottomRows(3).noalias() + = math::makeSkewSymmetric(T.translation()) * J.topRows(3); + + return J; } //============================================================================== @@ -125,15 +133,20 @@ Eigen::Vector3d BallJoint::getPositionDifferencesStatic( { const Eigen::Matrix3d R1 = convertToRotation(_q1); const Eigen::Matrix3d R2 = convertToRotation(_q2); + const Eigen::Matrix3d S = math::so3RightJacobian(_q1); - return convertToPositions(R1.transpose() * R2); + return S.inverse() * convertToPositions(R1.transpose() * R2); } //============================================================================== -void BallJoint::integratePositions(double _dt) +void BallJoint::integratePositions(double dt) { - Eigen::Matrix3d Rnext - = getR().linear() * convertToRotation(getVelocitiesStatic() * _dt); + const auto& q = getPositionsStatic(); + const auto& dq = getVelocitiesStatic(); + + const Eigen::Matrix3d S = math::so3RightJacobian(q); + const Eigen::Matrix3d Rnext + = convertToRotation(q) * convertToRotation(S * dq * dt); setPositionsStatic(convertToPositions(Rnext)); } @@ -212,28 +225,33 @@ void BallJoint::updateDegreeOfFreedomNames() //============================================================================== void BallJoint::updateRelativeTransform() const { - mR.linear() = convertToRotation(getPositionsStatic()); + Eigen::Isometry3d R = Eigen::Isometry3d::Identity(); + R.linear() = convertToRotation(getPositionsStatic()); - mT = Joint::mAspectProperties.mT_ParentBodyToJoint * mR + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * R * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } //============================================================================== -void BallJoint::updateRelativeJacobian(bool _mandatory) const +void BallJoint::updateRelativeJacobian(bool /*_mandatory*/) const { - if (_mandatory) - { - mJacobian = math::getAdTMatrix( - Joint::mAspectProperties.mT_ChildBodyToJoint).leftCols<3>(); - } + mJacobian = getRelativeJacobianStatic(getPositionsStatic()); } //============================================================================== void BallJoint::updateRelativeJacobianTimeDeriv() const { - assert(Eigen::Matrix6d::Zero().leftCols<3>() == mJacobianDeriv); + const auto& q = getPositionsStatic(); + const auto& dq = getVelocitiesStatic(); + const Eigen::Isometry3d& T = Joint::mAspectProperties.mT_ChildBodyToJoint; + + const Eigen::Matrix3d dJ = math::so3RightJacobianTimeDeriv(q, dq); + + mJacobianDeriv.topRows(3).noalias() = T.rotation() * dJ; + mJacobianDeriv.bottomRows(3).noalias() + = math::makeSkewSymmetric(T.translation()) * mJacobianDeriv.topRows(3); } //============================================================================== @@ -327,18 +345,5 @@ Eigen::Vector6d BallJoint::getScrewAxisGradientForForce( parentTransform * Joint::mAspectProperties.mT_ParentBodyToJoint, grad); } -//============================================================================== -const Eigen::Isometry3d& BallJoint::getR() const -{ - if(mNeedTransformUpdate) - { - updateRelativeTransform(); - mNeedTransformUpdate = false; - } - - return mR; -} - } // namespace dynamics } // namespace dart - diff --git a/dart/dynamics/BallJoint.hpp b/dart/dynamics/BallJoint.hpp index 5c502c580..084059572 100644 --- a/dart/dynamics/BallJoint.hpp +++ b/dart/dynamics/BallJoint.hpp @@ -167,16 +167,6 @@ class BallJoint : public GenericJoint // Documentation inherited void updateRelativeJacobianTimeDeriv() const override; -protected: - - /// Access mR, which is an auto-updating variable - const Eigen::Isometry3d& getR() const; - - /// Rotation matrix dependent on the generalized coordinates - /// - /// Do not use directly! Use getR() to access this - mutable Eigen::Isometry3d mR; - public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index 08ff63e13..746e06f5d 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -1605,6 +1605,71 @@ Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d& _angle) return ret; } +//============================================================================== +Eigen::Matrix3d so3LeftJacobian(const Eigen::Vector3d& w) +{ + return expMapJac(w); +} + +//============================================================================== +Eigen::Matrix3d so3RightJacobian(const Eigen::Vector3d& w) +{ + const double theta = w.norm(); + + Eigen::Matrix3d J = Eigen::Matrix3d::Zero(); + const Eigen::Matrix3d qss = math::makeSkewSymmetric(w); + const Eigen::Matrix3d qss2 = qss * qss; + + if (theta < EPSILON_EXPMAP_THETA) + J = Eigen::Matrix3d::Identity() - 0.5 * qss + (1.0 / 6.0) * qss2; + else + J = Eigen::Matrix3d::Identity() - ((1 - cos(theta)) / (theta * theta)) * qss + + ((theta - sin(theta)) / (theta * theta * theta)) * qss2; + + return J; +} + +//============================================================================== +Eigen::Matrix3d so3LeftJacobianTimeDeriv( + const Eigen::Vector3d& q, const Eigen::Vector3d& dq) +{ + return expMapJacDot(q, dq); +} + +//============================================================================== +Eigen::Matrix3d so3RightJacobianTimeDeriv( + const Eigen::Vector3d& q, const Eigen::Vector3d& dq) +{ + const double theta = q.norm(); + + Eigen::Matrix3d Jdot = Eigen::Matrix3d::Zero(); + const Eigen::Matrix3d qss = math::makeSkewSymmetric(q); + const Eigen::Matrix3d qss2 = qss * qss; + const Eigen::Matrix3d qdss = math::makeSkewSymmetric(dq); + const double ttdot = q.dot(dq); // theta*thetaDot + const double st = sin(theta); + const double ct = cos(theta); + const double t2 = theta * theta; + const double t3 = t2 * theta; + const double t4 = t3 * theta; + const double t5 = t4 * theta; + + if (theta < EPSILON_EXPMAP_THETA) + { + Jdot = -0.5 * qdss + (1.0 / 6.0) * (qss * qdss + qdss * qss); + Jdot += (1.0 / 12) * ttdot * qss + (-1.0 / 60) * ttdot * qss2; + } + else + { + Jdot = -((1 - ct) / t2) * qdss + + ((theta - st) / t3) * (qss * qdss + qdss * qss); + Jdot += -((theta * st + 2 * ct - 2) / t4) * ttdot * qss + + ((3 * st - theta * ct - 2 * theta) / t5) * ttdot * qss2; + } + + return Jdot; +} + // R = Exp(w) // p = sin(t) / t*v + (t - sin(t)) / t^3**w + (1 - cos(t)) / t^2*(w X v) // , when S = (w, v), t = |w| diff --git a/dart/math/Geometry.hpp b/dart/math/Geometry.hpp index 16f362fb7..cef8db1ae 100644 --- a/dart/math/Geometry.hpp +++ b/dart/math/Geometry.hpp @@ -159,6 +159,25 @@ Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d& _R); // Eigen::Vector3d matrixToEulerZYZ(const Eigen::Matrix3d& R); //------------------------------------------------------------------------------ + +/// Returns the Jacobian of an SO(3) element w.r.t. its exponential +/// coordinates where the Jacobian maps the time derivative of the exponential +/// coordinates to the angular velocity in the world frame. +Eigen::Matrix3d so3LeftJacobian(const Eigen::Vector3d& w); + +/// Returns the Jacobian of an SO(3) element w.r.t. its exponential +/// coordinates where the Jacobian maps the time derivative of the exponential +/// coordinates to the angular velocity in the body frame. +Eigen::Matrix3d so3RightJacobian(const Eigen::Vector3d& w); + +/// Returns the time derivative of the left Jacobian of SO(3). +Eigen::Matrix3d so3LeftJacobianTimeDeriv( + const Eigen::Vector3d& q, const Eigen::Vector3d& dq); + +/// Returns the time derivative of the right Jacobian of SO(3). +Eigen::Matrix3d so3RightJacobianTimeDeriv( + const Eigen::Vector3d& q, const Eigen::Vector3d& dq); + /// \brief Exponential mapping Eigen::Isometry3d expMap(const Eigen::Vector6d& _S); diff --git a/unittests/comprehensive/test_Joints.cpp b/unittests/comprehensive/test_Joints.cpp index a3fad8dea..f2269f8b5 100644 --- a/unittests/comprehensive/test_Joints.cpp +++ b/unittests/comprehensive/test_Joints.cpp @@ -326,14 +326,10 @@ TEST_F(JOINTS, TRANSLATIONAL_JOINT_2D) #endif // 3-dof joint -//TEST_F(JOINTS, BALL_JOINT) -//{ -// kinematicsTest(); -//} -// TODO(JS): Disabled the test compares analytical Jacobian and numerical -// Jacobian since the meaning of BallJoint Jacobian is changed per -// we now use angular velocity and angular accertions as BallJoint's generalized -// velocities and accelerations, repectively. +TEST_F(JOINTS, BALL_JOINT) +{ + kinematicsTest(); +} // 3-dof joint #ifdef ALL_TESTS From 452635803d3f08e3f8dad9eca73831673f8e503e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 15 Feb 2021 16:31:03 -0800 Subject: [PATCH 2/2] Print out commit ID --- ci/manylinux/tests/run_tests.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/ci/manylinux/tests/run_tests.sh b/ci/manylinux/tests/run_tests.sh index b775df0d1..f4c07483a 100755 --- a/ci/manylinux/tests/run_tests.sh +++ b/ci/manylinux/tests/run_tests.sh @@ -3,6 +3,7 @@ set -e VERSION=$(cat ../../../VERSION.txt) GIT_HASH=$(git log --pretty=format:'%H' -n 1) +echo "Testing commit: $GIT_HASH" docker build . --build-arg VERSION=${VERSION} --build-arg GIT_HASH=${GIT_HASH} IMAGE_ID=$(docker images | awk '{print $3}' | awk 'NR==2') echo "Build image ID $IMAGE_ID"