Skip to content

Commit

Permalink
Merge pull request #518 from dartsim/freejoint_balljoint
Browse files Browse the repository at this point in the history
Fix incorrect rotational motion of BallJoint and FreeJoint
  • Loading branch information
jslee02 committed Oct 15, 2015
2 parents 293b8fa + 95992d2 commit e0a1e05
Show file tree
Hide file tree
Showing 10 changed files with 354 additions and 133 deletions.
51 changes: 14 additions & 37 deletions dart/dynamics/BallJoint.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,8 @@ BallJoint::BallJoint(const Properties& _properties)
: MultiDofJoint<3>(_properties),
mR(Eigen::Isometry3d::Identity())
{
mJacobianDeriv = Eigen::Matrix<double, 6, 3>::Zero();

setProperties(_properties);
updateDegreeOfFreedomNames();
}
Expand All @@ -113,47 +115,28 @@ Joint* BallJoint::clone() const

//==============================================================================
Eigen::Matrix<double, 6, 3> BallJoint::getLocalJacobianStatic(
const Eigen::Vector3d& _positions) const
const Eigen::Vector3d& /*positions*/) const
{
// Jacobian expressed in the Joint frame
Eigen::Matrix<double, 6, 3> J;
J.topRows<3>() = math::expMapJac(-_positions);
J.bottomRows<3>() = Eigen::Matrix3d::Zero();

// Transform the reference frame to the child BodyNode frame
J = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint, J);

assert(!math::isNan(J));

return J;
return mJacobian;
}

//==============================================================================
Eigen::Vector3d BallJoint::getPositionDifferencesStatic(
const Eigen::Vector3d& _q2, const Eigen::Vector3d& _q1) const
{
Eigen::Vector3d dq;

const Eigen::Matrix3d Jw = getLocalJacobianStatic(_q1).topRows<3>();
const Eigen::Matrix3d R1T = math::expMapRot(-_q1);
const Eigen::Matrix3d R2 = math::expMapRot( _q2);
const Eigen::Matrix3d R1 = convertToRotation(_q1);
const Eigen::Matrix3d R2 = convertToRotation(_q2);

dq = Jw.inverse() * math::logMap(R1T * R2);

return dq;
return convertToPositions(R1.transpose() * R2);
}

//==============================================================================
void BallJoint::integratePositions(double _dt)
{
const Eigen::Isometry3d& R = getR();
Eigen::Isometry3d Rnext(Eigen::Isometry3d::Identity());

Rnext.linear() = R.linear()
* convertToRotation(getLocalJacobianStatic().topRows<3>()
* getVelocitiesStatic() * _dt);
Eigen::Matrix3d Rnext
= getR().linear() * convertToRotation(getVelocitiesStatic() * _dt);

setPositionsStatic(convertToPositions(Rnext.linear()));
setPositionsStatic(convertToPositions(Rnext));
}

//==============================================================================
Expand All @@ -179,22 +162,16 @@ void BallJoint::updateLocalTransform() const
}

//==============================================================================
void BallJoint::updateLocalJacobian(bool) const
void BallJoint::updateLocalJacobian(bool _mandatory) const
{
mJacobian = getLocalJacobianStatic(getPositionsStatic());
if (_mandatory)
mJacobian = math::getAdTMatrix(mJointP.mT_ChildBodyToJoint).leftCols<3>();
}

//==============================================================================
void BallJoint::updateLocalJacobianTimeDeriv() const
{
Eigen::Matrix<double, 6, 3> dJ;
dJ.topRows<3>() = math::expMapJacDot(getPositionsStatic(),
getVelocitiesStatic()).transpose();
dJ.bottomRows<3>() = Eigen::Matrix3d::Zero();

mJacobianDeriv = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint, dJ);

assert(!math::isNan(mJacobianDeriv));
assert(Eigen::Matrix6d::Zero().leftCols<3>() == mJacobianDeriv);
}

//==============================================================================
Expand Down
16 changes: 8 additions & 8 deletions dart/dynamics/BallJoint.h
Original file line number Diff line number Diff line change
Expand Up @@ -62,13 +62,13 @@ class BallJoint : public MultiDofJoint<3>
virtual ~BallJoint();

// Documentation inherited
virtual const std::string& getType() const override;
const std::string& getType() const override;

/// Get joint type for this class
static const std::string& getStaticType();

// Documentation inherited
virtual bool isCyclic(size_t _index) const override;
bool isCyclic(size_t _index) const override;

/// Get the Properties of this BallJoint
Properties getBallJointProperties() const;
Expand Down Expand Up @@ -105,24 +105,24 @@ class BallJoint : public MultiDofJoint<3>
BallJoint(const Properties& _properties);

// Documentation inherited
virtual Joint* clone() const override;
Joint* clone() const override;

