Revision control
Copy as Markdown
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0
#ifndef DY_ARTICULATION_CPUGPU_H
#define DY_ARTICULATION_CPUGPU_H
#include "foundation/PxSimpleTypes.h"
#include "foundation/PxBasicTemplates.h"
#include "foundation/PxVec3.h"
#include "foundation/PxQuat.h"
#include "PxArticulationJointReducedCoordinate.h"
#include "CmSpatialVector.h"
#include "DyFeatherstoneArticulationUtils.h"
#define DY_MIN_RESPONSE 1e-12f
#define DY_ARTICULATION_MIN_RESPONSE 1e-12f
#define DY_ARTICULATION_CFM 2e-4f
namespace physx
{
namespace Dy
{
/**
* Compute the parent-to-child world translation Rw from the stored world
* orientations and link-local anchor offsets. Precision-clean at any world
* coordinate: uses only unit world quaternions and small link-local offsets;
* no world-scale position subtraction.
*
* The joint anchor coincides on both bodies, so
* Rw = child.world.p - parent.world.p
* = parentQW.rotate(parentOffset) + childQW.rotate(slide - childOffset)
* where parentOffset = joint.parentPose.p (unchanged)
* childOffset = joint.childPose.p (NOT negated -- caller passes raw)
* slide = prismatic translation (zero for all other joint types)
*/
template<typename T>
PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3T<T> computeLinkRw(
const PxQuatT<T>& parentQW,
const PxQuatT<T>& childQW,
const PxVec3T<T>& parentOffset,
const PxVec3T<T>& childOffset,
const PxVec3T<T>& slide)
{
return parentQW.rotate(parentOffset) + childQW.rotate(slide - childOffset);
}
struct ArticulationImplicitDriveDesc
{
PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc(PxZERO)
: driveTargetVelPlusInitialBias(0.0f),
driveBiasCoefficient(0.0f),
driveVelMultiplier(0.0f),
driveImpulseMultiplier(0.0f),
driveTargetPosBias(0.0f)
{
}
PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc
(const PxReal targetVelPlusInitialBias, const PxReal biasCoefficient, const PxReal velMultiplier,
const PxReal impulseMultiplier, const PxReal targetPosBias)
: driveTargetVelPlusInitialBias(targetVelPlusInitialBias),
driveBiasCoefficient(biasCoefficient),
driveVelMultiplier(velMultiplier),
driveImpulseMultiplier(impulseMultiplier),
driveTargetPosBias(targetPosBias)
{
}
PxReal driveTargetVelPlusInitialBias;
PxReal driveBiasCoefficient;
PxReal driveVelMultiplier;
PxReal driveImpulseMultiplier;
PxReal driveTargetPosBias;
};
PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc computeImplicitDriveParamsForceDrive
(const PxReal stiffness, const PxReal damping, const PxReal dt, const PxReal simDt,
const PxReal unitResponse, const PxReal geomError, const PxReal targetVelocity, const bool isTGSSolver)
{
ArticulationImplicitDriveDesc driveDesc(PxZero);
const PxReal a = dt * (dt * stiffness + damping);
const PxReal b = dt * (damping * targetVelocity);
const PxReal x = unitResponse > 0.f ? 1.0f / (1.0f + a * unitResponse) : 0.f;
const PxReal initialBias = geomError - (simDt - dt) * targetVelocity; //equal to geomError for PGS as simDt = dt
const PxReal driveBiasCoefficient = stiffness * x * dt;
driveDesc.driveTargetVelPlusInitialBias = (x * b) + driveBiasCoefficient * initialBias;
driveDesc.driveVelMultiplier = -x * a;
driveDesc.driveBiasCoefficient = driveBiasCoefficient;
driveDesc.driveImpulseMultiplier = isTGSSolver ? 1.f : 1.0f - x;
driveDesc.driveTargetPosBias = isTGSSolver ? driveBiasCoefficient * targetVelocity : 0.f;
return driveDesc;
}
PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc computeImplicitDriveParamsAccelerationDrive
(const PxReal stiffness, const PxReal damping, const PxReal dt, const PxReal simDt,
const PxReal recipUnitResponse, const PxReal geomError, const PxReal targetVelocity, const bool isTGSSolver)
{
ArticulationImplicitDriveDesc driveDesc(PxZero);
const PxReal a = dt * (dt * stiffness + damping);
const PxReal b = dt * (damping * targetVelocity);
const PxReal x = 1.0f / (1.0f + a);
const PxReal initialBias = geomError - (simDt - dt) * targetVelocity; //equal to geomError for PGS as simDt = dt
const PxReal driveBiasCoefficient = stiffness * x * recipUnitResponse * dt;
driveDesc.driveTargetVelPlusInitialBias = (x * b * recipUnitResponse) + driveBiasCoefficient * initialBias;
driveDesc.driveVelMultiplier = -x * a * recipUnitResponse;
driveDesc.driveBiasCoefficient = driveBiasCoefficient;
driveDesc.driveImpulseMultiplier = isTGSSolver ? 1.f : 1.0f - x;
driveDesc.driveTargetPosBias = isTGSSolver ? driveBiasCoefficient * targetVelocity : 0.f;
return driveDesc;
}
/**
\brief Compute the parameters for an implicitly integrated spring.
\param[in] driveType is the type of drive.
\param[in] stiffness is the drive stiffness (force per unit position bias)
\param[in] damping is the drive damping (force per unit velocity bias)
\param[in] dt is the timestep that will be used to forward integrate the spring position bias.
\param[in] simDt is the simulation timestep.
\param[in] unitResponse is the multiplier that converts impulse to velocity change.
\param[in] recipUnitResponse is the reciprocal of unitResponse
\param[in] geomError is the position bias with value (targetPos - currentPos)
\param[in] targetVelocity is the target velocity of the drive.
\param[in] isTGSSolver should be set true when computing implicit spring params for TGS and false for PGS.
\return The implicit spring parameters.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE ArticulationImplicitDriveDesc computeImplicitDriveParams
(const PxArticulationDriveType::Enum driveType, const PxReal stiffness, const PxReal damping, const PxReal dt, const PxReal simDt,
const PxReal unitResponse, const PxReal recipUnitResponse, const PxReal geomError, const PxReal targetVelocity, const bool isTGSSolver)
{
ArticulationImplicitDriveDesc driveDesc(PxZero);
switch (driveType)
{
case PxArticulationDriveType::eFORCE:
{
driveDesc = computeImplicitDriveParamsForceDrive(stiffness, damping, dt, simDt, unitResponse, geomError, targetVelocity, isTGSSolver);
}
break;
case PxArticulationDriveType::eACCELERATION:
{
driveDesc = computeImplicitDriveParamsAccelerationDrive(stiffness, damping, dt, simDt, recipUnitResponse, geomError, targetVelocity, isTGSSolver);
}
break;
case PxArticulationDriveType::eNONE:
{
PX_ASSERT(false);
}
break;
}
return driveDesc;
}
/**
\brief Compute the friction impulse.
\param[in] frictionImpulse is the accumulated frictiom impulse on the current simulation step.
\param[in] staticFrictionImpulse is threshold to prevent motion of the joint.
\param[in] dynamicFrictionImpulse is constant friction applied to moving joints .
\param[in] viscousFrictionCoefficient is the coefficient of velocity dependent friction term.
\param[in] jointVel is the current velocity of the joint.
\return The friction impulse.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeFrictionImpulse
(PxReal frictionImpulse, const PxReal staticFrictionImpulse, const PxReal dynamicFrictionImpulse, const PxReal viscousFrictionCoefficient, const PxReal jointVel)
{
if (PxAbs(frictionImpulse) > staticFrictionImpulse)
frictionImpulse = PxClamp(frictionImpulse, -dynamicFrictionImpulse - viscousFrictionCoefficient * PxAbs(jointVel), dynamicFrictionImpulse + viscousFrictionCoefficient * PxAbs(jointVel));
return frictionImpulse;
}
/**
\brief Compute the drive impulse for an implicitly integrated spring.
\param[in] accumulatedDriveImpulse is the drive impulse that has accumulated since the solver started on the current simulation step.
\param[in] jointVel is the current velocity of the joint.
\param[in] jointDeltaPos is the change in joint position that has accumulated since the solver started on the current simulation step.
\param[in] elapsedTime is the time elapsed on the current simulation step (used only for the TGS solver).
\param[in] driveDesc is the implicit spring params.
\return The impulse for the implicitly integrated spring.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeDriveImpulse
(const PxReal accumulatedDriveImpulse, const PxReal jointVel, const PxReal jointDeltaPos, const PxReal elapsedTime, const ArticulationImplicitDriveDesc& driveDesc)
{
const PxReal unclampedForce =
accumulatedDriveImpulse * driveDesc.driveImpulseMultiplier
+ jointVel * driveDesc.driveVelMultiplier
+ driveDesc.driveTargetVelPlusInitialBias
- jointDeltaPos * driveDesc.driveBiasCoefficient
+ elapsedTime * driveDesc.driveTargetPosBias;
return unclampedForce;
}
PX_CUDA_CALLABLE PX_INLINE PxPair<PxReal, PxReal> computeBoundsForNegativeImpulse(
PxReal velDeviation,
PxReal max,
PxReal denom1,
PxReal denom2,
PxReal impulse)
{
PxReal bound1;
PxReal bound2;
// invalid case
if(PxAbs(denom1) < PX_EPS_F32)
{
return PxPair<PxReal, PxReal>(-PX_MAX_F32, PX_MAX_F32);
}
if (PxAbs(denom2) < PX_EPS_F32)
{
bound1 = (velDeviation - max) / denom1;
if (PxAbs(velDeviation + max) < PX_EPS_F32)
bound2 = PX_MAX_F32;
else
bound2 = (velDeviation + max) > 0.0f ? PX_MAX_F32 : -PX_MAX_F32;
return PxPair<PxReal, PxReal>(bound1, PxMin(0.0f, bound2));
}
bound1 = (velDeviation - max) / denom1;
bound2 = (velDeviation + max) / denom2;
if (denom2 >= 0.0f) {
return PxPair<PxReal, PxReal>(PxMax(bound1, impulse), PxMin(0.0f, bound2));
}
else {
return PxPair<PxReal, PxReal>( PxMax(PxMax(bound1, bound2), impulse), 0.0f);
}
}
PX_CUDA_CALLABLE PX_INLINE PxPair<PxReal, PxReal> computeBoundsForPositiveImpulse(
PxReal velDeviation,
PxReal max,
PxReal denom1,
PxReal denom2,
PxReal impulse)
{
PxReal bound1;
PxReal bound2;
// invalid case
if(PxAbs(denom2) < PX_EPS_F32)
{
return PxPair<PxReal, PxReal>(-PX_MAX_F32, PX_MAX_F32);
}
if (PxAbs(denom1) < PX_EPS_F32)
{
bound2 = (velDeviation + max) / denom2;
if (PxAbs(velDeviation - max) < PX_EPS_F32)
bound1 = -PX_MAX_F32;
else
bound1 = (velDeviation - max) > 0.0f ? PX_MAX_F32 : -PX_MAX_F32;
return PxPair<PxReal, PxReal>(PxMax(0.0f, bound1), bound2);
}
bound1 = (velDeviation - max) / denom1;
bound2 = (velDeviation + max) / denom2;
if (denom1 >= 0.0f)
return PxPair<PxReal, PxReal>(PxMax(0.0f, bound1), PxMin(bound2, impulse));
else
return PxPair<PxReal, PxReal>(0.0f, PxMin(PxMin(bound1, bound2), impulse));
}
/**
\brief Compute the drive impulse implicitly.
\param[in] jointVel0 is the joint velocity before drive impulse is applied.
\param[in] driveImpulse0 accumulated drive effort until the current iteration.
\param[in] driveImpulse drive impulse to be clamped.
\param[in] response response.
\param[in] maxJointVel max actuator velocity.
\param[in] maxImpulse max impulse.
\param[in] speedImpulseGradient speed impulse gradient of the performance envelope.
\return The clamped drive impulse.
*/
PX_CUDA_CALLABLE PX_INLINE PxReal clampDriveImpulse(
PxReal jointVel0,
PxReal driveImpulse0,
PxReal driveImpulse,
PxReal response,
PxReal maxJointVel,
PxReal maxImpulse,
PxReal speedImpulseGradient,
PxReal velocityDependentResistance)
{
PxReal velDeviation = driveImpulse0 * response - jointVel0;
PxPair<PxReal, PxReal> bounds1;
PxPair<PxReal, PxReal> bounds2;
if (driveImpulse > 0.0f) {
PxReal denom1 = response - speedImpulseGradient;
PxReal denom2 = response + speedImpulseGradient;
bounds1 = computeBoundsForPositiveImpulse(velDeviation, maxJointVel, denom1, denom2, driveImpulse);
if (velocityDependentResistance == 0.0f) {
bounds2 = PxPair<PxReal, PxReal>(-maxImpulse, maxImpulse);
} else {
denom1 = response - 1.0f / velocityDependentResistance;
denom2 = response + 1.0f / velocityDependentResistance;
bounds2 = computeBoundsForPositiveImpulse(velDeviation, maxImpulse / velocityDependentResistance, denom1, denom2, driveImpulse);
}
}
else
{
PxReal denom1 = response + speedImpulseGradient;
PxReal denom2 = response - speedImpulseGradient;
bounds1 = computeBoundsForNegativeImpulse(velDeviation, maxJointVel, denom1, denom2, driveImpulse);
if (velocityDependentResistance == 0.0f) {
bounds2 = PxPair<PxReal, PxReal>(-maxImpulse, maxImpulse);
} else {
denom1 = response + 1.0f / velocityDependentResistance;
denom2 = response - 1.0f / velocityDependentResistance;
bounds2 = computeBoundsForNegativeImpulse(velDeviation, maxImpulse / velocityDependentResistance, denom1, denom2, driveImpulse);
}
}
// Combine bounds
PxReal lowerBound = PxMax(bounds1.first, bounds2.first);
PxReal upperBound = PxMin(bounds1.second, bounds2.second);
// Check for invalid bounds
if (lowerBound > upperBound) {
return 0.0f;
}
return PxClamp(driveImpulse, lowerBound, upperBound);
}
/**
\brief Apply limit constraints to an articulation joint dof.
1) Compute the delta impulse required to maintain limit constraints.
2) Individually accumulate the impulses that have been applied to maintain both the lower and upper limit.
3) Compute the updated joint speed after applying the delta impulse.
\param[in] dt is the timestep of the simulation
\param[in] recipDt has value 1/dt
\param[in] isVelIter is true if we are performing a velocity iteration, false if performing a position iteration.
\param[in] response is the deltaSpeed response of the joint dof to a unit impulse.
\param[in] recipResponse has value 1/response.
\param[in] erp is the Baumgarte multiplier used to resolve a fraction of the limit error.
\param[in] errorLow is the lower bound of the limit.
\param[in] errorHigh is the upper bound of the limit.
\param[in] jointPDelta is the change to the joint position that has accumulated over solver iterations.
\param[in,out] lowImpulse_ is the accumulated impulse that has been applied to maintain the limit's lower bound.
\param[in,out] highImpulse_ is the accumulated impulse that has applied to maintain the limit's upper bound.
\param[in,out] jointV_ is the joint speed before and after applying the limit impulses.
\return deltaImpulse required to enforce the upper and lower limits.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeLimitImpulse
(const PxReal dt, const PxReal recipDt, const bool isVelIter,
const PxReal response, const PxReal recipResponse, const PxReal erp,
const PxReal errorLow, const PxReal errorHigh,
const PxReal jointPDelta,
PxReal& lowImpulse_, PxReal& highImpulse_, PxReal& jointV_)
{
// PT: avoid aliasing
PxReal jointV = jointV_;
PxReal lowImpulse = lowImpulse_;
PxReal highImpulse = highImpulse_;
const PxReal futureDeltaJointP = jointPDelta + jointV * dt;
// for all errors: Negative means violated
const PxReal currErrLow = errorLow + jointPDelta;
const PxReal nextErrLow = errorLow + futureDeltaJointP;
const PxReal currErrHigh = errorHigh - jointPDelta;
const PxReal nextErrHigh = errorHigh - futureDeltaJointP;
bool limited = false;
bool newLow = false;
bool newHigh = false;
const PxReal tolerance = 0.f;
PxReal deltaF = 0.f;
if (currErrLow < tolerance || nextErrLow < tolerance)
{
PxReal newJointV = jointV;
if (currErrLow < tolerance)
{
//Only apply an impulse to resolve the bias on pos iters.
//Any subsequent vel iter will attempt to bring the dof vel to zero in the
//code block that follows (if !limited) if this dof had already received an
//impulse to resolve the limit bias.
if (!isVelIter)
{
limited = true;
newJointV = -currErrLow * recipDt * erp;
}
}
else
{
// Currently we're not in violation of the limit but would be after this time step given the current velocity.
// To prevent that future violation, we want the current velocity to only take us right to the limit, not across it
limited = true;
newJointV = -currErrLow * recipDt;
}
// In position iterations, the newJointV is now such that we end up exactly on the limit after this time step (ignoring erp)
// However, we ignored the current velocity, which may already take us further away from the limit than the newJointV.
// Therefore, we additionally have to check now that the impulse we're applying is only repulsive overall.
if(limited)
{
const PxReal deltaV = newJointV - jointV;
deltaF = PxMax(lowImpulse + deltaV * recipResponse, 0.f) - lowImpulse; // accumulated limit impulse must be repulsive
lowImpulse += deltaF;
newLow = true;
}
}
else if (currErrHigh < tolerance || nextErrHigh < tolerance)
{
PxReal newJointV = jointV;
if (currErrHigh < tolerance)
{
if (!isVelIter)
{
limited = true;
newJointV = currErrHigh * recipDt * erp;
}
}
else
{
limited = true;
newJointV = currErrHigh * recipDt;
}
if(limited)
{
const PxReal deltaV = newJointV - jointV;
deltaF = PxMin(highImpulse + deltaV * recipResponse, 0.f) - highImpulse;
highImpulse += deltaF;
newHigh = true;
}
}
if (!limited)
{
// If no limit is violated right now, it could still be that a limit was active in an earlier iteration and
// overshot. Therefore, we give that limit from which the joint position is currently moving away a chance to
// pull back and correct the overshoot.
// The pull-back impulse is the smaller of
// a) The impulse needed to bring the joint velocity to zero.
// b) The opposite impulse of the already applied joint limit impulse, thereby cancelling out the accumulated effect of the limit.
//Note: if the limit was not breached, lowImpulse and highImpulse will both be 0 and deltaF will also be 0.
const PxReal impulseForZeroVel = -jointV * recipResponse;
if (jointV > 0.f) // moving away from the lower limit
{
deltaF = PxMax(impulseForZeroVel, -lowImpulse);
lowImpulse += deltaF;
newLow = true;
}
else // moving away from the higher limit
{
deltaF = PxMin(impulseForZeroVel, -highImpulse);
highImpulse += deltaF;
newHigh = true;
}
}
jointV += deltaF * response;
if(newLow)
lowImpulse_ = lowImpulse;
if(newHigh)
highImpulse_ = highImpulse;
jointV_ = jointV;
return deltaF;
}
/**
\brief Translate a spatial vector from the frame of one link (the source link) to the frame of another link (the target link).
\param[in] offset is the vector from the source link to the target link (== posTargetLink - posSourceLlink)
\param[in] s is the spatial vector in the frame of the source link with s.top representing the angular part of the
spatial vector and s.bottom representing the linear part of the spatial vector.
\return The spatial vector translated into the frame of the target link with top representing the angular part of the spatial vector
and bottom representing the linear part of the spatial vector.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF translateSpatialVector(const PxVec3& offset, const Cm::SpatialVectorF& s)
{
return Cm::SpatialVectorF(s.top, s.bottom + offset.cross(s.top));
}
/**
\brief Propagate to the parent link
a) a spatial impulse applied to a child link
b) a joint impulse applied to the child link's inbound joint.
The Mirtich equivalent is the equation for Y in Figure 5.7, page 141 but with a modification
to account for a joint impulse applied to the child link's inbound joint.
If the joint impulse is Q and the child link impulse is YChildW then the parent link impulse has
the form:
YParentW = translateChildToParent{ YChildW + (I * s) *(Q - s^T * YChildW)/(s * I * s^T) }
Optionally accumulate [Q - S^T * YChildW] because this can be useful to reuse when propagating
delta spatial velocity from parent link to child link.
\param[in] parentToChild is the vector from parent link to child link such that childLinkPos == parentLinkPos + parentToChild
\param[in] YChildW is the link impulse to apply to the child link expressed in the world frame.
\param[in] jointDofImpulse is an optional of array joint impulses ({Q}) to apply to each dof of the inbound joint of the child link.
\param[in] jointDofISInvStISW is (I * s) / (s^T * I * s) with one entry for each dof of the child link's inbound joint.
\param[in] jointDofMotionMatrixW is the motion matrix s with one entry for each dof of the child link's inbound joint.
\param[in] dofCount is the number of dofs of the child link's incoming joint.
\param[in,out] jointDofQMinusStY accumulates [Q - s^T * YChildW] for each dof of the child link's inbound joint.
\note jointDofQMinusStY may be NULL if there is no need to accumulate [Q - s^T * YChildW] for each dof of the child link's inbound joint.
\note jointDofImpulse may be NULL if the intention is that zero joint impulse should be propagated.
\note jointDofImpulse, jointDofISInvStISW, jointDofMotionMatrixW and jointDofQMinusStY have dofCount entries ie one entry for each dof of the joint.
\return The propagated spatial impulse in the world frame.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF propagateImpulseW
(const PxVec3& parentToChild,
const Cm::SpatialVectorF& YChildW,
const PxReal* jointDofImpulse, const Cm::SpatialVectorF* jointDofISInvStISW, const Cm::UnAlignedSpatialVector* jointDofMotionMatrixW, const PxU8 dofCount,
PxReal* jointDofQMinusStY)
{
//See Mirtich Figure 5.7 page 141
//Mirtich equivalent after accounting for joint impulse:
// childToParentTranform{ Y + (I * s) * (Q - s^T* Y)]/ (s^T * I * s) }
Cm::SpatialVectorF YParentW(PxVec3(0, 0, 0), PxVec3(0, 0, 0));
for (PxU8 ind = 0; ind < dofCount; ++ind)
{
//(Q - s^T* Y)
const Cm::UnAlignedSpatialVector& sa = jointDofMotionMatrixW[ind];
const PxReal Q = jointDofImpulse ? jointDofImpulse[ind] : 0.0f;
const PxReal QMinusStY = Q - (sa.innerProduct(YChildW));
PX_ASSERT(PxIsFinite(QMinusStY));
//(I * s) * (Q - s^T* Y)]/ (s^T * I * s)
YParentW += jointDofISInvStISW[ind] * QMinusStY;
//Accumulate (Q - s^T * Y)
PX_ASSERT(!jointDofQMinusStY || PxIsFinite(jointDofQMinusStY[ind]));
if(jointDofQMinusStY)
jointDofQMinusStY[ind] += QMinusStY;
}
//Y + [(I * s) * (Q - s^T* Y)]/ (s^T * I * s)]
YParentW += YChildW;
//parent space's spatial zero acceleration impulse
//parentToChild satisfies parent = child + parentToChild.
return translateSpatialVector(parentToChild, YParentW);
}
/**
/brief Propagate an acceleration (or velocity) from a parent link to a child link.
This function exploits existing knowledge of Q_i - s_i^T * Z_i^A. If this is not known it is recommended to use
propagateVelocityW().
\param[in] parentToChild is the vector from parent link to child link such that childLinkPos == parentLinkPos + parentToChild
\param[in] parentLinkAccelerationW is the parent link acceleration (or velocity) expressed in the world frame.
\param[in] invStISW is the Mirtich equivalent of 1/[S_i^T * I_i^A * S_i]
\param[in] motionMatrixW is the Mirth equivalent of S_i of the child link's inbound joint.
\param[in] IsW is the Mirtich equvialent of I_i^A * S_i (== S_i^T * I_i^A)
\param[in] QMinusSTZ is the equivalent of Q_i - S_i^T * ZA_i in Mirtich notiation with
Q_i the joint force (or impulse) of the child link's inbound joint and ZA_i the child link
zero acceleration force (or impulse)
\param[in] dofCount is the number of dofs on the child links' inbound joint.
\param[out] jointAcceleration (or joint velocity) is incremented with the change arising
from the propagated acceleration (or velocity).
\note jointAcceleration (or velocity) may be NULL.
\return The spatial acceleration (or velocity) of the child link.
\note See Mirtich p121 and equations for propagating forces/applying accelerations and
p141 for propagating velocities/applying impulses.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF propagateAccelerationW(
const PxVec3& parentToChild, const Cm::SpatialVectorF& parentLinkAccelerationW,
const InvStIs& invStISW, const Cm::UnAlignedSpatialVector* motionMatrixW, const Cm::SpatialVectorF* IsW, const PxReal* QMinusSTZ, const PxU32 dofCount,
PxReal* jointAcceleration)
{
//parentToChild satisfies parent = child + parentToChild.
Cm::SpatialVectorF motionAccelerationW = translateSpatialVector(-parentToChild, parentLinkAccelerationW); //parent velocity change
//[Q_i - (s^T * Z_i^A + I_i^A * c_i)] - s^T * I_i^A * translated(vParent)
//Alternatively we compute I_i^A * s == s^T * I_i^A
//[Q_i - (s^T * vParent + I_i^A * c_i)] - I_i^A *s * translated(vParent)
PxReal tJAccel[3];
for (PxU32 ind = 0; ind < dofCount; ++ind)
{
const PxReal temp = IsW[ind].innerProduct(motionAccelerationW);
tJAccel[ind] = (QMinusSTZ[ind] - temp);
}
//calculate jointAcceleration
//qdot = [1 / s^T * I_i^A * s] *{ [Q_i - (s^T * vParent + I_i^A * c_i)] - I_i^A *s * translated(vParent) }
//linkVel = translated(vParent) + qdot * s_i
for (PxU32 ind = 0; ind < dofCount; ++ind)
{
PxReal jVel = 0.f;
for (PxU32 ind2 = 0; ind2 < dofCount; ++ind2)
{
jVel += invStISW.invStIs[ind2][ind] * tJAccel[ind2];
}
motionAccelerationW.top += motionMatrixW[ind].top * jVel;
motionAccelerationW.bottom += motionMatrixW[ind].bottom * jVel;
if(jointAcceleration)
jointAcceleration[ind] += jVel;
}
return motionAccelerationW;
}
/**
\brief Compute the equivalent of (J * M^-1 * J^T) for a mimic joint.
\param[in] rAA is the deltaQDot arising at joint A/dof A as a consequence of a unit joint impulse applied to joint A/dofA.
\param[in] rAB is the deltaQDot arising at joint A/dof A as a consequence of a unit joint impulse applied to joint B/dofB.
\param[in] rBB is the deltaQDot arising at joint B/dof B as a consequence of a unit joint impulse applied to joint B/dofB.
\param[in] rBA is the deltaQDot arising at joint B/dof B as a consequence of a unit joint impulse applied to joint A/dofA.
\param[in] gearRatio is the gear ratio of the mimic joint [qA + gearRatio * qB + offset = 0]
\note deltaQDotA = rAA * jointImpulseA + rAB * jointImpulseB
deltaQDotB = rBA * jointImpulseA + rBB * jointImpulseB
rAA, rAB, rBA, rBB play the role of an inverse mass matrix as follows:
M^-1 = [rAA rAB]
[rBA rBB]
\return (J * M^-1 * J^T) for a mimic joint.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal computeRecipMimicJointEffectiveInertia
(const PxReal rAA, const PxReal rAB, const PxReal rBB, const PxReal rBA, const PxReal gearRatio)
{
return (rAA + gearRatio*(rAB + rBA) + gearRatio*gearRatio*rBB);
}
/*
\brief Compute the impulses to apply to jointA/dofA and jointB dofB that
satisfy the requirements of a mimimic joint.
\param[in] biasCoefficient is the Baumarte constant.
\param[in] dt is the timestep of the simulation.
\param[in] invDt is the reciprocal of dt used to forward integrate the joint positions.
\param[in] qA is the position of jointA/dofA
\param[in] qB is the position of jointB/dofB
\param[in] qADot is speed of jointA/dofA
\param[in] qBDot is speed of jointB/dofB
\param[in] gearRatio is a constant of the mimic joint constraint: qA + gearRatio*qB + offset = 0
\param[in] offset is a constant of the mimic joint constraint: qA + gearRatio*qB + offset = 0
\param[in] naturalFrequency is the oscillation frequency of the mimic joint's compliance (s^-1)
\param[in] dampingRatio is the damping ratio of the mimic joint's compliance.
\param[in] r is a constant derived from the mass matrix of the mimic joint.
\param[in] isVelocityIteration is true if we are computing the impulse during a velocity iteration
and false if we are computing the impulse during a position iteration.
\param[out] jointImpA is the joint impulse to apply to jointA/dofA to enforce the mimic constraint.
\param[out] jointImpB is the joint impulse to apply to jointB/dofB to enforce the mimic constraint.
\note r has value [J * M^-1 * J^T] with J the jacobian [1, beta] and
M^-1 = 2x2 mass matrix describing deltaQDot effect of unit impulse applied to jointA/dofA and jointB/dofB
\note The impulses are computed with a zero constraint bias during velocity iterations.
\note r is pre-computed with computeRecipMimicJointEffectiveInertia()
\note If naturalFrequency < 0 the mimic joint is treated as a hard constraint with no compliance.
\note If dampingRatio < 0 the mimic joint is treated as a hard constraint with no compliance.
\note biasCoefficient is ignored if the mimic joint is compliant (when naturalFrequency > 0 and dampingRatio > 0).
When this is the case, biasCoefficient is replaced with a value that reflects the compliance of the mimic joint.
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeMimicJointImpulses
(const PxReal biasCoefficient, const PxReal dt, const PxReal invDt,
const PxReal qA, const PxReal qB, const PxReal qADot, const PxReal qBDot,
const PxReal gearRatio, const PxReal offset,
const PxReal naturalFrequency, const PxReal dampingRatio,
const PxReal r,
const bool isVelocityIteration,
PxReal& jointImpA, PxReal& jointImpB)
{
//For velocity iterations we have erp = 0.
//But erp = dt*kp/(dt*kp + kd). If it is 0
//then kd >> dt*kp. If this is true then
//cfm = 1/[dt*(dt*kp + kd)] = 1/(dt*kd) = 0.
PxReal erp = 0.0f;
PxReal cfm = 0.0f;
if(naturalFrequency <= 0 || dampingRatio <= 0 || isVelocityIteration)
{
erp = isVelocityIteration ? 0.0f : biasCoefficient;
cfm = 0.0f;
}
else
{
//Note:
//cfm has units velocity per unit impulse = kg^-1 for linear constraints
//erp is dimensionless
//Governing equation is:
//Jv + (r + cfm)*lambda + (erp/dt)* C = 0
//with
//C = qA + G*qB + offset
//r = rAA + G*(rAB + rBA) + rBB
//and dt the timestep
//Solve for lambda:
//lambda = -(1/(cfm + r)] * (erp*C/dt + Jv)
// = -[1/(1 + r/cfm)] * [ C*(erp/(dt*cfm)) + v0*(1/cfm) ]
//with v0 = Jv
//Now compare with spring
//lambda = -[1/(1 + r*dt*(dt*kp + kd)][ C*dt*kp + v0*dt*(dt*kp + kd) ]
//Solve for cfm by comparing terms.
//1/cfm = dt*(dt*kp + kd)
//cfm = 1/[dt*(dt*kp + kd)]
//Solve for erp by comparing terms.
//erp/(dt*cfm) = dt*kp
//erp = dt*dt*kp*cfm = dt*kp/(dt*kp + kd)
//Summarise results for cfm, erp.
// cfm = 1/[dt*(dt*kp + kd)]
// erp = dt*kp/(dt*kp + kd)
//Now express cfm and erp in terms of natural frequency mu and damping ratio zeta
//Start with computing kp from mu.
//Remember that r plays role of reciprocal mass
//kp = mu^2 / r
//kd = 2 * (mu / r) * zeta
//Summarise:
//Given mu and zeta and r we have
//kp = mu^2 / r
//kd = 2 * (mu / r) * zeta
//cfm = 1/[dt*(dt*kp + kd)]
//erp = dt*kp/(dt*kp + kd)
//lambda = -(1/(cfm + r)] * (erp*C/dt + Jv)
//Compute cfm, erp from mu, zeta, r.
const PxReal mu = naturalFrequency;
const PxReal zeta = dampingRatio;
const PxReal kp = mu * mu / r;
const PxReal kd = 2.0f * mu * zeta / r;
cfm = 1.0f / (dt * (dt * kp + kd));
erp = (dt * kp) / (dt * kp + kd);
}
//Comute lambda
//lambda = -[ (erp * C/ dt) + Jv ] / [r + cfm]
//C = qA + gearRatio* qB + offset
//b = erp*C/dt
//JV = qADot + gearRatio*qBDot
const PxReal C = qA + gearRatio * qB + offset;
const PxReal b = erp * C * invDt;
const PxReal Jv = qADot + gearRatio * qBDot;
const PxReal effectiveInertia = 1.0f / (r + cfm);
const PxReal lambda = -(b + Jv) * effectiveInertia;
jointImpA = lambda;
jointImpB = gearRatio * lambda;
}
struct TendonImplicitSpringParams
{
PX_CUDA_CALLABLE PX_FORCE_INLINE TendonImplicitSpringParams(const PxReal biasCoef, const PxReal velMult, const PxReal impMult, const PxReal limitBiasCoef, const PxReal limitIMpMult)
: biasCoefficient(biasCoef),
velMultiplier(velMult),
impulseMultiplier(impMult),
limitBiasCoefficient(limitBiasCoef),
limitImpulseMultiplier(limitIMpMult)
{
}
PX_CUDA_CALLABLE PX_FORCE_INLINE TendonImplicitSpringParams()
: biasCoefficient(0.0f), velMultiplier(0.0f), impulseMultiplier(0.0f), limitBiasCoefficient(0.0f), limitImpulseMultiplier(0.0f)
{
}
PxReal biasCoefficient;
PxReal velMultiplier;
PxReal impulseMultiplier;
PxReal limitBiasCoefficient;
PxReal limitImpulseMultiplier;
};
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeSpringParams
(const PxReal stepDt, const bool isTGSSolver,
const PxReal unitResponse,
const PxReal stiffness, const PxReal damping,
PxReal& biasCoefficient, PxReal& velMultiplier, PxReal& impulseMultiplier)
{
const PxReal a = stepDt * (stepDt*stiffness + damping);
const PxReal x = unitResponse > 0.f ? 1.0f / (1.0f + a * unitResponse) : 0.f;
biasCoefficient = (-stiffness * x * stepDt);
velMultiplier = -x * a;
impulseMultiplier = isTGSSolver ? 1.f : 1.f - x;
}
/**
\brief Compute the parameters for implicit tendon springs (1 spring for rest length, 1 spring for limit).
\param[in] dt is the timestep that will be used to forward integrate the spring position bias.
\param[in] isTGSSolver is true if TGS solver is active, false if PGS solver is active.
\param[in] unitResponse is the multiplier that converts impulse to velocity change.
\param[in] stiffness is the stiffness of the rest length spring (force per unit position bias)
\param[in] damping is the damping of the rest length spring (force per unit velocity bias)
\param[in] limitStiffness is the stiffness of the limit spring (force per unit position bias)
\return The params used to compute the impulse arising from current tendon state.
\see computeTendonImpulse
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE TendonImplicitSpringParams computeTendonSpringParams
(const PxReal dt, const bool isTGSSolver,
const PxReal unitResponse,
const PxReal stiffness, const PxReal damping,
const PxReal limitStiffness)
{
TendonImplicitSpringParams params;
computeSpringParams(
dt, isTGSSolver,
unitResponse,
stiffness, damping,
params.biasCoefficient, params.velMultiplier, params.impulseMultiplier);
PxReal limitVelMultiplier = 0.0f;
computeSpringParams(
dt, isTGSSolver,
unitResponse,
limitStiffness, damping,
params.limitBiasCoefficient, limitVelMultiplier, params.limitImpulseMultiplier);
return params;
}
/**
\brief Compute the tendon impulse for the rest length spring and for the limit spring and compute the change in total impulse.
\param[in] recipCoefficient is a multiplier used to scale (restLengthError * biasCoefficient + tendonSpeed * velMultiplier)
\param[in] tendonImplicitSpringParams are the params for the implicit springs of the tendon.
\param[in] restLengthError is the bias of the rest length spring
\param[in] tendonSpeed is the speed of the tendon
\param[in] limitError is the bias of the limit spring
\param[in] accumulatedRestLengthImpulse is the accumualted impulse applied to the rest length spring
\param[in] accumulatedLimitImpulse is the accumualted impulse applied to the limit spring
\param[out] restLengthImpulse is the new impulse to apply to the rest length spring
\param[out] limitImpulse is the new impulse to apply to the limit spring
\param[out] deltaF is the change in total impulse
\see computeTendonSpringParams
\note restLengthImpulse = ((restLengthError*biasCoefficient*) + (tendonSpeed*velMultiplier))*recipCoefficient + accumulatedRestLengthImpulse*impulseMultiplier
\note accumulatedLimitImpulse = ((limitError*limitBiasCoefficient*)*recipCoefficient + accumulatedLimitImpulse*limitImpulseMultiplier
*/
PX_CUDA_CALLABLE PX_FORCE_INLINE void computeTendonImpulse
(const PxReal recipCoefficient,
const TendonImplicitSpringParams& tendonImplicitSpringParams,
const PxReal restLengthError, const PxReal tendonSpeed, const PxReal limitError,
const PxReal accumulatedRestLengthImpulse, const PxReal accumulatedLimitImpulse,
PxReal& restLengthImpulse, PxReal& limitImpulse, PxReal& deltaF)
{
restLengthImpulse =
(restLengthError * tendonImplicitSpringParams.biasCoefficient + tendonSpeed * tendonImplicitSpringParams.velMultiplier) * recipCoefficient
+ accumulatedRestLengthImpulse * tendonImplicitSpringParams.impulseMultiplier;
limitImpulse =
limitError * tendonImplicitSpringParams.limitBiasCoefficient * recipCoefficient
+ accumulatedLimitImpulse * tendonImplicitSpringParams.limitImpulseMultiplier;
deltaF = (restLengthImpulse - accumulatedRestLengthImpulse) + (limitImpulse - accumulatedLimitImpulse);
}
/**
\brief The default constraint resolution order is as follows:
dynamic 1d constraints + dynamic contact constraints
spatial tendons + fixed tendons + mimic joints
friction (per dof)
drive (per dof)
pos limit (per dof)
vel limit (per dof)
static 1d constraints + static contact constraints
For grabbing scenarios, the following constraint resolution order can bring behaviour advantages:
spatial tendons + fixed tendons + mimic joints
friction (per dof)
drive (per dof)
pos limit (per dof)
dynamic 1d constraints + dynamic contact constraints
static 1d constraints + static contact constraints
vel limit (per dof)
The default constraint order requires a single pass through the articulations
after completing dynamic contact.
The grabbing constraint order requires two passes through the articulations,
one before dynamic contact and one after.
ArticulationConstraintProcessingConfig controls the work that will be performed on articulations
during each pass (either one pass or two passes) through the articulations.
*/
class ArticulationConstraintProcessingConfig
{
public:
struct VelLimit
{
enum Enum
{
eNONE,
eBEFORE_STATIC_CONSTRAINTS,
eAFTER_STATIC_CONSTRAINTS
};
};
protected:
ArticulationConstraintProcessingConfig(
bool doSpatialTendonsFixedTendonsMimicJoints,
bool doFrictionDrivePosLimit,
VelLimit::Enum doVelLimit,
bool doStaticContactsAnd1dConstraints)
: mDoSpatialTendonsFixedTendonsMimicJoints(doSpatialTendonsFixedTendonsMimicJoints),
mDoFrictionDrivePosLimit(doFrictionDrivePosLimit),
mDoVelLimit(doVelLimit),
mDoStaticContactsAnd1dConstraints(doStaticContactsAnd1dConstraints){}
public:
bool mDoSpatialTendonsFixedTendonsMimicJoints;
bool mDoFrictionDrivePosLimit;
VelLimit::Enum mDoVelLimit;
bool mDoStaticContactsAnd1dConstraints;
};
class ArticulationConstraintProcessingConfigCPU : public ArticulationConstraintProcessingConfig
{
private:
ArticulationConstraintProcessingConfigCPU(
bool doSpatialTendonsFixedTendonsMimicJoints,
bool doFrictionDrivePosLimit,
ArticulationConstraintProcessingConfig::VelLimit::Enum doVelLimit,
bool doStaticContactsAnd1dConstraints)
: ArticulationConstraintProcessingConfig(doSpatialTendonsFixedTendonsMimicJoints, doFrictionDrivePosLimit, doVelLimit, doStaticContactsAnd1dConstraints)
{}
public:
//There are 3 distinct regimes on CPU:
//1) default contact order
//2) swapped contact order with no dynamic contact
//3) swapped contact order with dynamic contact
//1) sequence with default contact order:
//note that default contact order processes everything in a single pass.
//*dynamic contact*
//spatial tendons + fixed tendons + mimic joints
//friction + drive + pos limit
//vel limit
//static contact/1dconstraint
//2) sequence with swapped contact order and no dynamic contact
//note that in absence of dynamic contact we can process everything in a single pass
//spatial tendons + fixed tendons + mimic joints
//friction + drive + pos limit
//static contact/1dconstraint
//vel limit
//3) sequence with swapped contact order and dynamic contact
//Note that inclusion of dynamic contact requires 2 passes through the tree.
//spatial tendons + fixed tendons + mimic joints (1st pass)
//friction + drive + pos limit (1st pass)
//*dynamic contact*
//static contact/1d constraint (2nd pass)
//vel limit (2nd pass)
//Case 1) with false==solveArticulationContactLast and Case 2) with true==solveArticulationContactLast
static PX_FORCE_INLINE ArticulationConstraintProcessingConfigCPU getSinglePassConfig(const bool solveArticulationContactLast)
{
if(!solveArticulationContactLast)
return ArticulationConstraintProcessingConfigCPU(true, true, VelLimit::eBEFORE_STATIC_CONSTRAINTS, true);
else
return ArticulationConstraintProcessingConfigCPU(true, true, VelLimit::eAFTER_STATIC_CONSTRAINTS, true);
}
//Case 3)
static PX_FORCE_INLINE ArticulationConstraintProcessingConfigCPU getFirstPassConfig()
{
return ArticulationConstraintProcessingConfigCPU(true, true, VelLimit::eNONE, false);
}
//Case 3)
static PX_FORCE_INLINE ArticulationConstraintProcessingConfigCPU getSecondPassConfig()
{
return ArticulationConstraintProcessingConfigCPU(false, false, VelLimit::eAFTER_STATIC_CONSTRAINTS, true);
}
};
struct ArticulationConstraintProcessingConfigGPU : public ArticulationConstraintProcessingConfig
{
private:
ArticulationConstraintProcessingConfigGPU(
bool propagateRigidBodyImpulsesAndSolveSelfConstraints,
bool doSpatialTendonsFixedTendonsMimicJoints,
bool doFrictionDrivePosLimit,
ArticulationConstraintProcessingConfig::VelLimit::Enum doVelLimit,
bool doStaticContactsAnd1dConstraints)
: ArticulationConstraintProcessingConfig(doSpatialTendonsFixedTendonsMimicJoints, doFrictionDrivePosLimit, doVelLimit, doStaticContactsAnd1dConstraints),
mPropagateRigidBodyImpulsesAndSolveSelfConstraints(propagateRigidBodyImpulsesAndSolveSelfConstraints)
{}
public:
//There are 2 distinct regimes on GPU because we do not have a specific path in the absence of dynamic contact.
//1) default contact order
//2) swapped contact order with/without dynamic contact
//1) sequence with default contact order:
//note that default contact order processes everything in a single pass.
//*dynamic contact*
//propagate impulses after dynamic contact
//spatial tendons + fixed tendons + mimic joints
//friction + drive + pos limit
//vel limit
//static contact/1dconstraint
//2) sequence with swapped contact order and with/without dynamic contact
//spatial tendons + fixed tendons + mimic joints (1st pass)
//friction + drive + pos limit (1st pass)
//*dynamic contact*
//propagate impulses after dynamic contact (2nd pass)
//static contact/1d constraint (2nd pass)
//vel limit (2nd pass)
//Case 1)
static PX_FORCE_INLINE ArticulationConstraintProcessingConfigGPU getSinglePassConfig()
{
ArticulationConstraintProcessingConfigGPU items(true, true, true, VelLimit::eBEFORE_STATIC_CONSTRAINTS, true);
return items;
}
//Case 2)
static PX_FORCE_INLINE ArticulationConstraintProcessingConfigGPU getFirstPassConfig()
{
ArticulationConstraintProcessingConfigGPU items(false, true, true, VelLimit::eNONE, false);
return items;
}
//Case 2)
static PX_FORCE_INLINE ArticulationConstraintProcessingConfigGPU getSecondPassConfig()
{
ArticulationConstraintProcessingConfigGPU items(true, false, false, VelLimit::eAFTER_STATIC_CONSTRAINTS, true);
return items;
}
bool mPropagateRigidBodyImpulsesAndSolveSelfConstraints;
};
} //namespace Dy
} //namespace physx
#endif //DY_ARTICULATION_CPUGPU_H