RigidBodyCoupling (Artisynth API documentation)

java.lang.Object
- maspack.spatialmotion.RigidBodyCoupling

Direct Known Subclasses:

CustomSphericalCoupling, FullPlanarCoupling, ParameterizedCoupling, PlanarCoupling, RevoluteCoupling, RollPitchCoupling, SegmentedPlanarCoupling, SlottedRevoluteCoupling, SolidCoupling, SphericalCoupling
```
public abstract class RigidBodyCoupling
extends java.lang.Object
```
Enforces constraints between two rigid bodies A and B, or between one rigid body A and ground. In the latter case, B is equated with the (fixed) world frame.
The coupling has its own coordinate frame D, which is anchored to B via a fixed transform TDB. Body A is associated with an operation frame C, described with respect to A by the fixed transform TCA. Enforcement of the constraint involves trying to keep C on a constraint surface whose location is constant with respect to D. The constraint frame G is the frame on the constraint surface which is nearest to C. Wrenches to enforce the constraint are defined (initially) with repsect to G and then transformed into the body frames A and B. Ideally, C and G coincide; otherwise, the transform TCG defines the constraint error and a linearization of this, projected onto the constraint wrenches in G, gives the linearized distance associated with each constraint.

Field Summary

Fields
Modifier and Type	Field and Description
`static int`	`BILATERAL` Constraint is bilateral.
`static int`	`LINEAR` Constraint is linear.
`static int`	`ROTARY` Constraint is rotary.
`static boolean`	`useNewDerivatives`

Method Summary

All Methods Instance Methods Abstract Methods Concrete Methods
Modifier and Type	Method and Description
`double`	`findNearestAngle(double ref, double ang)` Given an angle `ang`, find an equivalent angle that is within +/- PI of a given reference angle `ref`.
`void`	`getAuxState(DataBuffer data)`
`int`	`getBilateralConstraints(java.util.ArrayList<RigidBodyConstraint> bilaterals)`
`Wrench`	`getBilateralForceG()`
`int`	`getBilateralImpulses(VectorNd lam, int idx)`
`double`	`getBreakAccel()` Returns the minimum acceleration normal to the constraint surface required to disengage a unilateral constraint.
`double`	`getBreakSpeed()` Returns the minimum speed normal to the constraint surface required to disengage a unilateral constraint.
`VectorNd`	`getCompliance()` Returns the compliances for all this coupling's constraint directions.
`RigidBodyConstraint`	`getConstraint(int idx)`
`VectorNi`	`getConstraintInfo()` Returns info for all this coupling's constraint directions.
`abstract void`	`getConstraintInfo(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info, RigidTransform3d TGD, RigidTransform3d TCD, RigidTransform3d TERR, boolean setEngaged)` Computes the constraint frame G and the associated constraint information.
`double`	`getContactDistance()` Returns the distance to the constraint surface required to engage a unilateral constraint.
`VectorNd`	`getDamping()` Returns the dampings for all this coupling's constraint directions.
`void`	`getInitialAuxState(DataBuffer newData, DataBuffer oldData)`
`double`	`getUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals, boolean setEngaged)`
`Wrench`	`getUnilateralForceG()`
`int`	`getUnilateralImpulses(VectorNd the, int idx)`
`abstract void`	`initializeConstraintInfo(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info)`
`int`	`maxConstraints()`
`abstract int`	`maxUnilaterals()` Returns the maximum number of unilateral constraints associated with this coupling.
`abstract int`	`numBilaterals()` Returns the number of bilateral constraints associated with this coupling.
`int`	`numUnilaterals()` Returns the number of currently engaged unilateral constraints.
`void`	`printConstraintInfo()` For debugging only
`abstract void`	`projectToConstraint(RigidTransform3d TGD, RigidTransform3d TCD)` Computes the frame G on the constraint surface which is closest to a given frame C.
`void`	`scaleDistance(double s)`
`void`	`setAuxState(DataBuffer data)`
`int`	`setBilateralImpulses(VectorNd lam, double h, int idx)`
`void`	`setBreakAccel(double a)` Sets the minimum acceleration normal to the constraint surface required to disengage a unilateral constraint.
`void`	`setBreakSpeed(double v)` Sets the minimum speed normal to the constraint surface required to disengage a unilateral constraint.
`void`	`setCompliance(VectorNd c)` Sets compliances for all this coupling's constraint directions.
`void`	`setContactDistance(double d)` Sets the distance to the constraint surface required to engage a unilateral constraint.
`void`	`setDamping(VectorNd c)` Sets dampings for all this coupling's constraint directions.
`void`	`setDistanceAndZeroDerivative(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo info, Twist err)`
`void`	`setDistancesAndZeroDerivatives(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info, int numc, Twist err)` Sets the `distance` fields within the `info` array to the dot product of `wrenchC` with `err`, and sets the `dotWrenchC` fields to 0.
`int`	`setUnilateralImpulses(VectorNd the, double h, int idx)`
`void`	`skipAuxState(DataBuffer data)`
`void`	`transformGeometry(GeometryTransformer gtr, RigidTransform3d TFW, RigidTransform3d TDW)` Transforms the geometry of this coupling, in response to an affine transform X applied in world coordinates.
`void`	`updateBodyStates(RigidTransform3d TCD, RigidTransform3d TGD, RigidTransform3d TERR, boolean setEngaged)`
`void`	`updateConstraintsFromC()`
`void`	`updateUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals, int offset, int numc)`
`void`	`zeroImpulses()`

