diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 672ce503c3191..68299b00e1f26 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -49,15 +49,6 @@ BallJoint::BallJoint(const std::string& _name) : MultiDofJoint(_name), mR(Eigen::Isometry3d::Identity()) { - // Jacobian - Eigen::Matrix J = Eigen::Matrix::Zero(); - J.topRows<3>() = Eigen::Matrix3d::Identity(); - mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J.col(0)); - mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J.col(1)); - mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J.col(2)); - assert(!math::isNan(mJacobian)); - - // Time derivative of Jacobian is always zero } //============================================================================== @@ -65,25 +56,6 @@ BallJoint::~BallJoint() { } -//============================================================================== -void BallJoint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) -{ - Joint::setTransformFromChildBodyNode(_T); - - 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; - - mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J0); - mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J1); - mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J2); - - assert(!math::isNan(mJacobian)); -} - //============================================================================== void BallJoint::integratePositions(double _dt) { @@ -105,14 +77,25 @@ void BallJoint::updateLocalTransform() //============================================================================== void BallJoint::updateLocalJacobian() { - // Do nothing since the Jacobian is constant + Eigen::Matrix J; + J.topRows<3>() = math::expMapJac(mPositions).transpose(); + J.bottomRows<3>() = Eigen::Matrix3d::Zero(); + + mJacobian = math::AdTJacFixed(mT_ChildBodyToJoint, J); + + assert(!math::isNan(mJacobian)); } //============================================================================== void BallJoint::updateLocalJacobianTimeDeriv() { - // Time derivative of ball joint is always zero - assert(mJacobianDeriv == (Eigen::Matrix::Zero())); + Eigen::Matrix dJ; + dJ.topRows<3>() = math::expMapJacDot(mPositions, mVelocities).transpose(); + dJ.bottomRows<3>() = Eigen::Matrix3d::Zero(); + + mJacobianDeriv = math::AdTJacFixed(mT_ChildBodyToJoint, dJ); + + assert(!math::isNan(mJacobianDeriv)); } } // namespace dynamics diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h index 9f514b38cca6b..3e08d384ab855 100644 --- a/dart/dynamics/BallJoint.h +++ b/dart/dynamics/BallJoint.h @@ -54,9 +54,6 @@ class BallJoint : public MultiDofJoint<3> /// Destructor virtual ~BallJoint(); - // Documentation inherited - virtual void setTransformFromChildBodyNode(const Eigen::Isometry3d& _T); - // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const { diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 97ba02220700a..ace5a05ec09d5 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -49,17 +49,6 @@ FreeJoint::FreeJoint(const std::string& _name) : MultiDofJoint(_name), mQ(Eigen::Isometry3d::Identity()) { - // Jacobian - Eigen::Matrix6d J = Eigen::Matrix6d::Identity(); - mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J.col(0)); - mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J.col(1)); - mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J.col(2)); - mJacobian.col(3) = math::AdT(mT_ChildBodyToJoint, J.col(3)); - mJacobian.col(4) = math::AdT(mT_ChildBodyToJoint, J.col(4)); - mJacobian.col(5) = math::AdT(mT_ChildBodyToJoint, J.col(5)); - assert(!math::isNan(mJacobian)); - - // Time derivative of Jacobian is always zero } //============================================================================== @@ -67,27 +56,11 @@ FreeJoint::~FreeJoint() { } -//============================================================================== -void FreeJoint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) -{ - Joint::setTransformFromChildBodyNode(_T); - - Eigen::Matrix6d J = Eigen::Matrix6d::Identity(); - - mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J.col(0)); - mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J.col(1)); - mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J.col(2)); - mJacobian.col(3) = math::AdT(mT_ChildBodyToJoint, J.col(3)); - mJacobian.col(4) = math::AdT(mT_ChildBodyToJoint, J.col(4)); - mJacobian.col(5) = math::AdT(mT_ChildBodyToJoint, J.col(5)); - - assert(!math::isNan(mJacobian)); -} - //============================================================================== void FreeJoint::integratePositions(double _dt) { - mQ = mQ * math::expMap(mVelocities * _dt); + mQ.linear() = mQ.linear() * math::expMapRot(mVelocities.head<3>() * _dt); + mQ.translation() = mVelocities.tail<3>() * _dt; mPositions = math::logMap(mQ); } @@ -95,7 +68,8 @@ void FreeJoint::integratePositions(double _dt) //============================================================================== void FreeJoint::updateLocalTransform() { - mQ = math::expMap(mPositions); + mQ.linear() = math::expMapRot(mPositions.head<3>()); + mQ.translation() = mPositions.tail<3>(); mT = mT_ParentBodyToJoint * mQ * mT_ChildBodyToJoint.inverse(); @@ -105,14 +79,44 @@ void FreeJoint::updateLocalTransform() //============================================================================== void FreeJoint::updateLocalJacobian() { - // Do nothing since Jacobian is constant + Eigen::Matrix6d J = Eigen::Matrix6d::Identity(); + J.topLeftCorner<3,3>() = math::expMapJac(mPositions.head<3>()).transpose(); + + mJacobian.leftCols<3>() + = math::AdTJacFixed(mT_ChildBodyToJoint, J.leftCols<3>()); + mJacobian.rightCols<3>() + = math::AdTJacFixed(mT_ChildBodyToJoint + * math::expAngular(-mPositions.head<3>()), + J.rightCols<3>()); + + assert(!math::isNan(mJacobian)); } //============================================================================== void FreeJoint::updateLocalJacobianTimeDeriv() { - // Time derivative of Jacobian is constant - assert(mJacobianDeriv == (Eigen::Matrix6d::Zero())); + Eigen::Matrix J; + J.topRows<3>() = Eigen::Matrix3d::Zero(); + J.bottomRows<3>() = Eigen::Matrix3d::Identity(); + + Eigen::Matrix dJ; + dJ.topRows<3>() = math::expMapJacDot(mPositions.head<3>(), + mVelocities.head<3>()).transpose(); + dJ.bottomRows<3>() = Eigen::Matrix3d::Zero(); + + const Eigen::Isometry3d T = mT_ChildBodyToJoint + * math::expAngular(-mPositions.head<3>()); + + mJacobianDeriv.leftCols<3>() = math::AdTJacFixed(mT_ChildBodyToJoint, dJ); + mJacobianDeriv.col(3) + = -math::ad(mJacobian.leftCols<3>() * mVelocities.head<3>(), + math::AdT(T, J.col(0))); + mJacobianDeriv.col(4) + = -math::ad(mJacobian.leftCols<3>() * mVelocities.head<3>(), + math::AdT(T, J.col(1))); + mJacobianDeriv.col(5) + = -math::ad(mJacobian.leftCols<3>() * mVelocities.head<3>(), + math::AdT(T, J.col(2))); } } // namespace dynamics diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index 75525cc6e6a81..2081610541f06 100644 --- a/dart/dynamics/FreeJoint.h +++ b/dart/dynamics/FreeJoint.h @@ -56,9 +56,6 @@ class FreeJoint : public MultiDofJoint<6> /// Destructor virtual ~FreeJoint(); - // Documentation inherited - virtual void setTransformFromChildBodyNode(const Eigen::Isometry3d& _T); - // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const { diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 885e025cd6670..e6e133580db4d 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -481,10 +481,10 @@ class MultiDofJoint : public Joint // For recursive dynamics algorithms //---------------------------------------------------------------------------- - /// Spatial Jacobian + /// Spatial Jacobian expressed in the child body frame Eigen::Matrix mJacobian; - /// Time derivative of spatial Jacobian + /// Time derivative of spatial Jacobian expressed in the child body frame Eigen::Matrix mJacobianDeriv; /// Inverse of projected articulated inertia diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 40a2091ca906f..abbdd39132705 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -475,10 +475,10 @@ class SingleDofJoint : public Joint // For recursive dynamics algorithms //---------------------------------------------------------------------------- - /// Spatial Jacobian + /// Spatial Jacobian expressed in the child body frame Eigen::Vector6d mJacobian; - /// Time derivative of spatial Jacobian + /// Time derivative of spatial Jacobian expressed in the child body frame Eigen::Vector6d mJacobianDeriv; /// Inverse of projected articulated inertia diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 71a5c06fb63cb..eeb7c1ae1158c 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -428,20 +428,8 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) // ddq[k] = 0.0; } - skeleton->setPositions(q); - skeleton->setVelocities(dq); - skeleton->setAccelerations(ddq); - skeleton->computeForwardKinematics(true, true, true); - - // Integrate state - skeleton->integratePositions(timeStep); - skeleton->integrateVelocities(timeStep); - - // Compute forward kinematics - skeleton->computeForwardKinematics(true, true, true); - - VectorXd qNext = skeleton->getPositions(); - VectorXd dqNext = skeleton->getVelocities(); + VectorXd qNext = q + dq * timeStep; + VectorXd dqNext = dq + ddq * timeStep; // For each body node for (size_t k = 0; k < skeleton->getNumBodyNodes(); ++k) diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index e3bf654d7480f..2cc32ee4eeb0c 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -165,16 +165,10 @@ void JOINTS::kinematicsTest(Joint* _joint) numericJ.col(i) = Ji; } - if (dynamic_cast(_joint) || dynamic_cast(_joint)) - { - // Skip this test for BallJoint and FreeJoint since Jacobian of BallJoint - // and FreeJoint is not obtained by the above method. - } - else + for (int i = 0; i < dof; ++i) { - for (int i = 0; i < dof; ++i) - for (int j = 0; j < 6; ++j) - EXPECT_NEAR(J.col(i)(j), numericJ.col(i)(j), JOINT_TOL); + for (int j = 0; j < 6; ++j) + EXPECT_NEAR(J.col(i)(j), numericJ.col(i)(j), JOINT_TOL); } //-------------------------------------------------------------------------- @@ -204,18 +198,10 @@ void JOINTS::kinematicsTest(Joint* _joint) numeric_dJ += dJ_dq * dq(i); } - - if (dynamic_cast(_joint) || dynamic_cast(_joint)) - { - // Skip this test for BallJoint and FreeJoint since time derivative of - // Jacobian of BallJoint and FreeJoint is not obtained by the above - // method. - } - else + for (int i = 0; i < dof; ++i) { - for (int i = 0; i < dof; ++i) - for (int j = 0; j < 6; ++j) - EXPECT_NEAR(dJ.col(i)(j), numeric_dJ.col(i)(j), JOINT_TOL); + for (int j = 0; j < 6; ++j) + EXPECT_NEAR(dJ.col(i)(j), numeric_dJ.col(i)(j), JOINT_TOL); } } @@ -403,4 +389,3 @@ int main(int argc, char* argv[]) return RUN_ALL_TESTS(); } -