//============================================================================== 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(); }