using MultiDofJoint::getLocalJacobianStatic;

// Documentation inherited
virtual void integratePositions(double _dt) override;
void integratePositions(double _dt) override;

// Documentation inherited
virtual void updateDegreeOfFreedomNames() override;
void updateDegreeOfFreedomNames() override;

// Documentation inherited
virtual void updateLocalTransform() const override;
void updateLocalTransform() const override;

// Documentation inherited
virtual void updateLocalJacobian(bool =true) const override;
void updateLocalJacobian(bool _mandatory = true) const override;

// Documentation inherited
virtual void updateLocalJacobianTimeDeriv() const override;
void updateLocalJacobianTimeDeriv() const override;

protected:

Expand Down
77 changes: 13 additions & 64 deletions dart/dynamics/FreeJoint.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -508,49 +508,29 @@ void FreeJoint::setAngularAcceleration(

//==============================================================================
Eigen::Matrix6d FreeJoint::getLocalJacobianStatic(
const Eigen::Vector6d& _positions) const
const Eigen::Vector6d& /*positions*/) const
{
// Jacobian expressed in the Joint frame
Eigen::Matrix6d J = Eigen::Matrix6d::Identity();
J.topLeftCorner<3,3>() = math::expMapJac(-_positions.head<3>());

// Transform the reference frame to the child BodyNode frame
J.leftCols<3>() = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint,
J.leftCols<3>());
J.bottomRightCorner<3,3>()
= mJointP.mT_ChildBodyToJoint.linear()
* math::expMapRot(-_positions.head<3>());

// Note that the top right 3x3 block of J is always zero
assert((J.topRightCorner<3,3>()) == Eigen::Matrix3d::Zero());

assert(!math::isNan(J));

return J;
return mJacobian;
}

//==============================================================================
Eigen::Vector6d FreeJoint::getPositionDifferencesStatic(
const Eigen::Vector6d& _q2,
const Eigen::Vector6d& _q1) const
{
Eigen::Vector6d dq;

const Eigen::Matrix3d Jw = getLocalJacobianStatic(_q1).topLeftCorner<3,3>();
const Eigen::Matrix3d R1T = math::expMapRot(-_q1.head<3>());
const Eigen::Matrix3d R2 = math::expMapRot( _q2.head<3>());
const Eigen::Isometry3d T1 = convertToTransform(_q1);
const Eigen::Isometry3d T2 = convertToTransform(_q2);

dq.head<3>() = Jw.inverse() * math::logMap(R1T * R2);
dq.tail<3>() = _q2.tail<3>() - _q1.tail<3>();

return dq;
return convertToPositions(T1.inverse() * T2);
}

//==============================================================================
FreeJoint::FreeJoint(const Properties& _properties)
: MultiDofJoint<6>(_properties),
mQ(Eigen::Isometry3d::Identity())
{
mJacobianDeriv = Eigen::Matrix6d::Zero();

setProperties(_properties);
updateDegreeOfFreedomNames();
}
Expand Down Expand Up @@ -584,15 +564,8 @@ bool FreeJoint::isCyclic(size_t _index) const
//==============================================================================
void FreeJoint::integratePositions(double _dt)
{
const Eigen::Vector6d& velocities = getVelocitiesStatic();

const Eigen::Isometry3d& Q = getQ();
Eigen::Isometry3d Qnext(Eigen::Isometry3d::Identity());

Qnext.linear() = Q.linear()
* math::expMapRot(getLocalJacobianStatic().topLeftCorner<3,3>()
* velocities.head<3>() * _dt);
Qnext.translation() = Q.translation() + velocities.tail<3>() * _dt;
const Eigen::Isometry3d Qnext
= getQ() * convertToTransform(getVelocitiesStatic() * _dt);

setPositionsStatic(convertToPositions(Qnext));
}
Expand Down Expand Up @@ -626,40 +599,16 @@ void FreeJoint::updateLocalTransform() const
}

//==============================================================================
void FreeJoint::updateLocalJacobian(bool) const
void FreeJoint::updateLocalJacobian(bool _mandatory) const
{
mJacobian = getLocalJacobian(getPositionsStatic());
if (_mandatory)
mJacobian = math::getAdTMatrix(mJointP.mT_ChildBodyToJoint);
}

