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

Fix incorrect rotational motion of BallJoint and FreeJoint #518

Merged
merged 6 commits into from
Oct 15, 2015
Merged

Fix incorrect rotational motion of BallJoint and FreeJoint #518

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f28d6e5
Change generalized coordinates of BallJoint and FreeJoint
jslee02 Oct 12, 2015
273376d
Add tests checking forward kinematics algorithm and finite difference…
jslee02 Oct 12, 2015
fbc4478
Revert unintended change
jslee02 Oct 12, 2015
4ff70fb
Use existing kinematics auto-update mechanism to update BallJoint/Fre…
jslee02 Oct 13, 2015
c3cd476
Merge remote-tracking branch 'origin/portable_uri' into freejoint_bal…
jslee02 Oct 13, 2015
95992d2
Don't need to compute Jacobians of BallJoint/FreeJoint in the constru…
jslee02 Oct 14, 2015
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
51 changes: 14 additions & 37 deletions dart/dynamics/BallJoint.cpp
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think if positions isn't going to be used at all here, then we should either

  1. Remove the positions argument entirely or
  2. Save the current positions in a temporary Eigen::Vector6d set the current positions to positions and then reset the positions to their original values at the end. If the incoming positions vector matches the current positions, then this shouldn't be very much overhead, because the bookkeeping should all shortcircuit.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh wait, I take that back. Does this function imply that the Jacobian is now completely agnostic to the current position values?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes. Previously a Jacobian of FreeJoint was dependent on joint positions like other joints but now it's only dependent on the transform from child BodyNode to the joint. Since this function is a virtual function declared in Joint so it still has positions argument.

{
// 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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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