Articulations¶
An articulation is a single actor comprising a set of links (each of which behaves like a rigid body) connected together with special joints. Every articulation has a tree-like structure - so there can be no loops or breaks. Their primary use is modelling physically actuated characters. They support higher mass ratios, more accurate drive models, have better dynamic stability and a more robust recovery from joint separation than standard PhysX joints. However, they are considerably more expensive to simulate.
Although articulations do not directly build on joints, they use very similar configuration mechanisms. In this section we assume familiarity with PhysX joints.
Maximal Coordinate and Reduced Articulations¶
PhysX provides two articulation implementations: maximal and reduced/generalized coordinates.
The Maximal coordinate articulation implementation is designed for game use-cases, specifically powered ragdoll simulations. As such, it supports only spherical joints with limits. This articulation implementation makes some accuracy sacrifices in favor of robustness.
The reduced coordinate articulation implementation is designed for robotics use-cases. It supports revolute, prismatic, fixed and spherical joints. It was written to favor accurate simulation and produces results that are very close to analytic models of kinematic chains. In addition, it provides inverse dynamics functionality.
Maximal Coordinate Articulations¶
Creating an Articulation¶
To create an articulation, first create the articulation actor without links:
PxArticulation* articulation = physics.createArticulation();
Then add links one by one, each time specifying a parent link (NULL for the parent of the initial link), and the pose of the new link:
PxArticulationLink* link = articulation->createLink(parent, linkPose);
PxRigidActorExt::createExclusiveShape(*link, linkGeometry, material);
PxRigidBodyExt::updateMassAndInertia(*link, 1.0f);
Articulation links have a restricted subset of the functionality of rigid bodies. They may not be kinematic, and they do not support damping, velocity clamping, or contact force thresholds. Sleep state and solver iteration counts are properties of the entire articulation rather than the individual links.
Each time a link is created beyond the first, a PxArticulationJoint is created between it and its parent. Specify the joint frames for each joint, in exactly the same way as for a PxJoint:
PxArticulationJoint* joint = static_cast<PxArticulationJoint*>(link->getInboundJoint());
joint->setParentPose(parentAttachment);
joint->setChildPose(childAttachment);
Finally, add the articulation to the scene:
scene.addArticulation(articulation);
Articulation Joints¶
The only form of articulation joint currently supported is an anatomical joint, whose properties are similar to D6 joint configured for a typical rag doll (see D6 Joint). Specifically, the joint is a spherical joint, with angular drive, a twist limit around the child joint frame's x-axis, and an elliptical swing cone limit around the parent joint frame's x-axis. The configuration of these properties is very similar to a D6 or spherical joint, but the options provided are slightly different.
The swing limit is a hard elliptical cone limit which does not support spring or restitution from movement perpendicular to the limit surface. You can set the limit ellipse angle as follows:
joint->setSwingLimit(yAngle, zAngle);
for the limit angles around y and z. Unlike the PxJoint cone limit the limit provides a tangential spring to limit movement of the axis along the limit surface. Once configured, enable the swing limit:
joint->setSwingLimitEnabled(true);
The twist limit allows configuration of upper and lower angles:
joint->setTwistLimit(lower, upper);
and again you must explicitly enable it:
joint->setTwistLimitEnabled(true);
As usual with joint limits, it is good practice to use a sufficient limit contactDistance value that the solver will start to enforce the limit before the limit threshold is exceeded.
Articulation joints are not breakable, and it is not possible to retrieve the constraint force applied at the joint.
Driving an Articulation¶
Articulations are driven through joint acceleration springs. You can set an orientation target, an angular velocity target, and spring and damping parameters that control how strongly the joint drives towards the target. You can also set compliance values, indicating how strongly a joint resists acceleration. A compliance near zero indicates very strong resistance, and a compliance of 1 indicates no resistance.
Articulations are driven in two phases. First the joint spring forces are applied (we use the term internal forces for these) and then any external forces such as gravity and contact forces. You may supply different compliance values at each joint for each phase.
Note that with joint acceleration springs, the required strength of the spring is estimated using just the mass of the two bodies connected by the joint. By contrast, articulation drive springs account for the masses of all the bodies in the articulation, and any stiffness from actuation at other joints. This estimation is an iterative process, controlled using the externalDriveIterations and internalDriveIterations properties of the PxArticulation class.
Instead of setting the target quaternion for the joint drive, it is possible to set the orientation error term directly as a rotation vector. The value is set as the imaginary part of the target quaternion, with the real part set to 0.
joint->setDriveType(PxArticulationJointDriveType::eERROR); joint->setTargetOrientation(PxQuat(error.x, error.y, error.z, 0));This allows the spring to be driven with a larger positional error than can be generated by the difference between 2 quaternions. Obtain the same behavior as with target quaternions by computing the error from the target quaternion, link frames, and joint frames as follows:
PxTransform cA2w = parentPose.transform(joint.parentPose); // parent attachment frame PxTransform cB2w = childPose.transform(joint.childPose); // child attachment frame transforms.cB2cA = transforms.cA2w.transformInv(transforms.cB2w); // relative transform if(transforms.cB2cA.q.w<0) // shortest path path transforms.cB2cA.q = -transforms.cB2cA.q; // rotation vector from relative transform to drive pose PxVec3 error = log(j.targetPosition * cB2cA.q.getConjugate());
Articulation Projection¶
When any of the joints in an articulation separate beyond a specified threshold, the articulation is projected back together automatically. Projection is an iterative process, and the PxArticulation functions PxArticulation::setSeparationTolerance() and PxArticulation::setMaxProjectionIterations() control when projection occurs and trade cost for robustness.
Articulations and Sleeping¶
Like rigid dynamic objects, articulations are also put into a sleep state if their energy falls below a certain threshold for a period of time. In general, all the points in the section Sleeping apply to articulations as well. The main difference is that articulations can only go to sleep if each individual articulation link fulfills the sleep criteria.
Reduced Coordinate Articulations¶
The PhysX reduced coordinate articulation provide a wide range of functionality required for the robotics simulation and other simulations that require accurate simulations of mechanical structures. The key difference between reduced coordinates and maximal coordinate articulations is that reduced coordinate articulations are guaranteed to exhibit no joint error.
The simulation may also be suitable for use in games to simulate humanoids. However, the introducing of velocity clamps or damping may be required to ensure stability of the simulation at large angular velocities, which would not be required when using the maximal coordinate articulations. The reason for this is the explicit integration of coriolis and centrifugal forces combined with the lack of automatic joint or body damping.
This technique has different performance properties to the maximal coordinate articulations or rigid bodies and joints. In a simulation without a significant number of contacts affecting the articulation, the simulation cost is generally proportional to the number of degrees of freedom, rather than the number of links. Therefore, in common robotics applications, where most joints have 0-1 degrees of freedom, the cost of simulating using reduced coordinate articulations is often lower than the cost of simulating the same robot using either maximal coordinate articulations or even rigid bodies and joints.
Creating a Reduced Coordinate Articulation¶
The process of creating a reduced coordinate articulation is very similar to the process of creating a maximal articulation. To create an articulation, first create the articulation actor without links:
PxArticulationReducedCoordinate* articulation = physics.createReducedCoordinateArticulation();
Then add links one by one, each time specifying a parent link (NULL for the parent of the initial link), and the pose of the new link:
PxArticulationLink* link = articulation->createLink(parent, linkPose);
PxRigidActorExt::createExclusiveShape(*link, linkGeometry, material);
PxRigidBodyExt::updateMassAndInertia(*link, 1.0f);
Articulation links have a restricted subset of the functionality of rigid bodies. They may not be kinematic, and they do not support damping, velocity clamping, or contact force thresholds. Sleep state and solver iteration counts are properties of the entire articulation rather than the individual links.
Each time a link is created beyond the first, a PxArticulationJoint is created between it and its parent. Specify the joint frames for each joint, in exactly the same way as for a PxJoint:
PxArticulationJointReducedCoordinate* joint = static_cast<PxArticulationJointReducedCoordinate*>(link->getInboundJoint());
joint->setParentPose(parentAttachment);
joint->setChildPose(childAttachment);
Finally, add the articulation to the scene:
scene.addArticulation(articulation);
Articulation Joints¶
The PxArticulationJointReducedCoordinate provides an interface that is similar to the PxD6Joint interface for rigid body joints. However, it incurs certain limitations. By default, all axes are locked and the joint type is undefined. The first requirement is that the user defines the joint type. This can currently be any of the following:
- PxArticulationJointType::eFIX - a fixed joint with 0 degrees of freedom
- PxArticulationJointType::ePRISMATIC - a prismatic (sliding) joint with 1 degree of freedom
- PxArticulationJointType::eREVOLUTE - a revolute (hinge) joint with 1 degree of freedom
- PxArticulationJointType::eSPHERICAL - a spherical (ball-in-socket) joint with up to 3 degrees of freedom
The joint type can be set below using the following code. Here will define the joint to be a revolute (hinge) joint:
joint->setJointType(PxArticulationJointType::eREVOLUTE);
After defining the joint type, you must enable motion on the required axes. Motion can be either locked (default), free or limited. Here, we set the twist axis to be limited:
joint->setMotion(PxArticulationAxis::eTWIST, PxArticulationMotion::eLIMITED);
If a joint is limited, you can define the limit range. Limit ranges are defined using a min/max pair. Here, we set a limit in the range (-0.2, 0.5):
PxReal lowLimit = -0.2f;
PxReal highLimit = 0.5f;
joint->setLimit(PxArticulationAxis::eTWIST, lowLimit, highLimit);
In addition, if required, a drive can be added to the joint. As with limits, this is defined on a per-axis basis. The drives operate like a PD controller and have 2 terms: stiffness and damping. Stiffness controls how strongly the drive drives towards a target joint position/angle and damping controls how strongly the joint drives towards the target velocity. In this case, we set a stiffness value of 10 and ad damping value of 0.1. In addition, we define the maximum motor force to be PX_MAX_F32:
PxReal stiffness = 10.f;
PxReal damping = 0.1f;
PxReal forceLimit = PX_MAX_F32;
joint->setDrive(PxArticulationAxis::eTWIST, stiffness, damping, forceLimit)
With these parameters the joint described will limit motion within (-0.2, 0.5) and drive towards a joint angle value of 0 with a stiffness of 10. The joint can be made to drive towards a target angle of 0.1 using the code below:
PxReal driveTargetAngle = 0.1f;
joint->setDriveTarget(PxArticulationAxis::eTWIST, driveTargetAngle);
Similarly, the joint can be made to drive towards a target velocity of -0.2 using the code below:
PxReal driveTargetVelocity = -0.2f;
joint->setDriveVelocity(PxArticulationAxis::eTWIST, driveTargetVelocity);
Articulation joints are not breakable, and it is not possible to retrieve the constraint force applied at the joint.
It should be noted that spherical joints are special-case handled by the reduced coordinate articulations. When there is just 1 degree of freedom, behavior is equivalent to a revolute joint type, but performance will be worse so it is recommended to use revolute joints instead of spherical joints wherever possible. When there are 2 or more degrees of freedom unlocked, rotational motion is integrated by rotating by decomposed quaternions rather than by euler angles to avoid gimbal lock. However, this technique can lead to rotational axis drift, which is corrected by additional constraints in the simulation, which could lead to slight movement on the remaining locked rotational axis.
The root link in a reduced coordinate articulation can be made to operate like a static body. This is advantageous over constraining the root link using a joint because the immoveable property of the root link is solved perfectly. The following code makes the root link of an articulation be fixed:
articulation->setArticulationFlag(PxArticulationFlag::eFIX_BASE, true);
Joint positions and velocities¶
Reduced coordinate articulations internally keep track of scalar joint positions, velocities and accelerations, with 1 entry corresponding to each degree of freedom. Joint position represents the relative offset between a parent and child link along/around a degree of freedom. If it is a rotational axis, this would represent an angle in radians. If it is a translational axis, this would represent a distance in whatever units the simulation is being performed in. Similarly, joint velocities represent a velocities around/along the degree of freedom in either radians/s or units/s, where units corresponds to the distance unit being used in the simulation.
Joint position values are used to integrate the links' poses. The root link's world space pose provides a reference frame from which the poses for all other links in the articulation are calculated. This ensures that there will never be any joint separation or rotations around locked axes.
The caveat with this approach is that it is not possible to simply update the pose of links in an articulation because this new transform could violate locked axes and would differ from the joint position. Instead, it is necessary to update the joint positions, which triggers new link poses to be computed. This ensures that the internal data from which poses are computed is internally consistent at all times.
Link velocities and joint velocities share a similar relationship. A link's world-space velocities are derived from the world-space velocity of their parent link and the current joint velocity. This means that the root link stores a world-space velocity and all other links' velocities are computed by propagating this velocity and the respective joint velocities through the articulation. As such, it is not possible to directly set the velocity of links in an articulation. Instead, it is necessary to modify the joint velocities from which the links' velocities are calculated. It is legal to apply world-space impulses on links. These will be propagated through the articulation as part of forward dynamics.
In addition to applying impulses and setting joint positions and velocities, it is possible to interact with the articulation links through the application of joint forces/torques, which behave like an actuator.
PxArticulationCache¶
Direct access to joint positions, velocities and forces are provided through a class called PxArticulationCache. This class exposes direct access to the scalar values that control a reduced coordinate articulation.
You create an articulation cache like this:
PxArticulationCache* cache = articulation->createCache();
This method creates a cache with which to store internal articulation state. This cache is constructed specifically for a given articulation and contains the exact space to store data about that articulation. It should not be shared between different articulations. Similarly, if the properties of an articulation change (a link is added/removed, degrees of freedom are changed etc.), it is necessary to destroy the cache and recreate it.
Once a cache has been created, you must copy data to the cache. In this case, we copy all data to the cache, but you can filter and only copy the specific data you are interested in:
articulation->copyInternalStateToCache(*cache, PxArticulationCache::eALL);
The cache stores its own copy of the internal state of the articulation. Any modifications to the state in the cache do not alter the internal state of the articulation that the cache was created from. To update the internal state of the articulation, it is necessary to apply the cache. In this example, we apply all state to the articulation, but only a subset of the state can be applied by passing different flags:
articulation->applyCache(*cache, PxArticulationCache::eALL);
It is not legal to copy state to a cache or apply a cache while the simulation is running.
A cache stores sufficient information to be able to record the state of an entire articulation at a snapshot in time and then reset the articulation back to that state. It is legal to create and maintain multiple articulation caches for a given articulation.
A cache provides access to the root link's state, including transform, velocities and accelerations. It also provides access to jointVelocity, jointAcceleration, jointPosition and jointForce.
Simple operations like resetting an articulation back to its original configuration or zeroing joint velocity can be done with the following code snippet:
PxMemZero(cache->jointVelocity, sizeof(PxReal)*articulation->getDofs());
Joint data (velocity, position etc.) provide one entry per degree of freedom. These values are stored in a specific order to facilitate propagation throughout the articulation. This order may be different from the order in which the joints were originally defined. PhysX provides some utility functions to compute the index of the a joint in these arrays. To compute the link's low-level index, call:
PxU32 llIndex = articulationLink->getLinkIndex();
To compute the number of degrees of freedom for the link, call:
PxU32 dofs = articulationLink->getInboundJointDof();
The location of the entries corresponding to a given link can be found by summing the number of degrees of freedom of all preceding links. A snippet providing an example of how to do this is provided below:
PxU32 dofStarts[TotalLinkCount];
dofStarts[0] = 0; //We know that the root link does not have a joint
for(PxU32 i = 1; i < TotalLinkCount; ++i)
{
PxU32 llIndex = links[i]->getLinkIndex();
PxU32 dofs = links[i]->getInboundJointDof();
dofStarts[llIndex] = dofs;
}
PxU32 count = 0;
for(PxU32 i = 1; i < TotalLinkCount; ++i)
{
PxU32 dofs = dofStarts[i];
dofStarts[i] = count;
count += dofs;
}
Setting a particular joint position for a given link's inbound joint can be done using the snippet below:
cache->jointPosition[dofStarts[link->getLinkIndex()]] = newJointPosition;
In addition, it contains several fields related to inverse dynamics, which will be described later in this document.
The Jacobian matrix is an important concept to roboticists. Multiplication with the Jacobian matrix maps the joint space velocities of the robot to world space velocities. The Jacobian matrix of an articulation can be computed using:
PxU32 nRows;
PxU32 nCols;
articulation->computeDenseJacobian(*cache, nRows, nCols);
This will write the Jacobian matrix to PxArticulationCache::denseJacobian; the dimensions of the matrix are written to the two unsigned integers. Note how the Jacobian matrix is a sparse triangular matrix so such explicit dense representation is in general not an optimal use of memory. PhysX does not use this representation internally.
Inverse Dynamics¶
In addition to forward dynamics, reduced coordinate articulations offer inverse dynamics functionality. This is a suite of utility functions to compute the joint forces required to counteract gravity, coriolis/centrifugal force, external forces and contacts/constraints. In addition, there are utility functions to compute kinematic jacobians, the mass matrix, coefficient matrix and lambda values.
The following description assumes knowledge of inverse dynamics concepts.
Prior to performing any inverse dynamics calculations, it is necessary to ensure that constant joint data has been computed. In order to do this, call:
articulation->commonInit();
The inverse dynamics functions operate on an articulation and a PxArticulationCache. The majority of properties in PxArticulationCache are stored in a reduced/generalized coordinate space, where one entry corresponds to a degree of freedom. To simplify working in this space, PhysX provides the following methods to pack and unpack data to convert between reduced/generalized and maximal coordinates.
To unpack data from reduced/generalized coordinates to maximal coordinates:
PxReal maximalJointForces[NbLinks*6]; //There are 6 dofs (x,y,z,twist,swing1,swing2) per link
articulation->unpackJointData(cache->jointForces, maximalJointForces);
To pack data from maximal coordinates to reduced/generalized coordinates:
articulation->packJointData(maximalJointForces, cache->jointForces);
The following method computes the joint force required to counteract gravity and deposits the set of joint forces in the force buffer in a PxArticulationCache:
articulation->computeGeneralizedGravityForce(*cache);
To compute the joint force required to counteract coriolis and centrifugal force, we must first provide the joint velocities of the articulation because coriolis/centrifugal forces are dependent on those values. In this example, we extract the velocities from the articulation before computing coriolis/centrifugal force:
articulation->copyInternalStateToCache(*cache, PxArticulationCache::eVELOCTY);
articulation->computeCoriolisAndCentrifugalForce(*cache);
To compute the joint acceleration for the articulation, call:
articulation->computeJointAcceleration(*cache);
Joint acceleration is dependent on gravity and current link velocity. The resulting acceleration can be used to compute a set of joint forces capable of maintaining the current joint velocities.
To convert a joint acceleration into a joint velocity. Input is cache.jointAcceleration, output is cache.jointForce:
articulation->computeJointForce(*cache);
The generalized mass matrix can be computed using the following method. Output is placed in cache.massMatrix. This matrix represents the joint space inertia of the articulation and can be used to convert joint accelerations into joint forces/torques:
articulation->computeGeneralizedMassMatrix(*cache);
Articulations are tree structures and do not support closed loops natively. PhysX can simulate closed loop systems by the use of joints. Additionally, contacts between links and other rigid bodies (e.g. the ground) can form loops if more than one link has contacts. These joints and contacts are solved by the rigid body solver during PhysX simulation, but it is often desirable to factor these constraints into inverse dynamics.
To facilitate this, PhysX provides a mechanism to register loop constraints with the articulation:
PxJoint* joint = ...;
articulation->addLoopJoint(joint);
articulation->removeLoopJoint(joint);
In order to represent contacts, use PxContactJoint : this is a new addition specifically for inverse dynamics that represents a contact as an extension to the joint system. Currently, the following features are restricted to the use of PxContactJoint. Support for additional joint types will be added in the near future.
Inverse dynamics provides functionality to calculate the coefficient matrix. This matrix is an NxM matrix, where N is the number of degrees of freedom in the articulation and M is the number constraint rows. This matrix represents the joint force caused by a unit impulse applied to each constraint:
PxU32 coefficientMatrixSize = articulation->getCoefficentMatrixSize();
PxReal* coefficientMatrix = new PxReal[coefficientMatrixSize];
cache->coefficientMatrix = coefficientMatrix;
articulation->computeCoefficientMatrix(*cache);
The coefficient matrix can be used to convert a set of lambda values (impulses applied by the respective constraints) into a set of joint forces to counteract these impulses. However, the lambda values are only known after a frame's simulation occurs, so it may be necessary to know these lambda values before they are applied in the solver. However, even if these lambda values are known ahead-of-time, applying a counteracting force may not yield the desired results because the act of applying additional force on the joints may influence the lambda values, resulting in a different set of joint forces.
In order to overcome this feedback-loop, inverse dynamics provides a function to compute the lambda values for the loop constraints, assuming that the joint forces required to counteract these lambdas are also applied. This technique is an iterative process that converges on a stable set of joint forces.
First, we must allocate sufficient space for the lambda values
PxReal* lambda = new PxReal[articulation->getNbLoopJoints()]; cache->lambda = lambda;
In order to compute the lambda values, we need to have 2 caches: one to store the initial state, and one to calculate the final state:
PxArticulationCache* initialState = articulation->createCache()
articulation->copyInternalStateToCache(*initialState, PxArticulationCache::eALL);
Now we can compute the joint forces values. In addition, we must compute the internal and external joint forces caused by gravity and coriolis/centrifugal force to ensure that we converge on a result that will match forward dynamics. The joint force values for internal/external accelerations can be calculated using the methods outlined earlier. The method is iterative and terminates either after reaching convergence or when a maximum number of iterations is run (in this case 200). Resulting joint force is output in cache.jointForce:
const PxU32 maxIterations = 200;
articulation->computeLambda(*cache, *initialState, jointForces, maxIterations);
Computes the dense representation of an inherently sparse matrix. Multiplication with this matrix maps joint space velocities to 6DOF world space linear and angular velocities:
articulation->getDenseJacobian(*cache, nRows, nCols);