33 #ifndef DART_DYNAMICS_DETAIL_GenericJoint_HPP_
34 #define DART_DYNAMICS_DETAIL_GenericJoint_HPP_
38 #include "dart/config.hpp"
44 #define GenericJoint_REPORT_DIM_MISMATCH(func, arg) \
46 dterr << "[GenericJoint::" #func "] Mismatch beteween size of " \
47 << #arg " [" << arg.size() << "] and the number of " \
48 << "DOFs [" << getNumDofs() << "] for Joint named [" \
49 << this->getName() << "].\n"; \
53 #define GenericJoint_REPORT_OUT_OF_RANGE(func, index) \
55 dterr << "[GenericJoint::" << #func << "] The index [" << index \
56 << "] is out of range for Joint named [" << this->getName() \
57 << "] which has " << this->getNumDofs() << " DOFs.\n"; \
61 #define GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(func) \
63 dterr << "[GenericJoint::" #func "] Unsupported actuator type (" \
64 << Joint::mAspectProperties.mActuatorType << ") for Joint [" \
65 << this->getName() << "].\n"; \
69 #define GenericJoint_SET_IF_DIFFERENT(mField, value) \
70 if (value == Base::mAspectProperties.mField) \
72 Base::mAspectProperties.mField = value; \
73 Joint::incrementVersion();
85 template <
class ConfigSpaceT>
89 template <
class ConfigSpaceT>
92 for (
auto i = 0u; i < NumDofs; ++i)
97 template <
class ConfigSpaceT>
105 template <
class ConfigSpaceT>
113 template <
class ConfigSpaceT>
116 setCommands(state.mCommands);
117 setPositionsStatic(state.mPositions);
118 setVelocitiesStatic(state.mVelocities);
119 setAccelerationsStatic(state.mAccelerations);
120 setForces(state.mForces);
124 template <
class ConfigSpaceT>
128 for (
auto i = 0u; i < NumDofs; ++i)
131 setPositionLowerLimit(i,
properties.mPositionLowerLimits[i]);
132 setPositionUpperLimit(i,
properties.mPositionUpperLimits[i]);
133 setInitialPosition(i,
properties.mInitialPositions[i]);
134 setVelocityLowerLimit(i,
properties.mVelocityLowerLimits[i]);
135 setVelocityUpperLimit(i,
properties.mVelocityUpperLimits[i]);
136 setInitialVelocity(i,
properties.mInitialVelocities[i]);
137 setAccelerationLowerLimit(i,
properties.mAccelerationLowerLimits[i]);
138 setAccelerationUpperLimit(i,
properties.mAccelerationUpperLimits[i]);
140 setForceUpperLimit(i,
properties.mForceUpperLimits[i]);
141 setSpringStiffness(i,
properties.mSpringStiffnesses[i]);
143 setDampingCoefficient(i,
properties.mDampingCoefficients[i]);
144 setCoulombFriction(i,
properties.mFrictions[i]);
149 template <
class ConfigSpaceT>
158 template <
class ConfigSpaceT>
168 template <
class ConfigSpaceT>
171 if (
nullptr == other)
178 template <
class ConfigSpaceT>
187 template <
class ConfigSpaceT>
199 template <
class ConfigSpaceT>
211 template <
class ConfigSpaceT>
218 template <
class ConfigSpaceT>
222 if (NumDofs <=
index)
224 dterr <<
"[GenericJoint::setDofName] Attempting to set the name of DOF "
225 <<
"index " <<
index <<
", which is out of bounds for the Joint ["
227 <<
"]. We will set the name of DOF index 0 instead.\n";
234 std::string& dofName = Base::mAspectProperties.mDofNames[
index];
240 = this->mChildBodyNode ? this->mChildBodyNode->getSkeleton() :
nullptr;
242 dofName = skel->mNameMgrForDofs.changeObjectName(mDofs[
index],
name);
250 template <
class ConfigSpaceT>
263 template <
class ConfigSpaceT>
266 if (NumDofs <=
index)
272 return Base::mAspectProperties.mPreserveDofNames[
index];
276 template <
class ConfigSpaceT>
279 if (NumDofs <=
index)
281 dterr <<
"[GenericJoint::getDofName] Requested name of DOF index [" <<
index
282 <<
"] in Joint [" << this->getName() <<
"], but that is "
283 <<
"out of bounds (max " << NumDofs - 1
284 <<
"). Returning name of DOF 0.\n";
286 return Base::mAspectProperties.mDofNames[0];
289 return Base::mAspectProperties.mDofNames[
index];
293 template <
class ConfigSpaceT>
296 if (
index >= getNumDofs())
302 return mDofs[
index]->mIndexInSkeleton;
306 template <
class ConfigSpaceT>
309 if (
index >= getNumDofs())
315 return mDofs[
index]->mIndexInTree;
319 template <
class ConfigSpaceT>
330 Base::mAspectProperties.mForceLowerLimits[
index],
331 Base::mAspectProperties.mForceUpperLimits[
index]);
336 dtwarn <<
"[GenericJoint::setCommand] Attempting to set a non-zero ("
337 << command <<
") command for a PASSIVE joint ["
338 << this->getName() <<
"].\n";
340 this->mAspectState.mCommands[
index] = command;
345 Base::mAspectProperties.mVelocityLowerLimits[
index],
346 Base::mAspectProperties.mVelocityUpperLimits[
index]);
351 dtwarn <<
"[GenericJoint::setCommand] Attempting to set a non-zero ("
352 << command <<
") command for a MIMIC joint [" << this->getName()
357 Base::mAspectProperties.mVelocityLowerLimits[
index],
358 Base::mAspectProperties.mVelocityUpperLimits[
index]);
363 Base::mAspectProperties.mAccelerationLowerLimits[
index],
364 Base::mAspectProperties.mAccelerationUpperLimits[
index]);
369 Base::mAspectProperties.mVelocityLowerLimits[
index],
370 Base::mAspectProperties.mVelocityUpperLimits[
index]);
376 dtwarn <<
"[GenericJoint::setCommand] Attempting to set a non-zero ("
377 << command <<
") command for a LOCKED joint [" << this->getName()
380 this->mAspectState.mCommands[
index] = command;
389 template <
class ConfigSpaceT>
398 return this->mAspectState.mCommands[
index];
402 template <
class ConfigSpaceT>
405 if (
static_cast<size_t>(commands.size()) != getNumDofs())
416 Base::mAspectProperties.mForceLowerLimits,
417 Base::mAspectProperties.mForceUpperLimits);
420 if (Vector::Zero() != commands)
422 dtwarn <<
"[GenericJoint::setCommands] Attempting to set a non-zero ("
423 << commands.transpose() <<
") command for a PASSIVE joint ["
424 << this->getName() <<
"].\n";
426 this->mAspectState.mCommands = commands;
431 Base::mAspectProperties.mVelocityLowerLimits,
432 Base::mAspectProperties.mVelocityUpperLimits);
435 if (Vector::Zero() != commands)
437 dtwarn <<
"[GenericJoint::setCommands] Attempting to set a non-zero ("
438 << commands.transpose() <<
") command for a MIMIC joint ["
439 << this->getName() <<
"].\n";
443 Base::mAspectProperties.mVelocityLowerLimits,
444 Base::mAspectProperties.mVelocityUpperLimits);
449 Base::mAspectProperties.mAccelerationLowerLimits,
450 Base::mAspectProperties.mAccelerationUpperLimits);
455 Base::mAspectProperties.mVelocityLowerLimits,
456 Base::mAspectProperties.mVelocityUpperLimits);
460 if (Vector::Zero() != commands)
462 dtwarn <<
"[GenericJoint::setCommands] Attempting to set a non-zero ("
463 << commands.transpose() <<
") command for a LOCKED joint ["
464 << this->getName() <<
"].\n";
466 this->mAspectState.mCommands = commands;
475 template <
class ConfigSpaceT>
478 return this->mAspectState.mCommands;
482 template <
class ConfigSpaceT>
485 this->mAspectState.mCommands.setZero();
489 template <
class ConfigSpaceT>
492 if (
index >= getNumDofs())
506 this->notifyPositionUpdated();
510 template <
class ConfigSpaceT>
513 if (
index >= getNumDofs())
523 template <
class ConfigSpaceT>
526 if (
static_cast<size_t>(positions.size()) != getNumDofs())
532 setPositionsStatic(positions);
536 template <
class ConfigSpaceT>
539 return getPositionsStatic();
543 template <
class ConfigSpaceT>
547 if (
index >= getNumDofs())
557 template <
class ConfigSpaceT>
560 if (
index >= getNumDofs())
566 return Base::mAspectProperties.mPositionLowerLimits[
index];
570 template <
class ConfigSpaceT>
572 const Eigen::VectorXd& lowerLimits)
574 if (
static_cast<size_t>(lowerLimits.size()) != getNumDofs())
584 template <
class ConfigSpaceT>
587 return Base::mAspectProperties.mPositionLowerLimits;
591 template <
class ConfigSpaceT>
595 if (
index >= getNumDofs())
605 template <
class ConfigSpaceT>
608 if (
index >= getNumDofs())
614 return Base::mAspectProperties.mPositionUpperLimits[
index];
618 template <
class ConfigSpaceT>
620 const Eigen::VectorXd& upperLimits)
622 if (
static_cast<size_t>(upperLimits.size()) != getNumDofs())
632 template <
class ConfigSpaceT>
635 return Base::mAspectProperties.mPositionUpperLimits;
639 template <
class ConfigSpaceT>
642 if (
index >= getNumDofs())
648 return std::isfinite(Base::mAspectProperties.mPositionUpperLimits[
index])
649 || std::isfinite(Base::mAspectProperties.mPositionLowerLimits[
index]);
653 template <
class ConfigSpaceT>
656 if (
index >= getNumDofs())
662 setPosition(
index, Base::mAspectProperties.mInitialPositions[
index]);
666 template <
class ConfigSpaceT>
669 setPositionsStatic(Base::mAspectProperties.mInitialPositions);
673 template <
class ConfigSpaceT>
675 size_t index,
double initial)
677 if (
index >= getNumDofs())
687 template <
class ConfigSpaceT>
690 if (
index >= getNumDofs())
696 return Base::mAspectProperties.mInitialPositions[
index];
700 template <
class ConfigSpaceT>
702 const Eigen::VectorXd& initial)
704 if (
static_cast<size_t>(initial.size()) != getNumDofs())
714 template <
class ConfigSpaceT>
717 return Base::mAspectProperties.mInitialPositions;
721 template <
class ConfigSpaceT>
724 if (this->mAspectState.mPositions == positions)
727 this->mAspectState.mPositions = positions;
728 this->notifyPositionUpdated();
732 template <
class ConfigSpaceT>
736 return this->mAspectState.mPositions;
740 template <
class ConfigSpaceT>
743 if (this->mAspectState.mVelocities == velocities)
746 this->mAspectState.mVelocities = velocities;
747 this->notifyVelocityUpdated();
751 template <
class ConfigSpaceT>
755 return this->mAspectState.mVelocities;
759 template <
class ConfigSpaceT>
762 if (this->mAspectState.mAccelerations == accels)
765 this->mAspectState.mAccelerations = accels;
766 this->notifyAccelerationUpdated();
770 template <
class ConfigSpaceT>
774 return this->mAspectState.mAccelerations;
778 template <
class ConfigSpaceT>
781 if (
index >= getNumDofs())
792 this->notifyVelocityUpdated();
795 this->mAspectState.mCommands[
index] = this->getVelocitiesStatic()[
index];
799 template <
class ConfigSpaceT>
802 if (
index >= getNumDofs())
808 return getVelocitiesStatic()[
index];
812 template <
class ConfigSpaceT>
814 const Eigen::VectorXd& velocities)
816 if (
static_cast<size_t>(velocities.size()) != getNumDofs())
822 setVelocitiesStatic(velocities);
825 this->mAspectState.mCommands = this->getVelocitiesStatic();
829 template <
class ConfigSpaceT>
832 return getVelocitiesStatic();
836 template <
class ConfigSpaceT>
840 if (
index >= getNumDofs())
850 template <
class ConfigSpaceT>
853 if (
index >= getNumDofs())
859 return Base::mAspectProperties.mVelocityLowerLimits[
index];
863 template <
class ConfigSpaceT>
865 const Eigen::VectorXd& lowerLimits)
867 if (
static_cast<size_t>(lowerLimits.size()) != getNumDofs())
877 template <
class ConfigSpaceT>
880 return Base::mAspectProperties.mVelocityLowerLimits;
884 template <
class ConfigSpaceT>
888 if (
index >= getNumDofs())
898 template <
class ConfigSpaceT>
901 if (
index >= getNumDofs())
907 return Base::mAspectProperties.mVelocityUpperLimits[
index];
911 template <
class ConfigSpaceT>
913 const Eigen::VectorXd& upperLimits)
915 if (
static_cast<size_t>(upperLimits.size()) != getNumDofs())
925 template <
class ConfigSpaceT>
928 return Base::mAspectProperties.mVelocityUpperLimits;
932 template <
class ConfigSpaceT>
935 if (
index >= getNumDofs())
941 setVelocity(
index, Base::mAspectProperties.mInitialVelocities[
index]);
945 template <
class ConfigSpaceT>
948 setVelocitiesStatic(Base::mAspectProperties.mInitialVelocities);
952 template <
class ConfigSpaceT>
954 size_t index,
double initial)
956 if (
index >= getNumDofs())
966 template <
class ConfigSpaceT>
969 if (
index >= getNumDofs())
975 return Base::mAspectProperties.mInitialVelocities[
index];
979 template <
class ConfigSpaceT>
981 const Eigen::VectorXd& initial)
983 if (
static_cast<size_t>(initial.size()) != getNumDofs())
993 template <
class ConfigSpaceT>
996 return Base::mAspectProperties.mInitialVelocities;
1000 template <
class ConfigSpaceT>
1004 if (
index >= getNumDofs())
1015 this->notifyAccelerationUpdated();
1018 this->mAspectState.mCommands[
index] = this->getAccelerationsStatic()[
index];
1022 template <
class ConfigSpaceT>
1025 if (
index >= getNumDofs())
1031 return getAccelerationsStatic()[
index];
1035 template <
class ConfigSpaceT>
1037 const Eigen::VectorXd& accelerations)
1039 if (
static_cast<size_t>(accelerations.size()) != getNumDofs())
1045 setAccelerationsStatic(accelerations);
1048 this->mAspectState.mCommands = this->getAccelerationsStatic();
1052 template <
class ConfigSpaceT>
1055 return getAccelerationsStatic();
1059 template <
class ConfigSpaceT>
1063 if (
index >= getNumDofs())
1073 template <
class ConfigSpaceT>
1076 if (
index >= getNumDofs())
1082 return Base::mAspectProperties.mAccelerationLowerLimits[
index];
1086 template <
class ConfigSpaceT>
1088 const Eigen::VectorXd& lowerLimits)
1090 if (
static_cast<size_t>(lowerLimits.size()) != getNumDofs())
1100 template <
class ConfigSpaceT>
1103 return Base::mAspectProperties.mAccelerationLowerLimits;
1107 template <
class ConfigSpaceT>
1111 if (
index >= getNumDofs())
1120 template <
class ConfigSpaceT>
1123 if (
index >= getNumDofs())
1129 return Base::mAspectProperties.mAccelerationUpperLimits[
index];
1133 template <
class ConfigSpaceT>
1135 const Eigen::VectorXd& upperLimits)
1137 if (
static_cast<size_t>(upperLimits.size()) != getNumDofs())
1147 template <
class ConfigSpaceT>
1150 return Base::mAspectProperties.mAccelerationUpperLimits;
1154 template <
class ConfigSpaceT>
1157 setAccelerationsStatic(Vector::Zero());
1161 template <
class ConfigSpaceT>
1164 if (
index >= getNumDofs())
1173 this->mAspectState.mCommands[
index] = this->mAspectState.mForces[
index];
1177 template <
class ConfigSpaceT>
1180 if (
index >= getNumDofs())
1186 return this->mAspectState.mForces[
index];
1190 template <
class ConfigSpaceT>
1193 if (
static_cast<size_t>(forces.size()) != getNumDofs())
1199 this->mAspectState.mForces = forces;
1202 this->mAspectState.mCommands = this->mAspectState.mForces;
1206 template <
class ConfigSpaceT>
1209 return this->mAspectState.mForces;
1213 template <
class ConfigSpaceT>
1216 if (
index >= getNumDofs())
1226 template <
class ConfigSpaceT>
1229 if (
index >= getNumDofs())
1235 return Base::mAspectProperties.mForceLowerLimits[
index];
1239 template <
class ConfigSpaceT>
1241 const Eigen::VectorXd& lowerLimits)
1243 if (
static_cast<size_t>(lowerLimits.size()) != getNumDofs())
1253 template <
class ConfigSpaceT>
1256 return Base::mAspectProperties.mForceLowerLimits;
1260 template <
class ConfigSpaceT>
1263 if (
index >= getNumDofs())
1273 template <
class ConfigSpaceT>
1276 if (
index >= getNumDofs())
1282 return Base::mAspectProperties.mForceUpperLimits[
index];
1286 template <
class ConfigSpaceT>
1288 const Eigen::VectorXd& upperLimits)
1290 if (
static_cast<size_t>(upperLimits.size()) != getNumDofs())
1300 template <
class ConfigSpaceT>
1303 return Base::mAspectProperties.mForceUpperLimits;
1307 template <
class ConfigSpaceT>
1310 this->mAspectState.mForces.setZero();
1313 this->mAspectState.mCommands = this->mAspectState.mForces;
1317 template <
class ConfigSpaceT>
1319 size_t index,
double velocityChange)
1321 if (
index >= getNumDofs())
1327 mVelocityChanges[
index] = velocityChange;
1331 template <
class ConfigSpaceT>
1334 if (
index >= getNumDofs())
1340 return mVelocityChanges[
index];
1344 template <
class ConfigSpaceT>
1347 mVelocityChanges.setZero();
1351 template <
class ConfigSpaceT>
1353 size_t index,
double impulse)
1355 if (
index >= getNumDofs())
1361 mConstraintImpulses[
index] = impulse;
1365 template <
class ConfigSpaceT>
1368 if (
index >= getNumDofs())
1374 return mConstraintImpulses[
index];
1378 template <
class ConfigSpaceT>
1381 mConstraintImpulses.setZero();
1385 template <
class ConfigSpaceT>
1388 const Point& point = math::integratePosition<ConfigSpaceT>(
1389 math::toManifoldPoint<ConfigSpaceT>(getPositionsStatic()),
1390 getVelocitiesStatic(),
1393 setPositionsStatic(math::toEuclideanPoint<ConfigSpaceT>(point));
1397 template <
class ConfigSpaceT>
1400 setVelocitiesStatic(math::integrateVelocity<ConfigSpaceT>(
1401 getVelocitiesStatic(), getAccelerationsStatic(), dt));
1405 template <
class ConfigSpaceT>
1407 const Eigen::VectorXd& q2,
const Eigen::VectorXd& q1)
const
1409 if (
static_cast<size_t>(q1.size()) != getNumDofs()
1410 ||
static_cast<size_t>(q2.size()) != getNumDofs())
1412 dterr <<
"[GenericJoint::getPositionsDifference] q1's size [" << q1.size()
1413 <<
"] or q2's size [" << q2.size() <<
"] must both equal the dof ["
1414 << this->getNumDofs() <<
"] for Joint [" << this->getName() <<
"].\n";
1416 return Eigen::VectorXd::Zero(getNumDofs());
1419 return getPositionDifferencesStatic(q2, q1);
1423 template <
class ConfigSpaceT>
1424 typename ConfigSpaceT::Vector
1433 template <
class ConfigSpaceT>
1436 if (
index >= getNumDofs())
1448 template <
class ConfigSpaceT>
1451 if (
index >= getNumDofs())
1457 return Base::mAspectProperties.mSpringStiffnesses[
index];
1461 template <
class ConfigSpaceT>
1464 if (
index >= getNumDofs())
1474 template <
class ConfigSpaceT>
1477 if (
index >= getNumDofs())
1483 return Base::mAspectProperties.mRestPositions[
index];
1487 template <
class ConfigSpaceT>
1490 if (
index >= getNumDofs())
1502 template <
class ConfigSpaceT>
1505 if (
index >= getNumDofs())
1511 return Base::mAspectProperties.mDampingCoefficients[
index];
1515 template <
class ConfigSpaceT>
1519 if (
index >= getNumDofs())
1531 template <
class ConfigSpaceT>
1534 if (
index >= getNumDofs())
1540 return Base::mAspectProperties.mFrictions[
index];
1544 template <
class ConfigSpaceT>
1549 = getPositionsStatic() - Base::mAspectProperties.mRestPositions;
1554 Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
1561 template <
class ConfigSpaceT>
1564 return getRelativeJacobianStatic();
1568 template <
class ConfigSpaceT>
1572 if (this->mIsRelativeJacobianDirty)
1574 this->updateRelativeJacobian(
false);
1575 this->mIsRelativeJacobianDirty =
false;
1582 template <
class ConfigSpaceT>
1584 const Eigen::VectorXd& positions)
const
1586 return getRelativeJacobianStatic(positions);
1590 template <
class ConfigSpaceT>
1594 return getRelativeJacobianTimeDerivStatic();
1598 template <
class ConfigSpaceT>
1602 if (this->mIsRelativeJacobianTimeDerivDirty)
1604 this->updateRelativeJacobianTimeDeriv();
1605 this->mIsRelativeJacobianTimeDerivDirty =
false;
1608 return mJacobianDeriv;
1612 template <
class ConfigSpaceT>
1614 : mVelocityChanges(
Vector::Zero()),
1615 mImpulses(
Vector::Zero()),
1616 mConstraintImpulses(
Vector::Zero()),
1619 mInvProjArtInertia(
Matrix::Zero()),
1620 mInvProjArtInertiaImplicit(
Matrix::Zero()),
1621 mTotalForce(
Vector::Zero()),
1622 mTotalImpulse(
Vector::Zero())
1624 for (
auto i = 0u; i <
NumDofs; ++i)
1625 mDofs[i] = this->createDofPointer(i);
1633 template <
class ConfigSpaceT>
1636 const SkeletonPtr& skel = this->mChildBodyNode->getSkeleton();
1637 for (
auto i = 0u; i < NumDofs; ++i)
1639 Base::mAspectProperties.mDofNames[i]
1640 = skel->mNameMgrForDofs.issueNewNameAndAdd(
1641 mDofs[i]->getName(), mDofs[i]);
1646 template <
class ConfigSpaceT>
1649 assert(this->mChildBodyNode);
1650 return this->mChildBodyNode->getBodyForce()
1651 - this->getRelativeJacobianStatic() * this->mAspectState.mForces;
1655 template <
class ConfigSpaceT>
1658 this->mSpatialVelocity
1659 = this->getRelativeJacobianStatic() * this->getVelocitiesStatic();
1663 template <
class ConfigSpaceT>
1666 this->mSpatialAcceleration = this->getRelativePrimaryAcceleration()
1667 + this->getRelativeJacobianTimeDerivStatic()
1668 * this->getVelocitiesStatic();
1672 template <
class ConfigSpaceT>
1675 this->mPrimaryAcceleration
1676 = this->getRelativeJacobianStatic() * this->getAccelerationsStatic();
1680 template <
class ConfigSpaceT>
1684 vel.noalias() += getRelativeJacobianStatic() * getVelocitiesStatic();
1691 template <
class ConfigSpaceT>
1698 childVelocity, getRelativeJacobianStatic() * getVelocitiesStatic())
1699 + getRelativeJacobianTimeDerivStatic() * getVelocitiesStatic();
1705 template <
class ConfigSpaceT>
1709 acc.noalias() += getRelativeJacobianStatic() * getAccelerationsStatic();
1716 template <
class ConfigSpaceT>
1721 velocityChange.noalias() += getRelativeJacobianStatic() * mVelocityChanges;
1728 template <
class ConfigSpaceT>
1734 return mInvProjArtInertia;
1738 template <
class ConfigSpaceT>
1744 return mInvProjArtInertiaImplicit;
1748 template <
class ConfigSpaceT>
1758 addChildArtInertiaToDynamic(parentArtInertia, childArtInertia);
1763 addChildArtInertiaToKinematic(parentArtInertia, childArtInertia);
1772 template <
class ConfigSpaceT>
1777 JacobianMatrix AIS = childArtInertia * getRelativeJacobianStatic();
1779 PI.noalias() -= AIS * mInvProjArtInertia * AIS.transpose();
1789 template <
class ConfigSpaceT>
1796 this->getRelativeTransform().
inverse(), childArtInertia);
1800 template <
class ConfigSpaceT>
1810 addChildArtInertiaImplicitToDynamic(parentArtInertia, childArtInertia);
1815 addChildArtInertiaImplicitToKinematic(parentArtInertia, childArtInertia);
1824 template <
class ConfigSpaceT>
1829 JacobianMatrix AIS = childArtInertia * getRelativeJacobianStatic();
1831 PI.noalias() -= AIS * mInvProjArtInertiaImplicit * AIS.transpose();
1841 template <
class ConfigSpaceT>
1848 this->getRelativeTransform().
inverse(), childArtInertia);
1852 template <
class ConfigSpaceT>
1862 updateInvProjArtInertiaDynamic(artInertia);
1867 updateInvProjArtInertiaKinematic(artInertia);
1876 template <
class ConfigSpaceT>
1885 mInvProjArtInertia = math::inverse<ConfigSpaceT>(projAI);
1892 template <
class ConfigSpaceT>
1900 template <
class ConfigSpaceT>
1910 updateInvProjArtInertiaImplicitDynamic(artInertia, timeStep);
1915 updateInvProjArtInertiaImplicitKinematic(artInertia, timeStep);
1924 template <
class ConfigSpaceT>
1933 projAI += (timeStep * Base::mAspectProperties.mDampingCoefficients
1934 + timeStep * timeStep * Base::mAspectProperties.mSpringStiffnesses)
1938 mInvProjArtInertiaImplicit = math::inverse<ConfigSpaceT>(projAI);
1945 template <
class ConfigSpaceT>
1953 template <
class ConfigSpaceT>
1966 addChildBiasForceToDynamic(
1967 parentBiasForce, childArtInertia, childBiasForce, childPartialAcc);
1972 addChildBiasForceToKinematic(
1973 parentBiasForce, childArtInertia, childBiasForce, childPartialAcc);
1982 template <
class ConfigSpaceT>
1994 + getRelativeJacobianStatic() * getInvProjArtInertiaImplicit()
2008 parentBiasForce +=
math::dAdInvT(this->getRelativeTransform(), beta);
2012 template <
class ConfigSpaceT>
2024 + getRelativeJacobianStatic() * getAccelerationsStatic());
2037 _parentBiasForce +=
math::dAdInvT(this->getRelativeTransform(), beta);
2041 template <
class ConfigSpaceT>
2053 addChildBiasImpulseToDynamic(
2054 parentBiasImpulse, childArtInertia, childBiasImpulse);
2059 addChildBiasImpulseToKinematic(
2060 parentBiasImpulse, childArtInertia, childBiasImpulse);
2069 template <
class ConfigSpaceT>
2077 + childArtInertia * getRelativeJacobianStatic()
2078 * getInvProjArtInertia() * mTotalImpulse;
2085 _parentBiasImpulse +=
math::dAdInvT(this->getRelativeTransform(), beta);
2089 template <
class ConfigSpaceT>
2098 +=
math::dAdInvT(this->getRelativeTransform(), childBiasImpulse);
2102 template <
class ConfigSpaceT>
2106 assert(timeStep > 0.0);
2111 this->mAspectState.mForces = this->mAspectState.mCommands;
2112 updateTotalForceDynamic(bodyForce, timeStep);
2117 this->mAspectState.mForces.setZero();
2118 updateTotalForceDynamic(bodyForce, timeStep);
2121 setAccelerationsStatic(this->mAspectState.mCommands);
2122 updateTotalForceKinematic(bodyForce, timeStep);
2125 setAccelerationsStatic(
2126 (this->mAspectState.mCommands - getVelocitiesStatic()) / timeStep);
2127 updateTotalForceKinematic(bodyForce, timeStep);
2130 setVelocitiesStatic(Vector::Zero());
2131 setAccelerationsStatic(Vector::Zero());
2132 updateTotalForceKinematic(bodyForce, timeStep);
2141 template <
class ConfigSpaceT>
2147 = -Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
2148 getPositionsStatic() - Base::mAspectProperties.mRestPositions
2149 + getVelocitiesStatic() * timeStep);
2152 const Vector dampingForce
2153 = -Base::mAspectProperties.mDampingCoefficients.cwiseProduct(
2154 getVelocitiesStatic());
2157 mTotalForce = this->mAspectState.mForces + springForce + dampingForce
2158 - getRelativeJacobianStatic().transpose() * bodyForce;
2162 template <
class ConfigSpaceT>
2170 template <
class ConfigSpaceT>
2180 updateTotalImpulseDynamic(bodyImpulse);
2185 updateTotalImpulseKinematic(bodyImpulse);
2194 template <
class ConfigSpaceT>
2199 mTotalImpulse = mConstraintImpulses
2200 - getRelativeJacobianStatic().transpose() * bodyImpulse;
2204 template <
class ConfigSpaceT>
2212 template <
class ConfigSpaceT>
2215 mTotalImpulse.setZero();
2219 template <
class ConfigSpaceT>
2229 updateAccelerationDynamic(artInertia, spatialAcc);
2234 updateAccelerationKinematic(artInertia, spatialAcc);
2243 template <
class ConfigSpaceT>
2248 setAccelerationsStatic(
2249 getInvProjArtInertiaImplicit()
2251 - getRelativeJacobianStatic().transpose() * artInertia
2252 *
math::AdInvT(this->getRelativeTransform(), spatialAcc)));
2259 template <
class ConfigSpaceT>
2268 template <
class ConfigSpaceT>
2278 updateVelocityChangeDynamic(artInertia, velocityChange);
2283 updateVelocityChangeKinematic(artInertia, velocityChange);
2292 template <
class ConfigSpaceT>
2298 = getInvProjArtInertia()
2300 - getRelativeJacobianStatic().transpose() * artInertia
2301 *
math::AdInvT(this->getRelativeTransform(), velocityChange));
2308 template <
class ConfigSpaceT>
2317 template <
class ConfigSpaceT>
2321 bool withDampingForces,
2322 bool withSpringForces)
2324 this->mAspectState.mForces
2325 = getRelativeJacobianStatic().transpose() * bodyForce;
2329 if (withDampingForces)
2331 const typename ConfigSpaceT::Vector dampingForces
2332 = -Base::mAspectProperties.mDampingCoefficients.cwiseProduct(
2333 getVelocitiesStatic() + getAccelerationsStatic() * timeStep);
2334 this->mAspectState.mForces -= dampingForces;
2339 if (withSpringForces)
2341 const typename ConfigSpaceT::Vector springForces
2342 = -Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
2343 getPositionsStatic() - Base::mAspectProperties.mRestPositions
2344 + getVelocitiesStatic() * timeStep
2345 + getAccelerationsStatic() * timeStep * timeStep);
2346 this->mAspectState.mForces -= springForces;
2351 template <
class ConfigSpaceT>
2355 bool withDampingForces,
2356 bool withSpringForces)
2368 updateForceID(bodyForce, timeStep, withDampingForces, withSpringForces);
2377 template <
class ConfigSpaceT>
2381 mImpulses = getRelativeJacobianStatic().transpose() * bodyImpulse;
2385 template <
class ConfigSpaceT>
2399 updateImpulseID(bodyImpulse);
2408 template <
class ConfigSpaceT>
2417 updateConstrainedTermsDynamic(timeStep);
2422 updateConstrainedTermsKinematic(timeStep);
2431 template <
class ConfigSpaceT>
2434 const double invTimeStep = 1.0 / timeStep;
2436 setVelocitiesStatic(getVelocitiesStatic() + mVelocityChanges);
2437 setAccelerationsStatic(
2438 getAccelerationsStatic() + mVelocityChanges * invTimeStep);
2439 this->mAspectState.mForces.noalias() += mImpulses * invTimeStep;
2444 template <
class ConfigSpaceT>
2448 this->mAspectState.mForces.noalias() += mImpulses / timeStep;
2452 template <
class ConfigSpaceT>
2460 beta.noalias() += childArtInertia * getRelativeJacobianStatic()
2461 * getInvProjArtInertia() * mInvM_a;
2468 parentBiasForce +=
math::dAdInvT(this->getRelativeTransform(), beta);
2472 template <
class ConfigSpaceT>
2480 beta.noalias() += childArtInertia * getRelativeJacobianStatic()
2481 * getInvProjArtInertiaImplicit() * mInvM_a;
2488 parentBiasForce +=
math::dAdInvT(this->getRelativeTransform(), beta);
2492 template <
class ConfigSpaceT>
2497 mInvM_a = this->mAspectState.mForces
2498 - getRelativeJacobianStatic().transpose() * bodyForce;
2502 template <
class ConfigSpaceT>
2504 Eigen::MatrixXd& _invMassMat,
2510 mInvMassMatrixSegment
2511 = getInvProjArtInertia()
2513 - getRelativeJacobianStatic().transpose() * artInertia
2514 *
math::AdInvT(this->getRelativeTransform(), spatialAcc));
2520 size_t iStart = mDofs[0]->mIndexInTree;
2523 _invMassMat.block<NumDofs, 1>(iStart, _col) = mInvMassMatrixSegment;
2527 template <
class ConfigSpaceT>
2529 Eigen::MatrixXd& invMassMat,
2535 mInvMassMatrixSegment
2536 = getInvProjArtInertiaImplicit()
2538 - getRelativeJacobianStatic().transpose() * artInertia
2539 *
math::AdInvT(this->getRelativeTransform(), spatialAcc));
2545 size_t iStart = mDofs[0]->mIndexInTree;
2548 invMassMat.block<NumDofs, 1>(iStart, col) = mInvMassMatrixSegment;
2552 template <
class ConfigSpaceT>
2556 acc += getRelativeJacobianStatic() * mInvMassMatrixSegment;
2560 template <
class ConfigSpaceT>
2564 return getRelativeJacobianStatic().transpose() * spatial;
#define dterr
Output an error message.
Definition: Console.hpp:49
#define dtwarn
Output a warning message.
Definition: Console.hpp:46
BodyPropPtr properties
Definition: SdfParser.cpp:80
std::string * name
Definition: SkelParser.cpp:1697
bool * preserveName
Definition: SkelParser.cpp:1696
Eigen::VectorXd velocity
Definition: SkelParser.cpp:109
Eigen::VectorXd force
Definition: SkelParser.cpp:111
std::size_t index
Definition: SkelParser.cpp:1672
Eigen::VectorXd acceleration
Definition: SkelParser.cpp:110
double * friction
Definition: SkelParser.cpp:1694
Eigen::VectorXd position
Definition: SkelParser.cpp:108
AspectProperties mAspectProperties
Aspect::Properties data, directly accessible to your derived class.
Definition: EmbeddedAspect.hpp:228
typename Impl::AspectState AspectState
Definition: EmbeddedAspect.hpp:440
AspectState mAspectState
Aspect::State data, directly accessible to your derived class.
Definition: EmbeddedAspect.hpp:415
typename Impl::AspectProperties AspectProperties
Definition: EmbeddedAspect.hpp:442
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:55
Definition: GenericJoint.hpp:48
void setAspectState(const AspectState &state)
Set the AspectState of this GenericJoint.
Definition: GenericJoint.hpp:114
void setForceLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:1240
double getCommand(std::size_t index) const override
Definition: GenericJoint.hpp:390
typename ConfigSpaceT::Matrix Matrix
Definition: GenericJoint.hpp:59
void addChildArtInertiaImplicitToKinematic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1842
Eigen::VectorXd getCommands() const override
Definition: GenericJoint.hpp:476
void resetVelocities() override
Definition: GenericJoint.hpp:946
const Vector & getPositionsStatic() const
Fixed-size version of getPositions()
Definition: GenericJoint.hpp:734
void setAccelerationsStatic(const Vector &accels)
Fixed-size version of setAccelerations()
Definition: GenericJoint.hpp:760
void copy(const ThisClass &otherJoint)
Copy the Properties of another GenericJoint.
Definition: GenericJoint.hpp:159
void setForceUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:1287
void updateTotalImpulseKinematic(const Eigen::Vector6d &bodyImpulse)
Definition: GenericJoint.hpp:2205
void setCoulombFriction(std::size_t index, double friction) override
Definition: GenericJoint.hpp:1516
const JacobianMatrix & getRelativeJacobianTimeDerivStatic() const
Fixed-size version of getRelativeJacobianTimeDeriv()
Definition: GenericJoint.hpp:1600
void setRestPosition(std::size_t index, double q0) override
Definition: GenericJoint.hpp:1462
void setAccelerations(const Eigen::VectorXd &accelerations) override
Definition: GenericJoint.hpp:1036
void updateTotalImpulse(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2171
void updateForceFD(const Eigen::Vector6d &bodyForce, double timeStep, bool withDampingForcese, bool withSpringForces) override
Definition: GenericJoint.hpp:2352
const Matrix & getInvProjArtInertiaImplicit() const
Get the inverse of projected articulated inertia for implicit joint damping and spring forces.
Definition: GenericJoint.hpp:1740
Eigen::VectorXd getPositionUpperLimits() const override
Definition: GenericJoint.hpp:633
void setVelocityUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:912
void updateForceID(const Eigen::Vector6d &bodyForce, double timeStep, bool withDampingForces, bool withSpringForces) override
Definition: GenericJoint.hpp:2318
Eigen::VectorXd getAccelerationUpperLimits() const override
Definition: GenericJoint.hpp:1148
void updateAccelerationKinematic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc)
Definition: GenericJoint.hpp:2260
void updateVelocityChangeDynamic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange)
Definition: GenericJoint.hpp:2293
Eigen::VectorXd getVelocityLowerLimits() const override
Definition: GenericJoint.hpp:878
void updateInvProjArtInertiaImplicit(const Eigen::Matrix6d &artInertia, double timeStep) override
Definition: GenericJoint.hpp:1901
void setVelocityLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:864
void updateConstrainedTermsDynamic(double timeStep)
Definition: GenericJoint.hpp:2432
void setPositionLowerLimit(std::size_t index, double position) override
Definition: GenericJoint.hpp:544
const GenericJoint< ConfigSpaceT >::JacobianMatrix & getRelativeJacobianStatic() const
Fixed-size version of getRelativeJacobian()
Definition: GenericJoint.hpp:1570
void addChildArtInertiaToDynamic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1773
const math::Jacobian getRelativeJacobianTimeDeriv() const override
Definition: GenericJoint.hpp:1591
void setVelocities(const Eigen::VectorXd &velocities) override
Definition: GenericJoint.hpp:813
void addChildBiasImpulseToKinematic(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse)
Definition: GenericJoint.hpp:2090
void addChildArtInertiaImplicitTo(Eigen::Matrix6d &parentArtInertiaImplicit, const Eigen::Matrix6d &childArtInertiaImplicit) override
Definition: GenericJoint.hpp:1801
void setPosition(std::size_t index, double position) override
Definition: GenericJoint.hpp:490
void updateVelocityChange(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange) override
Definition: GenericJoint.hpp:2269
void setCommand(std::size_t index, double command) override
Definition: GenericJoint.hpp:320
std::array< DegreeOfFreedom *, NumDofs > mDofs
Array of DegreeOfFreedom objects.
Definition: GenericJoint.hpp:678
void setInitialPosition(std::size_t index, double initial) override
Definition: GenericJoint.hpp:674
Eigen::VectorXd getForces() const override
Definition: GenericJoint.hpp:1207
void resetCommands() override
Definition: GenericJoint.hpp:483
void setVelocityChange(std::size_t index, double velocityChange) override
Definition: GenericJoint.hpp:1318
void setAccelerationUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:1134
void setPartialAccelerationTo(Eigen::Vector6d &partialAcceleration, const Eigen::Vector6d &childVelocity) override
Definition: GenericJoint.hpp:1692
double getVelocity(std::size_t index) const override
Definition: GenericJoint.hpp:800
Eigen::Vector6d getBodyConstraintWrench() const override
Definition: GenericJoint.hpp:1647
void addAccelerationTo(Eigen::Vector6d &acc) override
Definition: GenericJoint.hpp:1706
void updateTotalImpulseDynamic(const Eigen::Vector6d &bodyImpulse)
Definition: GenericJoint.hpp:2195
void setForce(std::size_t index, double force) override
Definition: GenericJoint.hpp:1162
void setInitialVelocity(std::size_t index, double initial) override
Definition: GenericJoint.hpp:953
double getForce(std::size_t index) const override
Definition: GenericJoint.hpp:1178
void setInitialVelocities(const Eigen::VectorXd &initial) override
Definition: GenericJoint.hpp:980
void updateInvProjArtInertiaDynamic(const Eigen::Matrix6d &artInertia)
Definition: GenericJoint.hpp:1877
DegreeOfFreedom * getDof(std::size_t index) override
void setPositionUpperLimit(std::size_t index, double position) override
Definition: GenericJoint.hpp:592
void updateInvProjArtInertiaImplicitKinematic(const Eigen::Matrix6d &artInertia, double timeStep)
Definition: GenericJoint.hpp:1946
const math::Jacobian getRelativeJacobian() const override
Definition: GenericJoint.hpp:1562
std::size_t getNumDofs() const override
Definition: GenericJoint.hpp:212
void resetForces() override
Definition: GenericJoint.hpp:1308
void addChildArtInertiaTo(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia) override
Definition: GenericJoint.hpp:1749
Eigen::VectorXd getPositionLowerLimits() const override
Definition: GenericJoint.hpp:585
bool isDofNamePreserved(size_t index) const override
Definition: GenericJoint.hpp:264
void addVelocityTo(Eigen::Vector6d &vel) override
Definition: GenericJoint.hpp:1681
void preserveDofName(size_t index, bool preserve) override
Definition: GenericJoint.hpp:251
double getVelocityUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:899
double getVelocityChange(std::size_t index) const override
Definition: GenericJoint.hpp:1332
void updateTotalForce(const Eigen::Vector6d &bodyForce, double timeStep) override
Definition: GenericJoint.hpp:2103
void resetConstraintImpulses() override
Definition: GenericJoint.hpp:1379
virtual Vector getPositionDifferencesStatic(const Vector &q2, const Vector &q1) const
Fixed-size version of getPositionDifferences()
Definition: GenericJoint.hpp:1425
void setVelocity(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:779
double getRestPosition(std::size_t index) const override
Definition: GenericJoint.hpp:1475
void updateConstrainedTerms(double timeStep) override
Definition: GenericJoint.hpp:2409
void integrateVelocities(double dt) override
Definition: GenericJoint.hpp:1398
Eigen::VectorXd getPositions() const override
Definition: GenericJoint.hpp:537
void addChildBiasImpulseToDynamic(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse)
Definition: GenericJoint.hpp:2070
double getAcceleration(std::size_t index) const override
Definition: GenericJoint.hpp:1023
Eigen::VectorXd getVelocities() const override
Definition: GenericJoint.hpp:830
Eigen::VectorXd getInitialPositions() const override
Definition: GenericJoint.hpp:715
Eigen::VectorXd getPositionDifferences(const Eigen::VectorXd &q2, const Eigen::VectorXd &q1) const override
Definition: GenericJoint.hpp:1406
void setPositionLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:571
double getAccelerationUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1121
void setForceLowerLimit(size_t index, double force) override
Definition: GenericJoint.hpp:1214
void setSpringStiffness(std::size_t index, double k) override
Definition: GenericJoint.hpp:1434
void updateRelativePrimaryAcceleration() const override
Definition: GenericJoint.hpp:1673
void setProperties(const Properties &properties)
Set the Properties of this GenericJoint.
Definition: GenericJoint.hpp:98
double getForceLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1227
static constexpr std::size_t NumDofs
Definition: GenericJoint.hpp:50
const Vector & getVelocitiesStatic() const
Fixed-size version of getVelocities()
Definition: GenericJoint.hpp:753
double getPositionUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:606
const std::string & getDofName(size_t index) const override
Definition: GenericJoint.hpp:277
ThisClass & operator=(const ThisClass &other)
Same as copy(const GenericJoint&)
Definition: GenericJoint.hpp:179
double computePotentialEnergy() const override
Definition: GenericJoint.hpp:1545
double getSpringStiffness(std::size_t index) const override
Definition: GenericJoint.hpp:1449
void setDampingCoefficient(std::size_t index, double coeff) override
Definition: GenericJoint.hpp:1488
void updateInvProjArtInertia(const Eigen::Matrix6d &artInertia) override
Definition: GenericJoint.hpp:1853
void resetVelocity(std::size_t index) override
Definition: GenericJoint.hpp:933
void setPositionsStatic(const Vector &positions)
Fixed-size version of setPositions()
Definition: GenericJoint.hpp:722
void addChildBiasForceForInvAugMassMatrix(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce) override
Definition: GenericJoint.hpp:2473
void updateConstrainedTermsKinematic(double timeStep)
Definition: GenericJoint.hpp:2445
void setAccelerationLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:1087
void setAspectProperties(const AspectProperties &properties)
Set the AspectProperties of this GenericJoint.
Definition: GenericJoint.hpp:125
Eigen::VectorXd getForceUpperLimits() const override
Definition: GenericJoint.hpp:1301
detail::GenericJointProperties< ConfigSpaceT > Properties
Definition: GenericJoint.hpp:62
void updateImpulseFD(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2386
GenericJoint(const ThisClass &)=delete
void updateInvProjArtInertiaImplicitDynamic(const Eigen::Matrix6d &artInertia, double timeStep)
Definition: GenericJoint.hpp:1925
void updateTotalForceKinematic(const Eigen::Vector6d &bodyForce, double timeStep)
Definition: GenericJoint.hpp:2163
void resetTotalImpulses() override
Definition: GenericJoint.hpp:2213
void integratePositions(double dt) override
Definition: GenericJoint.hpp:1386
void setPositions(const Eigen::VectorXd &positions) override
Definition: GenericJoint.hpp:524
void addChildBiasForceToDynamic(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc)
Definition: GenericJoint.hpp:1983
Eigen::VectorXd getForceLowerLimits() const override
Definition: GenericJoint.hpp:1254
double getInitialVelocity(std::size_t index) const override
Definition: GenericJoint.hpp:967
Eigen::VectorXd getAccelerations() const override
Definition: GenericJoint.hpp:1053
typename ConfigSpaceT::Vector Vector
Definition: GenericJoint.hpp:57
void updateVelocityChangeKinematic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange)
Definition: GenericJoint.hpp:2309
const Vector & getAccelerationsStatic() const
Fixed-size version of getAccelerations()
Definition: GenericJoint.hpp:772
Eigen::VectorXd getInitialVelocities() const override
Definition: GenericJoint.hpp:994
void setCommands(const Eigen::VectorXd &commands) override
Definition: GenericJoint.hpp:403
Eigen::VectorXd getSpatialToGeneralized(const Eigen::Vector6d &spatial) override
Definition: GenericJoint.hpp:2561
typename ConfigSpaceT::JacobianMatrix JacobianMatrix
Definition: GenericJoint.hpp:58
void getInvAugMassMatrixSegment(Eigen::MatrixXd &invMassMat, const size_t col, const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2528
double getVelocityLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:851
void updateImpulseID(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2378
void setVelocityLowerLimit(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:837
double getInitialPosition(std::size_t index) const override
Definition: GenericJoint.hpp:688
void updateAcceleration(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2220
void addChildBiasForceToKinematic(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc)
Definition: GenericJoint.hpp:2013
void setVelocityUpperLimit(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:885
typename ConfigSpaceT::Point Point
Definition: GenericJoint.hpp:55
void updateInvProjArtInertiaKinematic(const Eigen::Matrix6d &artInertia)
Definition: GenericJoint.hpp:1893
bool hasPositionLimit(std::size_t index) const override
Definition: GenericJoint.hpp:640
void setInitialPositions(const Eigen::VectorXd &initial) override
Definition: GenericJoint.hpp:701
virtual ~GenericJoint()
Destructor.
Definition: GenericJoint.hpp:90
void addChildBiasForceTo(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc) override
Definition: GenericJoint.hpp:1954
double getCoulombFriction(std::size_t index) const override
Definition: GenericJoint.hpp:1532
void addInvMassMatrixSegmentTo(Eigen::Vector6d &acc) override
Definition: GenericJoint.hpp:2553
void setConstraintImpulse(std::size_t index, double impulse) override
Definition: GenericJoint.hpp:1352
void resetPosition(std::size_t index) override
Definition: GenericJoint.hpp:654
double getForceUpperLimit(size_t index) const override
Definition: GenericJoint.hpp:1274
void registerDofs() override
Definition: GenericJoint.hpp:1634
void addVelocityChangeTo(Eigen::Vector6d &velocityChange) override
Definition: GenericJoint.hpp:1717
const std::string & setDofName(std::size_t index, const std::string &name, bool preserveName=true) override
Definition: GenericJoint.hpp:219
void setAccelerationUpperLimit(std::size_t index, double acceleration) override
Definition: GenericJoint.hpp:1108
double getDampingCoefficient(std::size_t index) const override
Definition: GenericJoint.hpp:1503
void setPositionUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:619
Eigen::VectorXd getAccelerationLowerLimits() const override
Definition: GenericJoint.hpp:1101
void updateTotalForceDynamic(const Eigen::Vector6d &bodyForce, double timeStep)
Definition: GenericJoint.hpp:2142
void setVelocitiesStatic(const Vector &velocities)
Fixed-size version of setVelocities()
Definition: GenericJoint.hpp:741
void updateRelativeSpatialAcceleration() const override
Definition: GenericJoint.hpp:1664
void updateRelativeSpatialVelocity() const override
Definition: GenericJoint.hpp:1656
void setForceUpperLimit(size_t index, double force) override
Definition: GenericJoint.hpp:1261
size_t getIndexInTree(size_t index) const override
Definition: GenericJoint.hpp:307
Eigen::VectorXd getVelocityUpperLimits() const override
Definition: GenericJoint.hpp:926
void setAccelerationLowerLimit(size_t index, double acceleration) override
Definition: GenericJoint.hpp:1060
double getPosition(std::size_t index) const override
Definition: GenericJoint.hpp:511
void addChildArtInertiaImplicitToDynamic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1825
double getAccelerationLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1074
void addChildBiasImpulseTo(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse) override
Definition: GenericJoint.hpp:2042
double getPositionLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:558
void updateAccelerationDynamic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc)
Definition: GenericJoint.hpp:2244
void resetPositions() override
Definition: GenericJoint.hpp:667
void addChildBiasForceForInvMassMatrix(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce) override
Definition: GenericJoint.hpp:2453
void getInvMassMatrixSegment(Eigen::MatrixXd &invMassMat, const size_t col, const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2503
Properties getGenericJointProperties() const
Get the Properties of this GenericJoint.
Definition: GenericJoint.hpp:151
const Matrix & getInvProjArtInertia() const
Get the inverse of the projected articulated inertia.
Definition: GenericJoint.hpp:1730
void setForces(const Eigen::VectorXd &forces) override
Definition: GenericJoint.hpp:1191
void updateTotalForceForInvMassMatrix(const Eigen::Vector6d &bodyForce) override
Definition: GenericJoint.hpp:2493
void resetAccelerations() override
Definition: GenericJoint.hpp:1155
void setAcceleration(std::size_t index, double acceleration) override
Definition: GenericJoint.hpp:1001
void addChildArtInertiaToKinematic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1790
double getConstraintImpulse(std::size_t index) const override
Definition: GenericJoint.hpp:1366
size_t getIndexInSkeleton(size_t index) const override
Definition: GenericJoint.hpp:294
void resetVelocityChanges() override
Definition: GenericJoint.hpp:1345
static constexpr ActuatorType PASSIVE
Definition: Joint.hpp:67
static constexpr ActuatorType SERVO
Definition: Joint.hpp:68
static constexpr ActuatorType ACCELERATION
Definition: Joint.hpp:70
void updateArticulatedInertia() const
Tells the Skeleton to update the articulated inertia if it needs updating.
Definition: Joint.cpp:578
static constexpr ActuatorType LOCKED
Definition: Joint.hpp:72
static constexpr ActuatorType FORCE
Definition: Joint.hpp:66
static constexpr ActuatorType MIMIC
Definition: Joint.hpp:69
void setProperties(const Properties &properties)
Set the Properties of this Joint.
Definition: Joint.cpp:109
static constexpr ActuatorType VELOCITY
Definition: Joint.hpp:71
#define GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(func)
Definition: GenericJoint.hpp:61
#define GenericJoint_REPORT_DIM_MISMATCH(func, arg)
Definition: GenericJoint.hpp:44
#define GenericJoint_REPORT_OUT_OF_RANGE(func, index)
Definition: GenericJoint.hpp:53
#define GenericJoint_SET_IF_DIFFERENT(mField, value)
Definition: GenericJoint.hpp:69
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
SpaceT::Matrix inverse(const typename SpaceT::Matrix &mat)
Definition: ConfigurationSpace.hpp:187
T clip(const T &val, const T &lower, const T &upper)
Definition: Helpers.hpp:159
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:114
Eigen::Vector6d AdInvT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
fast version of Ad(Inv(T), V)
Definition: Geometry.cpp:776
Eigen::Vector6d dAdInvT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
fast version of dAd(Inv(T), F)
Definition: Geometry.cpp:850
Eigen::Vector6d ad(const Eigen::Vector6d &_X, const Eigen::Vector6d &_Y)
adjoint mapping
Definition: Geometry.cpp:808
Inertia transformInertia(const Eigen::Isometry3d &_T, const Inertia &_I)
Definition: Geometry.cpp:1414
bool isNan(double _v)
Returns whether _v is a NaN (Not-A-Number) value.
Definition: Helpers.hpp:187
Definition: BulletCollisionDetector.cpp:65
Definition: GenericJointAspect.hpp:191
Definition: GenericJointAspect.hpp:88
EuclideanPoint mInitialPositions
Initial positions.
Definition: GenericJointAspect.hpp:102
Vector mInitialVelocities
Initial velocities.
Definition: GenericJointAspect.hpp:111
Definition: JointAspect.hpp:112