//==============================================================================
void PlanarJoint::updateRelativeJacobianTimeDeriv() const
{
Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
J.block<3, 1>(3, 0) = mAspectProperties.mTransAxis1;
J.block<3, 1>(3, 1) = mAspectProperties.mTransAxis2;
J.block<3, 1>(0, 2) = mAspectProperties.mRotAxis;
const Eigen::Matrix<double, 6, 3>& Jacobian = getRelativeJacobianStatic();
const Eigen::Vector3d& velocities = getVelocitiesStatic();
mJacobianDeriv.col(0)
= -math::ad(Jacobian.col(2) * velocities[2],
math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint
* math::expAngular(mAspectProperties.mRotAxis
* -getPositionsStatic()[2]),
J.col(0)));
mJacobianDeriv.col(1)
= -math::ad(Jacobian.col(2) * velocities[2],
math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint
* math::expAngular(mAspectProperties.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 RevoluteJoint::updateRelativeJacobian(bool _mandatory) const
{
if(_mandatory)
mJacobian = getRelativeJacobianStatic(getPositionsStatic());
}
//==============================================================================
void PlanarJoint::updateRelativeJacobian(bool) const
{
mJacobian = getRelativeJacobianStatic(getPositionsStatic());
}
//==============================================================================
Eigen::Matrix<double, 6, 3> TranslationalJoint::getRelativeJacobianStatic(
const Eigen::Vector3d& /*_positions*/) const
{
// The Jacobian is always constant w.r.t. the generalized coordinates.
return getRelativeJacobianStatic();
}
