Skip to content

Commit

Permalink
Merge pull request #389 from dartsim/finite_differences
Browse files Browse the repository at this point in the history 
Differences of generalized coordinates
  • Loading branch information
@karenliu
karenliu committed May 26, 2015
2 parents 82f6e77 + af95121 commit d1ec56c
Show file tree
Hide file tree
Showing 21 changed files with 571 additions and 136 deletions.
48 changes: 41 additions & 7 deletions dart/dynamics/BallJoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,46 @@ Eigen::Matrix3d BallJoint::convertToRotation(const Eigen::Vector3d& _positions)
return math::expMapRot(_positions);
}

//==============================================================================
void BallJoint::setPositionsStatic(const Eigen::Vector3d& _positions)
{
mR = convertToRotation(_positions);

MultiDofJoint::setPositionsStatic(_positions);
}

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

const Eigen::Matrix3d Jw = getLocalJacobianStatic(_q0).topRows<3>();
const Eigen::Matrix3d R0T = math::expMapRot(-_q0);
const Eigen::Matrix3d R1 = math::expMapRot( _q1);

dq = Jw.inverse() * math::logMap(R0T * R1);

return dq;
}

//==============================================================================
Eigen::Matrix<double, 6, 3> BallJoint::getLocalJacobianStatic(
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(mT_ChildBodyToJoint, J);

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

return J;
}

//==============================================================================
void BallJoint::integratePositions(double _dt)
{
Expand Down Expand Up @@ -104,13 +144,7 @@ void BallJoint::updateLocalTransform() const
//==============================================================================
void BallJoint::updateLocalJacobian(bool) const
{
Eigen::Matrix<double, 6, 3> J;
J.topRows<3>() = math::expMapJac(getPositionsStatic()).transpose();
J.bottomRows<3>() = Eigen::Matrix3d::Zero();

mJacobian = math::AdTJacFixed(mT_ChildBodyToJoint, J);

assert(!math::isNan(mJacobian));
mJacobian = getLocalJacobian(getPositionsStatic());
}

//==============================================================================
Expand Down
14 changes: 14 additions & 0 deletions dart/dynamics/BallJoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,21 @@ class BallJoint : public MultiDofJoint<3>
/// Convert a BallJoint-style position vector into a rotation matrix
static Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions);

// Documentation inherited
void setPositionsStatic(const Eigen::Vector3d& _positions) override;

// Documentation inherited
Eigen::Vector3d getPositionDifferencesStatic(
const Eigen::Vector3d& _q0, const Eigen::Vector3d& _q1) const override;

// Documentation inherited
Eigen::Matrix<double, 6, 3> getLocalJacobianStatic(
const Eigen::Vector3d& _positions) const override;

protected:

using MultiDofJoint::getLocalJacobianStatic;

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

Expand Down
122 changes: 66 additions & 56 deletions dart/dynamics/EulerJoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,52 +116,14 @@ Eigen::Matrix3d EulerJoint::convertToRotation(const Eigen::Vector3d& _positions)
}

//==============================================================================
void EulerJoint::updateDegreeOfFreedomNames()
Eigen::Matrix<double, 6, 3> EulerJoint::getLocalJacobianStatic(
const Eigen::Vector3d& _positions) const
{
std::vector<std::string> affixes;
switch (mAxisOrder)
{
case AO_ZYX:
affixes.push_back("_z");
affixes.push_back("_y");
affixes.push_back("_x");
break;
case AO_XYZ:
affixes.push_back("_x");
affixes.push_back("_y");
affixes.push_back("_z");
break;
default:
dterr << "Unsupported axis order in EulerJoint named '" << mName
<< "' (" << mAxisOrder << ")\n";
}
Eigen::Matrix<double, 6, 3> J;

if (affixes.size() == 3)
{
for (size_t i = 0; i < 3; ++i)
{
if(!mDofs[i]->isNamePreserved())
mDofs[i]->setName(mName + affixes[i], false);
}
}
}