//==============================================================================
void FreeJoint::updateLocalJacobianTimeDeriv() const
{
Eigen::Matrix<double, 6, 3> J;
J.topRows<3>() = Eigen::Matrix3d::Zero();
J.bottomRows<3>() = Eigen::Matrix3d::Identity();

const Eigen::Vector6d& positions = getPositionsStatic();
const Eigen::Vector6d& velocities = getVelocitiesStatic();
Eigen::Matrix<double, 6, 3> dJ;
dJ.topRows<3>() = math::expMapJacDot(positions.head<3>(),
velocities.head<3>()).transpose();
dJ.bottomRows<3>() = Eigen::Matrix3d::Zero();

const Eigen::Isometry3d T = mJointP.mT_ChildBodyToJoint
* math::expAngular(-positions.head<3>());

mJacobianDeriv.leftCols<3>() =
math::AdTJacFixed(mJointP.mT_ChildBodyToJoint, dJ);
const Eigen::Matrix<double, 6, 6>& Jacobian = getLocalJacobianStatic();
mJacobianDeriv.col(3)
= -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
math::AdT(T, J.col(0)));
mJacobianDeriv.col(4)
= -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
math::AdT(T, J.col(1)));
mJacobianDeriv.col(5)
= -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
math::AdT(T, J.col(2)));
assert(Eigen::Matrix6d::Zero() == mJacobianDeriv);
}

//==============================================================================
Expand Down
12 changes: 6 additions & 6 deletions dart/dynamics/FreeJoint.h
Original file line number Diff line number Diff line change
Expand Up @@ -272,24 +272,24 @@ class FreeJoint : public MultiDofJoint<6>
FreeJoint(const Properties& _properties);

// Documentation inherited
virtual Joint* clone() const override;
Joint* clone() const override;

using MultiDofJoint::getLocalJacobianStatic;

// Documentation inherited
virtual void integratePositions(double _dt) override;
void integratePositions(double _dt) override;

// Documentation inherited
virtual void updateDegreeOfFreedomNames() override;
void updateDegreeOfFreedomNames() override;

// Documentation inherited
virtual void updateLocalTransform() const override;
void updateLocalTransform() const override;

// Documentation inherited
virtual void updateLocalJacobian(bool =true) const override;
void updateLocalJacobian(bool _mandatory = true) const override;

// Documentation inherited
virtual void updateLocalJacobianTimeDeriv() const override;
void updateLocalJacobianTimeDeriv() const override;

protected:

Expand Down
4 changes: 2 additions & 2 deletions dart/dynamics/Joint.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -386,13 +386,13 @@ void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T)
}

//==============================================================================
const Eigen::Isometry3d&Joint::getTransformFromParentBodyNode() const
const Eigen::Isometry3d& Joint::getTransformFromParentBodyNode() const
{
return mJointP.mT_ParentBodyToJoint;
}

//==============================================================================
const Eigen::Isometry3d&Joint::getTransformFromChildBodyNode() const
const Eigen::Isometry3d& Joint::getTransformFromChildBodyNode() const
{
return mJointP.mT_ChildBodyToJoint;
}
Expand Down
14 changes: 14 additions & 0 deletions dart/math/Geometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -635,6 +635,20 @@ Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V) {
return res;
}

//==============================================================================
Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T)
{
Eigen::Matrix6d AdT;

AdT.topLeftCorner<3, 3>() = T.linear();
AdT.topRightCorner<3, 3>().setZero();
AdT.bottomLeftCorner<3, 3>()
= makeSkewSymmetric(T.translation()) * T.linear();
AdT.bottomRightCorner<3, 3>() = T.linear();

return AdT;
}

Eigen::Vector6d AdR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V) {
//--------------------------------------------------------------------------
// w' = R*w
Expand Down
4 changes: 3 additions & 1 deletion dart/math/Geometry.h
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,6 @@
#define DART_MATH_GEOMETRY_H_

#include <Eigen/Dense>
#include <Eigen/SVD>

#include "dart/math/MathTypes.h"

Expand Down Expand Up @@ -200,6 +199,9 @@ Eigen::Vector6d logMap(const Eigen::Isometry3d& _T);
/// where @f$T=(R,p)@in SE(3), @quad V=(w,v)@in se(3) @f$.
Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);

/// \brief Get linear transformation matrix of Adjoint mapping
Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T);

/// Adjoint mapping for dynamic size Jacobian
template<typename Derived>
typename Derived::PlainObject AdTJac(const Eigen::Isometry3d& _T,
Expand Down
17 changes: 17 additions & 0 deletions dart/math/Helpers.h
Original file line number Diff line number Diff line change
Expand Up @@ -281,6 +281,23 @@ Eigen::Matrix<double, N, 1> randomVector(double _limit)
return randomVector<N>(-std::abs(_limit), std::abs(_limit));
}

//==============================================================================
inline Eigen::VectorXd randomVectorXd(size_t size, double min, double max)
{
Eigen::VectorXd v = Eigen::VectorXd::Zero(size);

for (size_t i = 0; i < size; ++i)
v[i] = random(min, max);

return v;
}

//==============================================================================
inline Eigen::VectorXd randomVectorXd(size_t size, double limit)
{
return randomVectorXd(size, -std::abs(limit), std::abs(limit));
}

} // namespace math

namespace Color
Expand Down
Loading

0 comments on commit e0a1e05

Please sign in to comment.