//==============================================================================
void PlanarJoint::updateLocalJacobianTimeDeriv() const
{
Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
J.block<3, 1>(3, 0) = mPlanarP.mTransAxis1;
J.block<3, 1>(3, 1) = mPlanarP.mTransAxis2;
J.block<3, 1>(0, 2) = mPlanarP.mRotAxis;
const Eigen::Matrix<double, 6, 3>& Jacobian = getLocalJacobianStatic();
const Eigen::Vector3d& velocities = getVelocitiesStatic();
mJacobianDeriv.col(0)
= -math::ad(Jacobian.col(2) * velocities[2],
math::AdT(mJointP.mT_ChildBodyToJoint
* math::expAngular(mPlanarP.mRotAxis
* -getPositionsStatic()[2]),
J.col(0)));
mJacobianDeriv.col(1)
= -math::ad(Jacobian.col(2) * velocities[2],
math::AdT(mJointP.mT_ChildBodyToJoint
* math::expAngular(mPlanarP.mRotAxis
* -getPositionsStatic()[2]),
J.col(1)));
assert(mJacobianDeriv.col(2) == Eigen::Vector6d::Zero());
assert(!math::isNan(mJacobianDeriv.col(0)));
assert(!math::isNan(mJacobianDeriv.col(1)));
}
//==============================================================================
void BallJoint::integratePositions(double _dt)
{
Eigen::Matrix3d Rnext
= getR().linear() * convertToRotation(getVelocitiesStatic() * _dt);
setPositionsStatic(convertToPositions(Rnext));
}
//==============================================================================
void FreeJoint::integratePositions(double _dt)
{
const Eigen::Isometry3d Qnext
= getQ() * convertToTransform(getVelocitiesStatic() * _dt);
setPositionsStatic(convertToPositions(Qnext));
}
//==============================================================================
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 = mT_ChildBodyToJoint
* math::expAngular(-positions.head<3>());
mJacobianDeriv.leftCols<3>() = math::AdTJacFixed(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)));
}
//==============================================================================
void FreeJoint::setRelativeSpatialAcceleration(
const Eigen::Vector6d& newSpatialAcceleration)
{
const Eigen::Matrix6d& J = getLocalJacobianStatic();
const Eigen::Matrix6d& dJ = getLocalJacobianTimeDerivStatic();
setAccelerationsStatic(
J.inverse() * (newSpatialAcceleration - dJ * getVelocitiesStatic()));
}
//==============================================================================
void FreeJoint::integratePositions(double _dt)
{
const Eigen::Vector6d& velocities = getVelocitiesStatic();
mQ.linear() = mQ.linear() * math::expMapRot(
getLocalJacobianStatic().topRows<3>() * velocities * _dt);
mQ.translation() = mQ.translation() + velocities.tail<3>() * _dt;
setPositionsStatic(convertToPositions(mQ));
}
//==============================================================================
void FreeJoint::setSpatialAcceleration(
const Eigen::Vector6d& newSpatialAcceleration,
const Frame* relativeTo,
const Frame* inCoordinatesOf)
{
assert(nullptr != relativeTo);
assert(nullptr != inCoordinatesOf);
if (getChildBodyNode() == relativeTo)
{
dtwarn << "[FreeJoint::setSpatialAcceleration] Invalid reference "
<< "frame for newSpatialAcceleration. It shouldn't be the child "
<< "BodyNode.\n";
return;
}
// Change the reference frame of "newSpatialAcceleration" to the child body
// node frame.
Eigen::Vector6d targetRelSpatialAcc = newSpatialAcceleration;
if (getChildBodyNode() != inCoordinatesOf)
{
targetRelSpatialAcc
= math::AdR(inCoordinatesOf->getTransform(getChildBodyNode()),
newSpatialAcceleration);
}
// Compute the target relative spatial acceleration from the parent body node
// to the child body node.
if (getChildBodyNode()->getParentFrame() != relativeTo)
{
if (relativeTo->isWorld())
{
const Eigen::Vector6d parentAcceleration
= math::AdInvT(
getLocalTransform(),
getChildBodyNode()->getParentFrame()->getSpatialAcceleration())
+ math::ad(getChildBodyNode()->getSpatialVelocity(),
getLocalJacobianStatic() * getVelocitiesStatic());
targetRelSpatialAcc -= parentAcceleration;
}
else
{
const Eigen::Vector6d parentAcceleration
= math::AdInvT(
getLocalTransform(),
getChildBodyNode()->getParentFrame()->getSpatialAcceleration())
+ math::ad(getChildBodyNode()->getSpatialVelocity(),
getLocalJacobianStatic() * getVelocitiesStatic());
const Eigen::Vector6d arbitraryAcceleration =
math::AdT(relativeTo->getTransform(getChildBodyNode()),
relativeTo->getSpatialAcceleration())
- math::ad(getChildBodyNode()->getSpatialVelocity(),
math::AdT(relativeTo->getTransform(getChildBodyNode()),
relativeTo->getSpatialVelocity()));
targetRelSpatialAcc += -parentAcceleration + arbitraryAcceleration;
}
}
setRelativeSpatialAcceleration(targetRelSpatialAcc);
}
//==============================================================================
void EulerJoint::updateRelativeJacobianTimeDeriv() const
{
// double q0 = mPositions[0];
const Eigen::Vector3d& positions = getPositionsStatic();
double q1 = positions[1];
double q2 = positions[2];
// double dq0 = mVelocities[0];
const Eigen::Vector3d& velocities = getVelocitiesStatic();
double dq1 = velocities[1];
double dq2 = velocities[2];
// double c0 = cos(q0);
double c1 = cos(q1);
double c2 = cos(q2);
// double s0 = sin(q0);
double s1 = sin(q1);
double s2 = sin(q2);
Eigen::Vector6d dJ0 = Eigen::Vector6d::Zero();
Eigen::Vector6d dJ1 = Eigen::Vector6d::Zero();
Eigen::Vector6d dJ2 = Eigen::Vector6d::Zero();
switch (getAxisOrder())
{
case AxisOrder::XYZ:
{
//------------------------------------------------------------------------
// dS = [ -(dq1*c2*s1) - dq2*c1*s2, dq2*c2, 0
// -(dq2*c1*c2) + dq1*s1*s2, -(dq2*s2), 0
// dq1*c1, 0, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//------------------------------------------------------------------------
dJ0 << -(dq1*c2*s1) - dq2*c1*s2, -(dq2*c1*c2) + dq1*s1*s2, dq1*c1,
0, 0, 0;
dJ1 << dq2*c2, -(dq2*s2), 0.0, 0.0, 0.0, 0.0;
dJ2.setZero();
break;
}
case AxisOrder::ZYX:
{
//------------------------------------------------------------------------
// dS = [ -c1*dq1, 0, 0
// c2*c1*dq2-s2*s1*dq1, -s2*dq2, 0
// -s1*c2*dq1-c1*s2*dq2, -c2*dq2, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//------------------------------------------------------------------------
dJ0 << -c1*dq1, c2*c1*dq2 - s2*s1*dq1, -s1*c2*dq1 - c1*s2*dq2,
0.0, 0.0, 0.0;
dJ1 << 0.0, -s2*dq2, -c2*dq2,
0.0, 0.0, 0.0;
dJ2.setZero();
break;
}
default:
{
dterr << "Undefined Euler axis order\n";
break;
}
}
mJacobianDeriv.col(0) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ0);
mJacobianDeriv.col(1) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ1);
mJacobianDeriv.col(2) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ2);
assert(!math::isNan(mJacobianDeriv));
}