Methods inherited from class java.lang.Object
equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

- Field Detail
  - useNewDerivatives
```
public static boolean useNewDerivatives
```
  - BILATERAL
```
public static final int BILATERAL
```
    Constraint is bilateral.
    
    See Also:
    
    Constant Field Values
  - LINEAR
```
public static final int LINEAR
```
    Constraint is linear.
    
    See Also:
    
    Constant Field Values
  - ROTARY
```
public static final int ROTARY
```
    Constraint is rotary.
    
    See Also:
    
    Constant Field Values
- Method Detail
  - setContactDistance
```
public void setContactDistance(double d)
```
    Sets the distance to the constraint surface required to engage a unilateral constraint. The default value is 0. A negative value will cause the constraint surface to be pentrated before the unilateral constraint is engaged.
    
    Parameters:
    
    d - contact distance for unilateral constraints
  - getContactDistance
```
public double getContactDistance()
```
    Returns the distance to the constraint surface required to engage a unilateral constraint.
    
    Returns:
    
    contact distance for unilateral constraints
  - setBreakSpeed
```
public void setBreakSpeed(double v)
```
    Sets the minimum speed normal to the constraint surface required to disengage a unilateral constraint. This feature is used to prevent contact chattering. The default value is -infinity, implying that the feature is disabled.
    
    Parameters:
    
    v - minimum normal speed for breaking unilateral constraints
  - getBreakSpeed
```
public double getBreakSpeed()
```
    Returns the minimum speed normal to the constraint surface required to disengage a unilateral constraint.
    
    Returns:
    
    minimum normal speed for breaking unilateral constraints
  - setBreakAccel
```
public void setBreakAccel(double a)
```
    Sets the minimum acceleration normal to the constraint surface required to disengage a unilateral constraint. This feature is used to prevent contact chattering. The default value is -infinity, implying that the feature is disabled.
    
    Parameters:
    
    a - minimum normal acceleration for breaking unilateral constraints
  - getBreakAccel
```
public double getBreakAccel()
```
    Returns the minimum acceleration normal to the constraint surface required to disengage a unilateral constraint.
    
    Returns:
    
    minimum normal acceleration for breaking unilateral constraints
  - getCompliance
```
public VectorNd getCompliance()
```
    Returns the compliances for all this coupling's constraint directions. The default values are all zero.
    
    Returns:
    
    compliances for this coupling
  - setCompliance
```
public void setCompliance(VectorNd c)
```
    Sets compliances for all this coupling's constraint directions.
    
    Parameters:
    
    c - new compliance values
  - getDamping
