Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Consistent Jacobian and its derivatives of BallJoint and FreeJoint #284

Merged
merged 1 commit into from
Dec 11, 2014
Merged

Consistent Jacobian and its derivatives of BallJoint and FreeJoint #284

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 14 additions & 31 deletions dart/dynamics/BallJoint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,41 +49,13 @@ BallJoint::BallJoint(const std::string& _name)
: MultiDofJoint(_name),
mR(Eigen::Isometry3d::Identity())
{
// Jacobian
Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::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
}

//==============================================================================
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)
{
Expand All @@ -105,14 +77,25 @@ void BallJoint::updateLocalTransform()
//==============================================================================
void BallJoint::updateLocalJacobian()
{
// Do nothing since the Jacobian is constant
Eigen::Matrix<double, 6, 3> 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<double, 6, 3>::Zero()));
Eigen::Matrix<double, 6, 3> 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
Expand Down
3 changes: 0 additions & 3 deletions dart/dynamics/BallJoint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
70 changes: 37 additions & 33 deletions dart/dynamics/FreeJoint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,53 +49,27 @@ 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
}

//==============================================================================
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);
}

//==============================================================================
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();

Expand All @@ -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<double, 6, 3> J;
J.topRows<3>() = Eigen::Matrix3d::Zero();
J.bottomRows<3>() = Eigen::Matrix3d::Identity();

Eigen::Matrix<double, 6, 3> 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
Expand Down
3 changes: 0 additions & 3 deletions dart/dynamics/FreeJoint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
4 changes: 2 additions & 2 deletions dart/dynamics/MultiDofJoint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -481,10 +481,10 @@ class MultiDofJoint : public Joint
// For recursive dynamics algorithms
//----------------------------------------------------------------------------

/// Spatial Jacobian
/// Spatial Jacobian expressed in the child body frame
Eigen::Matrix<double, 6, DOF> mJacobian;

/// Time derivative of spatial Jacobian
/// Time derivative of spatial Jacobian expressed in the child body frame
Eigen::Matrix<double, 6, DOF> mJacobianDeriv;

/// Inverse of projected articulated inertia
Expand Down
4 changes: 2 additions & 2 deletions dart/dynamics/SingleDofJoint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
16 changes: 2 additions & 14 deletions unittests/testDynamics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
27 changes: 6 additions & 21 deletions unittests/testJoints.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -165,16 +165,10 @@ void JOINTS::kinematicsTest(Joint* _joint)
numericJ.col(i) = Ji;
}

if (dynamic_cast<BallJoint*>(_joint) || dynamic_cast<FreeJoint*>(_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);
}

//--------------------------------------------------------------------------
Expand Down Expand Up @@ -204,18 +198,10 @@ void JOINTS::kinematicsTest(Joint* _joint)
numeric_dJ += dJ_dq * dq(i);
}


if (dynamic_cast<BallJoint*>(_joint) || dynamic_cast<FreeJoint*>(_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);
}
}

Expand Down Expand Up @@ -403,4 +389,3 @@ int main(int argc, char* argv[])
return RUN_ALL_TESTS();
}