//==============================================================================
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 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 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 PrismaticJoint::updateLocalJacobian(bool _mandatory) const
{
if(_mandatory)
mJacobian = getLocalJacobianStatic(getPositionsStatic());
}
//==============================================================================
void PlanarJoint::updateLocalJacobian(bool) const
{
mJacobian = getLocalJacobianStatic(getPositionsStatic());
}
//==============================================================================
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 FreeJoint::setRelativeSpatialVelocity(
const Eigen::Vector6d& newSpatialVelocity)
{
setVelocitiesStatic(getLocalJacobianStatic().inverse() * newSpatialVelocity);
}