//==============================================================================
void EulerJoint::updateLocalTransform() const
{
mT = mT_ParentBodyToJoint * convertToTransform(getPositionsStatic())
* mT_ChildBodyToJoint.inverse();

assert(math::verifyTransform(mT));
}

//==============================================================================
void EulerJoint::updateLocalJacobian(bool) const
{
// double q0 = mPositions[0];
const Eigen::Vector3d& positions = getPositionsStatic();
double q1 = positions[1];
double q2 = positions[2];
// double q0 = _positions[0];
const double q1 = _positions[1];
const double q2 = _positions[2];

// double c0 = cos(q0);
double c1 = cos(q1);
Expand Down Expand Up @@ -195,9 +157,9 @@ void EulerJoint::updateLocalJacobian(bool) const
if (fabs(getPositionsStatic()[1]) == DART_PI * 0.5)
std::cout << "Singular configuration in ZYX-euler joint ["
<< mName << "]. ("
<< positions[0] << ", "
<< positions[1] << ", "
<< positions[2] << ")"
<< _positions[0] << ", "
<< _positions[1] << ", "
<< _positions[2] << ")"
<< std::endl;
#endif

Expand All @@ -218,12 +180,12 @@ void EulerJoint::updateLocalJacobian(bool) const
J2 << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;

#ifndef NDEBUG
if (fabs(positions[1]) == DART_PI * 0.5)
if (fabs(_positions[1]) == DART_PI * 0.5)
std::cout << "Singular configuration in ZYX-euler joint ["
<< mName << "]. ("
<< positions[0] << ", "
<< positions[1] << ", "
<< positions[2] << ")"
<< _positions[0] << ", "
<< _positions[1] << ", "
<< _positions[2] << ")"
<< std::endl;
#endif

Expand All @@ -236,14 +198,14 @@ void EulerJoint::updateLocalJacobian(bool) const
}
}

mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
J.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
J.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
J.col(2) = math::AdT(mT_ChildBodyToJoint, J2);

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

#ifndef NDEBUG
Eigen::MatrixXd JTJ = mJacobian.transpose() * mJacobian;
Eigen::MatrixXd JTJ = J.transpose() * J;
Eigen::FullPivLU<Eigen::MatrixXd> luJTJ(JTJ);
// Eigen::FullPivLU<Eigen::MatrixXd> luS(mS);
double det = luJTJ.determinant();
Expand All @@ -257,6 +219,54 @@ void EulerJoint::updateLocalJacobian(bool) const
// std::cout << "mS: \n" << mS << std::endl;
}
#endif

return J;
}

//==============================================================================
void EulerJoint::updateDegreeOfFreedomNames()
{
std::vector<std::string> affixes;
switch (mAxisOrder)
{
case AO_ZYX:
affixes.push_back("_z");
affixes.push_back("_y");
affixes.push_back("_x");
break;
case AO_XYZ:
affixes.push_back("_x");
affixes.push_back("_y");
affixes.push_back("_z");
break;
default:
dterr << "Unsupported axis order in EulerJoint named '" << mName
<< "' (" << mAxisOrder << ")\n";
}

if (affixes.size() == 3)
{
for (size_t i = 0; i < 3; ++i)
{
if(!mDofs[i]->isNamePreserved())
mDofs[i]->setName(mName + affixes[i], false);
}
}
}

//==============================================================================
void EulerJoint::updateLocalTransform() const
{
mT = mT_ParentBodyToJoint * convertToTransform(getPositionsStatic())
* mT_ChildBodyToJoint.inverse();

assert(math::verifyTransform(mT));
}

//==============================================================================
void EulerJoint::updateLocalJacobian(bool) const
{
mJacobian = getLocalJacobianStatic(getPositionsStatic());
}

//==============================================================================
Expand Down
7 changes: 7 additions & 0 deletions dart/dynamics/EulerJoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,14 @@ class EulerJoint : public MultiDofJoint<3>

Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions) const;