```
public VectorNd getDamping()
```
    Returns the dampings for all this coupling's constraint directions. The default values are all zero.
    
    Returns:
    
    dampings for this coupling
  - setDamping
```
public void setDamping(VectorNd c)
```
    Sets dampings for all this coupling's constraint directions.
    
    Parameters:
    
    c - new damping values
  - getConstraintInfo
```
public VectorNi getConstraintInfo()
```
    Returns info for all this coupling's constraint directions. Values are given as settings of the flags BILATERAL, LINEAR, and ROTARY.
    
    Returns:
    
    dampings for this coupling
  - printConstraintInfo
```
public void printConstraintInfo()
```
    For debugging only
  - getBilateralForceG
```
public Wrench getBilateralForceG()
```
  - getUnilateralForceG
```
public Wrench getUnilateralForceG()
```
  - updateConstraintsFromC
```
public void updateConstraintsFromC()
```
  - updateBodyStates
```
public void updateBodyStates(RigidTransform3d TCD,
                             RigidTransform3d TGD,
                             RigidTransform3d TERR,
                             boolean setEngaged)
```
  - getConstraint
```
public RigidBodyConstraint getConstraint(int idx)
```
  - getBilateralConstraints
```
public int getBilateralConstraints(java.util.ArrayList<RigidBodyConstraint> bilaterals)
```
  - setBilateralImpulses
```
public int setBilateralImpulses(VectorNd lam,
                                double h,
                                int idx)
```
  - zeroImpulses
```
public void zeroImpulses()
```
  - getBilateralImpulses
```
public int getBilateralImpulses(VectorNd lam,
                                int idx)
```
  - numUnilaterals
```
public int numUnilaterals()
```
    Returns the number of currently engaged unilateral constraints.
    
    Returns:
    
    number of engaged unilateral contraints
  - getUnilateralConstraints
```
public double getUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals,
                                       boolean setEngaged)
```
  - updateUnilateralConstraints
```
public void updateUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals,
                                        int offset,
                                        int numc)
```
  - setUnilateralImpulses
```
public int setUnilateralImpulses(VectorNd the,
                                 double h,
                                 int idx)
```
  - getUnilateralImpulses
```
public int getUnilateralImpulses(VectorNd the,
                                 int idx)
```
  - maxConstraints
```
public int maxConstraints()
```
  - projectToConstraint
```
public abstract void projectToConstraint(RigidTransform3d TGD,
                                         RigidTransform3d TCD)
```
    Computes the frame G on the constraint surface which is closest to a given frame C. The input consists of the transform from C to D.
    
    Parameters:
    
    TGD - returns the transform from G to D
    
    TCD - transform from frame C to D
  - numBilaterals
```
public abstract int numBilaterals()
```
    Returns the number of bilateral constraints associated with this coupling.
    
    Returns:
    
    number of bilateral constraints
  - maxUnilaterals
```
public abstract int maxUnilaterals()
```
    Returns the maximum number of unilateral constraints associated with this coupling.
    
    Returns:
    
    maximum number of unilateral constraints
  - getConstraintInfo
```
public abstract void getConstraintInfo(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info,
                                       RigidTransform3d TGD,
                                       RigidTransform3d TCD,
                                       RigidTransform3d TERR,
                                       boolean setEngaged)
```
    Computes the constraint frame G and the associated constraint information. G is determined by projecting C onto the constraint surface.
    Information for each constraint wrench is returned through an array of ConstraintInfo objects supplied by the argument info. This array should have a fixed number of elements equal to the number of bilateral constraints plus the maximum number of unilateral constraints. Bilateral constraints appear first, followed by the unilateral constraints. Constraint wrenches and their derivatives (with respect to frame G) are set within the fields wrenchC and dotWrenchC, repsectively. Distances to set within the distance; each of these should be the dot product of the wrench with the linearization of the constraint error TCG. For computing wrench derivatives, this method may use myVelBA, which gives the current velocity of B with repsect to A, in coordinate frame D. Information only needs to be returned for constraints which are potentially active, or engaged. Constraints which are engaged have their ConstraintInfo.engaged field set to a non-zero value. Bilateral constraints are always engaged, and their ConstraintInfo.engaged field is automatically set to 1 by the system. For unilateral constraints, the determination of whether or not the constraint is engaged, and the setting of the engaged field, should be done by this method if the argument setEngaged is true. Otherwise, if setEngaged is false, the method should take the engaged settings as given. Constraints which are engaged are those which are returned by the calls getBilateralConstraints or getUnilateralConstraints.
    
    Parameters:
    
    info - used to return information for each possible constraint wrenches
    
    TGD - returns the transform from G to D
    
    TCD - transform from operation frame C to D
    
    TERR - TODO
    
    setEngaged - if true, this method should determine if the constraint is engaged.
  - initializeConstraintInfo
