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 3 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
61 changes: 25 additions & 36 deletions dart/dynamics/BallJoint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,16 @@ const std::string& BallJoint::getStaticType()
return name;
}

//==============================================================================
void BallJoint::setTransformFromChildBodyNode(const Eigen::Isometry3d& T)
{
Joint::setTransformFromChildBodyNode(T);

// We update Jacobian here since the Jacobian of BallJoint is a function of
// "T" but not of joint positions.
mJacobian = math::getAdTMatrix(T).leftCols<3>();
Copy link
Member

Choose a reason for hiding this comment

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

Same thing here: I think this should be moved to the updateLocalJacobian() function and wrapped in the _mandatory flag.

Copy link
Member Author

Choose a reason for hiding this comment

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

👍

}

//==============================================================================
bool BallJoint::isCyclic(size_t _index) const
{
Expand Down Expand Up @@ -101,6 +111,10 @@ BallJoint::BallJoint(const Properties& _properties)
: MultiDofJoint<3>(_properties),
mR(Eigen::Isometry3d::Identity())
{
mJacobian
= math::getAdTMatrix(mJointP.mT_ChildBodyToJoint).leftCols<3>();
mJacobianDeriv = Eigen::Matrix<double, 6, 3>::Zero();

setProperties(_properties);
updateDegreeOfFreedomNames();
}
Expand All @@ -113,47 +127,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);

dq = Jw.inverse() * math::logMap(R1T * R2);
const Eigen::Matrix3d R1 = convertToRotation(_q1);
const Eigen::Matrix3d R2 = convertToRotation(_q2);

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

//==============================================================================
void BallJoint::integratePositions(double _dt)
{
const Eigen::Isometry3d& R = getR();
Eigen::Isometry3d Rnext(Eigen::Isometry3d::Identity());
Eigen::Matrix3d Rnext
= getR().linear() * convertToRotation(getVelocitiesStatic() * _dt);

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

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

//==============================================================================
Expand Down Expand Up @@ -181,20 +176,14 @@ void BallJoint::updateLocalTransform() const
//==============================================================================
void BallJoint::updateLocalJacobian(bool) const
{
mJacobian = getLocalJacobianStatic(getPositionsStatic());
// Do nothing. Jacobian will be updated by
// BallJoint::setTransformFromChildBodyNode() instead.
}

//==============================================================================
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
19 changes: 11 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,16 @@ 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;
void setTransformFromChildBodyNode(const Eigen::Isometry3d& T) override;

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

/// Get the Properties of this BallJoint
Properties getBallJointProperties() const;
Expand Down Expand Up @@ -105,24 +108,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 =true) const override;

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

protected:

Expand Down
86 changes: 23 additions & 63 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,30 @@ 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::Isometry3d T1 = convertToTransform(_q1);
const Eigen::Isometry3d T2 = convertToTransform(_q2);

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

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())
{
mJacobian = math::getAdTMatrix(mJointP.mT_ChildBodyToJoint);
mJacobianDeriv = Eigen::Matrix6d::Zero();

setProperties(_properties);
updateDegreeOfFreedomNames();
}
Expand All @@ -574,6 +555,16 @@ const std::string& FreeJoint::getStaticType()
return name;
}

//==============================================================================
void FreeJoint::setTransformFromChildBodyNode(const Eigen::Isometry3d& T)
{
Joint::setTransformFromChildBodyNode(T);

// We update Jacobian here since the Jacobian of FreeJoint is a function of
// "T" but not of joint positions.
mJacobian = math::getAdTMatrix(T);
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 we should move this to updateLocalJacobian() and call updateLocalJacobian() here. Then in the updateLocalJacobian() function we would wrap this up in the _mandatory flag so the computation only gets performed when something demands it.

Copy link
Member Author

Choose a reason for hiding this comment

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

👍 Then, I think we don't need this overriding function since updateLocalJacobian() is called in Joint::setTransformFromChildBodyNode().

Copy link
Member Author

Choose a reason for hiding this comment

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

By the way, don't we need _mandatory flag for updateLocalJacobianDeriv()?

Copy link
Member

Choose a reason for hiding this comment

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

The motivation of the _mandatory flag was because the function could be called either (1) when a property that the Jacobian depends on has changed or (2) when a state that the Jacobian might depend on has changed.

My impression with updateLocalJacobianDeriv() is that it's either always zero or always dependent on state. There wouldn't be a situation where it would depend on a property but not depend on a state. If I'm wrong about that, then we should definitely introduce a _mandatory flag for it. Although even if my assumption is correct, I would be okay with introducing a _mandatory flag for the sake of consistency if nothing else.

Copy link
Member Author

Choose a reason for hiding this comment

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

You are correct. I scanned all the Joints for double check. I don't think we need to introduce the flag since it seems the consistency is not that important here.

}

//==============================================================================
bool FreeJoint::isCyclic(size_t _index) const
{
Expand All @@ -584,15 +575,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 @@ -628,38 +612,14 @@ void FreeJoint::updateLocalTransform() const
//==============================================================================
void FreeJoint::updateLocalJacobian(bool) const
{
mJacobian = getLocalJacobian(getPositionsStatic());
// Do nothing. Jacobian will be updated by
// FreeJoint::setTransformFromChildBodyNode() instead.
}

//==============================================================================
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
15 changes: 9 additions & 6 deletions dart/dynamics/FreeJoint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,9 @@ class FreeJoint : public MultiDofJoint<6>
/// Get joint type for this class
static const std::string& getStaticType();

// Documentation inherited
void setTransformFromChildBodyNode(const Eigen::Isometry3d& T) override;

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

Expand Down Expand Up @@ -272,24 +275,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 =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
Loading