// Documentation inherited
Eigen::Matrix<double, 6, 3> getLocalJacobianStatic(
const Eigen::Vector3d& _positions) const override;

protected:

using MultiDofJoint::getLocalJacobianStatic;

/// Set the names of this joint's DegreesOfFreedom. Used during construction
/// and when axis order is changed.
virtual void updateDegreeOfFreedomNames();
Expand Down
65 changes: 51 additions & 14 deletions dart/dynamics/FreeJoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,12 +76,60 @@ Eigen::Isometry3d FreeJoint::convertToTransform(
return tf;
}

//==============================================================================
void FreeJoint::setPositionsStatic(const Eigen::Vector6d& _positions)
{
mQ = convertToTransform(_positions);

MultiDofJoint::setPositionsStatic(_positions);
}

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

const Eigen::Matrix3d Jw = getLocalJacobianStatic(_q0).topLeftCorner<3,3>();
const Eigen::Matrix3d R0T = math::expMapRot(-_q0.head<3>());
const Eigen::Matrix3d R1 = math::expMapRot( _q1.head<3>());

dq.head<3>() = Jw.inverse() * math::logMap(R0T * R1);
dq.tail<3>() = _q1.tail<3>() - _q0.tail<3>();

return dq;
}

//==============================================================================
Eigen::Matrix6d FreeJoint::getLocalJacobianStatic(
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(mT_ChildBodyToJoint, J.leftCols<3>());
J.bottomRightCorner<3,3>()
= 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;
}

//==============================================================================
void FreeJoint::integratePositions(double _dt)
{
const Eigen::Vector6d& velocities = getVelocitiesStatic();
mQ.linear() = mQ.linear() * math::expMapRot(
getLocalJacobianStatic().topRows<3>() * velocities * _dt);

mQ.linear() = mQ.linear()
* math::expMapRot(getLocalJacobianStatic().topLeftCorner<3,3>()
* velocities.head<3>() * _dt);
mQ.translation() = mQ.translation() + velocities.tail<3>() * _dt;

setPositionsStatic(convertToPositions(mQ));
Expand Down Expand Up @@ -117,18 +165,7 @@ void FreeJoint::updateLocalTransform() const
//==============================================================================
void FreeJoint::updateLocalJacobian(bool) const
{
const Eigen::Vector6d& positions = getPositionsStatic();
Eigen::Matrix6d J = Eigen::Matrix6d::Identity();
J.topLeftCorner<3,3>() = math::expMapJac(positions.head<3>()).transpose();

mJacobian.leftCols<3>()
= math::AdTJacFixed(mT_ChildBodyToJoint, J.leftCols<3>());
mJacobian.rightCols<3>()
= math::AdTJacFixed(mT_ChildBodyToJoint
* math::expAngular(-positions.head<3>()),
J.rightCols<3>());

assert(!math::isNan(mJacobian));
mJacobian = getLocalJacobian(getPositionsStatic());
}

//==============================================================================
Expand Down
14 changes: 14 additions & 0 deletions dart/dynamics/FreeJoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,21 @@ class FreeJoint : public MultiDofJoint<6>
/// Convert a FreeJoint-style 6D vector into a transform
static Eigen::Isometry3d convertToTransform(const Eigen::Vector6d& _positions);

// Documentation inherited
void setPositionsStatic(const Eigen::Vector6d& _positions) override;

// Documentation inherited
Eigen::Matrix6d getLocalJacobianStatic(
const Eigen::Vector6d& _positions) const override;

// Documentation inherited
Eigen::Vector6d getPositionDifferencesStatic(
const Eigen::Vector6d& _q0, const Eigen::Vector6d& _q1) const override;

protected:

using MultiDofJoint::getLocalJacobianStatic;

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

Expand Down
Loading

0 comments on commit d1ec56c

Please sign in to comment.