```
public abstract void initializeConstraintInfo(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info)
```
  - transformGeometry
```
public void transformGeometry(GeometryTransformer gtr,
                              RigidTransform3d TFW,
                              RigidTransform3d TDW)
```
    Transforms the geometry of this coupling, in response to an affine transform X applied in world coordinates. In order to facilitate the application of this transform, this method also provides the current transforms from the C and D frames to world coordinates. It also provides the rotational component Ra of X.M, which should be determined from the left polar decomposition of X.M:
```
 X.M = P Ra
 
```
    Given these arguments, a point p of the coupling defined with respect to frame D would transform according to
```
 p' = TDW.R^T Ra^T X.M TDW.R p
 
```
    which is equivalent to transforming p to world coordinates (using TDW), applying X, and then transforming back to D using the modified TDW produced by applying X.
    Parameters:
    
    gtr - transformer implementing the transformation
    
    TFW - current transform from C to world
    
    TDW - current transform from D to world
  - scaleDistance
```
public void scaleDistance(double s)
```
  - skipAuxState
```
public void skipAuxState(DataBuffer data)
```
  - getAuxState
```
public void getAuxState(DataBuffer data)
```
  - setAuxState
```
public void setAuxState(DataBuffer data)
```
  - getInitialAuxState
```
public void getInitialAuxState(DataBuffer newData,
                               DataBuffer oldData)
```
  - setDistancesAndZeroDerivatives
```
public void setDistancesAndZeroDerivatives(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info,
                                           int numc,
                                           Twist err)
```
    Sets the distance fields within the info array to the dot product of wrenchC with err, and sets the dotWrenchC fields to 0.
  - setDistanceAndZeroDerivative
```
public void setDistanceAndZeroDerivative(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo info,
                                         Twist err)
```
  - findNearestAngle
```
public double findNearestAngle(double ref,
                               double ang)
```
    Given an angle ang, find an equivalent angle that is within +/- PI of a given reference angle ref.
    
    Parameters:
    
    ref - reference angle (radians)
    
    ang - initial angle (radians)
    
    Returns:
    
    angle equivalent to ang within +/- PI of ref.

Class RigidBodyCoupling

Field Summary

Method Summary

Methods inherited from class java.lang.Object

Field Detail

useNewDerivatives

BILATERAL

LINEAR

ROTARY

Method Detail

setContactDistance

getContactDistance

setBreakSpeed

getBreakSpeed

setBreakAccel

getBreakAccel

getCompliance

setCompliance

getDamping

setDamping

getConstraintInfo

printConstraintInfo

getBilateralForceG

getUnilateralForceG

updateConstraintsFromC

updateBodyStates

getConstraint

getBilateralConstraints

setBilateralImpulses

zeroImpulses

getBilateralImpulses

numUnilaterals

getUnilateralConstraints

updateUnilateralConstraints

setUnilateralImpulses

getUnilateralImpulses

maxConstraints

projectToConstraint

numBilaterals

maxUnilaterals

getConstraintInfo

initializeConstraintInfo

transformGeometry

scaleDistance

skipAuxState

getAuxState

setAuxState

getInitialAuxState

setDistancesAndZeroDerivatives

setDistanceAndZeroDerivative

findNearestAngle