maspack.spatialmotion

Class RigidBodyCoupling

• java.lang.Object
• maspack.spatialmotion.RigidBodyCoupling
• Method Summary

All Methods
Modifier and Type Method and Description
`double` ```clipCoordinate(int idx, double value)```
Clips `value` to the range for the `idx`-ith coordinate supported by this coupling.
`RigidBodyCoupling` `clone()`
`void` `coordinatesToTCD(RigidTransform3d TCD)`
Computes the TCD transform for the connector's current coordinate values, if coordinates are supported.
`abstract void` ```coordinatesToTCD(RigidTransform3d TCD, VectorNd coords)```
Computes the TCD transform for a set of coordinates, if coordinates are supported.
`double` ```findNearestAngle(double ref, double ang)```
Given an angle `ang`, find an equivalent angle that is within +/- PI of a given reference angle `ref`.
`int` `getBilateralConstraints(java.util.ArrayList<RigidBodyConstraint> bilaterals)`
Collects the bilateral constraints for this coupling by appending them to the list `bilterals`.
`Wrench` `getBilateralForceG()`
Returns the most recently computed bilateral constraint forces, expressed as a wrench in frame G.
`int` ```getBilateralForces(VectorNd lam, int idx)```
Gets the bilateral constraint forces (i.e., Lagrange multipliers) that have most recently set in this coupling.
`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)`
Returns the `idx`-th constraint that was added inside `initializeConstraintInfo()`.
`VectorNi` `getConstraintFlags()`
Returns flags for all this coupling's constraint directions.
`double` `getConstraintForce(int idx)`
Returns the constraint force currently acting on the `idx`-th constraint.
`double` ```getCoordinate(int idx, RigidTransform3d TGD)```
Returns the current value for the `idx`-ith coordinate supported by this coupling.
`DoubleInterval` `getCoordinateRange(int idx)`
Returns the range for the `idx`-ith coordinate supported by this coupling.
`void` ```getCoordinates(VectorNd coords, RigidTransform3d TGD)```
Returns (and possibly updates) the current coordinate values for this coupling, if coordinates are supported.
`VectorNd` `getDamping()`
Returns the dampings for all this coupling's constraint directions.
`double` `getLinearLimitTol()`
Returns the penetration tolerance for linear limit constraints.
`double` `getRotaryLimitTol()`
Returns the penetration tolerance for rotary limit constraints.
`void` `getState(DataBuffer data)`
Stores the state for this coupling.
`double` ```getUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals, boolean updateEngaged)```
Collects the engaged unilateral constraints for this coupling by appending them to the list `unilterals`.
`Wrench` `getUnilateralForceG()`
Returns the most recently computed unilateral constraint forces, expressed as a wrench in frame G.
`int` ```getUnilateralForces(VectorNd the, int idx)```
Gets the unilateral constraint forces (i.e., Lagrange multipliers) that have most recently been set for the currently engaged unilateral constraints in this coupling.
`abstract void` `initializeConstraints()`
Called inside the `RigidBodyCoupling` constructor to allocate constraint and coordinate information, using calls to the `addConstraint` and `addCoordinate` methods.
`static void` `main(java.lang.String[] args)`
`int` `numBilaterals()`
Returns the number of bilateral constraints associated with this coupling.
`int` `numConstraints()`
Returns the maximum number of constraints associated with this coupling.
`int` `numCoordinates()`
Returns the number of coordinates associated with this coupling.
`int` `numEngagedUnilaterals()`
Returns the number of currently engaged unilateral constraints.
`int` `numUnilaterals()`
Returns the number of unilateral constraints associated with this coupling, engaged or otherwise.
`void` `printConstraintInfo()`
For debugging only
`void` ```projectAndUpdateCoordinates(RigidTransform3d TGD, RigidTransform3d TCD)```
`abstract void` ```projectToConstraints(RigidTransform3d TGD, RigidTransform3d TCD, VectorNd coords)```
Project a transform TCD onto the nearest transform TGD that is legal given this coupling's bilateral constraints.
`void` `scaleDistance(double s)`
Scales the distance units of this constraint.
`int` ```setBilateralForces(VectorNd lam, double h, int idx)```
Sets the bilateral constraint forces (i.e., Lagrange multipliers) that have been computed for this coupling.
`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 this coupling's constraints.
`void` ```setCoordinateRange(int idx, DoubleInterval range)```
Sets the range for the `idx`-ith coordinate supported by this coupling.
`void` ```setCoordinateValue(int idx, double value, RigidTransform3d TGD)```
Sets the value for the `idx`-ith coordinate supported by this coupling.
`void` ```setCoordinateValues(VectorNd coords, RigidTransform3d TGD)```
Sets the coordinate values for this coupling, if coordinates are supported.
`void` `setDamping(VectorNd c)`
Sets dampings for this coupling's constraints.
`void` `setLinearLimitTol(double tol)`
Sets the penetration tolerance for linear limit constraints.
`void` `setRotaryLimitTol(double tol)`
Sets the penetration tolerance for rotary limit constraints.
`void` `setState(DataBuffer data)`
Loads the state for the coupling.
`int` ```setUnilateralForces(VectorNd the, double h, int idx)```
Sets the unilateral constraint forces (i.e., Lagrange multipliers) that have been computed for the currently engaged unilateral constraints in this coupling.
`void` ```TCDToCoordinates(VectorNd coords, RigidTransform3d TCD)```
If coordinates are supported by this coupling, compute their values based on the supplied transform TCD from frame C to D, and return the result in `coords`.
`void` ```transformGeometry(GeometryTransformer gtr, RigidTransform3d TCW, RigidTransform3d TDW)```
Transforms the geometry of this coupling.
`void` ```updateBodyStates(RigidTransform3d TCD, RigidTransform3d TGD, RigidTransform3d TERR, Twist velGD, boolean updateEngaged)```
`abstract void` ```updateConstraints(RigidTransform3d TGD, RigidTransform3d TCD, Twist errC, Twist velGD, boolean updateEngaged)```
`void` ```updateUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals, int idx, int numc)```
Called by `BodyConnector` in ArtiSynth core to update the unilateral constraints.
`void` `zeroForces()`
Zeros the constraint forces in the coupling.
• Methods inherited from class java.lang.Object

`equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait`
• Method Detail

• getLinearLimitTol

`public double getLinearLimitTol()`
Returns the penetration tolerance for linear limit constraints. See `setLinearLimitTol(double)`.
Returns:
penetration tolerance for linear limits
• setLinearLimitTol

`public void setLinearLimitTol(double tol)`
Sets the penetration tolerance for linear limit constraints. Although the coupling does not enforce these limits directly, this information may be used in deciding when to break (set enabled to 0) limit constraints.
Parameters:
`tol` - penetration tolerance for linear limits
• getRotaryLimitTol

`public double getRotaryLimitTol()`
Returns the penetration tolerance for rotary limit constraints. See `setRotaryLimitTol(double)`.
Returns:
penetration tolerance for rotary limits
• setRotaryLimitTol

`public void setRotaryLimitTol(double tol)`
Sets the penetration tolerance for rotary limit constraints. Although the coupling does not enforce these limits directly, this information may be used in deciding when to break (set enabled to 0) limit constraints.
Parameters:
`tol` - penetration tolerance for rotary limits
• setBreakSpeed

`public void setBreakSpeed(double v)`
Sets the minimum speed normal to the constraint surface required to disengage a unilateral constraint. The default value is 0.
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
• 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 this coupling's constraints.
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 this coupling's constraints.
Parameters:
`c` - new damping values
• printConstraintInfo

`public void printConstraintInfo()`
For debugging only
• getBilateralForceG

`public Wrench getBilateralForceG()`
Returns the most recently computed bilateral constraint forces, expressed as a wrench in frame G.
Returns:
current bilateral wrench in frame G
• getUnilateralForceG

`public Wrench getUnilateralForceG()`
Returns the most recently computed unilateral constraint forces, expressed as a wrench in frame G.
Returns:
current unilateral wrench in frame G
• getConstraintForce

`public double getConstraintForce(int idx)`
Returns the constraint force currently acting on the `idx`-th constraint.
Parameters:
`idx` - constraint index
Returns:
constraint force
• getBilateralConstraints

`public int getBilateralConstraints(java.util.ArrayList<RigidBodyConstraint> bilaterals)`
Collects the bilateral constraints for this coupling by appending them to the list `bilterals`. The collected constraints are used for the mechanical system solve.
Parameters:
`bilaterals` - collects the bilateral constraints
Returns:
number of bilateral constraints
• setBilateralForces

```public int setBilateralForces(VectorNd lam,
double h,
int idx)```
Sets the bilateral constraint forces (i.e., Lagrange multipliers) that have been computed for this coupling. These are supplied in `lam`, starting at location `idx`, as impulse values that should be scaled by `h` to obtain forces. The method should return `idx` incremented by the number of bilateral constraints.
Parameters:
`lam` - supplies the bilateral impulse values
`h` - time step value to scale impulses into forces
`idx` - starting location for impulses in `lam`
Returns:
updated value for idx
• zeroForces

`public void zeroForces()`
Zeros the constraint forces in the coupling.
• getBilateralForces

```public int getBilateralForces(VectorNd lam,
int idx)```
Gets the bilateral constraint forces (i.e., Lagrange multipliers) that have most recently set in this coupling. These are returned in `lam`, starting at location `idx`. The method should return `idx` incremented by the number of bilateral constraints.
Parameters:
`lam` - returns the bilateral forces
`idx` - starting location for impulses in `lam`
Returns:
updated value for idx
• numEngagedUnilaterals

`public int numEngagedUnilaterals()`
Returns the number of currently engaged unilateral constraints.
Returns:
number of engaged unilateral contraints
• getUnilateralConstraints

```public double getUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals,
boolean updateEngaged)```
Collects the engaged unilateral constraints for this coupling by appending them to the list `unilterals`. If `updatedEngaged` is `true`, the method will first check the engagement of each constraint and break it if certain conditions are met.
Parameters:
`unilaterals` - collects the unilateral constraints
`updateEngaged` - if `true`, update constraint engagement
Returns:
number of engaged unilateral constraints
• updateUnilateralConstraints

```public void updateUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals,
int idx,
int numc)```
Called by `BodyConnector` in ArtiSynth core to update the unilateral constraints. This is usually done before the velocity solve to account for changes resulting from the position correction step. The constraints to be updated are supplied by `unilaterals` starting at `idx`.

At present, no changes are made to the constraints and so this method does nothing. However, it does perform a sanity check to ensure that the constraints match the current settings in the coupling.

Parameters:
`unilaterals` - constraints to be updated
`idx` - starting index in `unilaterals`
`numc` - number of constraints to update
• setUnilateralForces

```public int setUnilateralForces(VectorNd the,
double h,
int idx)```
Sets the unilateral constraint forces (i.e., Lagrange multipliers) that have been computed for the currently engaged unilateral constraints in this coupling. These are supplied in `the`, starting at location `idx`, as impulse values that should be scaled by `h` to obtain forces. The method should return `idx` incremented by the number of currently engaged unilateral constraints.
Parameters:
`the` - supplies the unilateral impulse values
`h` - time step value to scale impulses into forces
`idx` - starting location for impulses in `the`
Returns:
updated value for idx
• getUnilateralForces

```public int getUnilateralForces(VectorNd the,
int idx)```
Gets the unilateral constraint forces (i.e., Lagrange multipliers) that have most recently been set for the currently engaged unilateral constraints in this coupling. These are returned in `the`, starting at location `idx`. The method should return `idx` incremented by the number of currently engaged unilateral constraints.
Parameters:
`the` - returns the unilateral forces
`idx` - starting location for impulses in `the`
Returns:
updated value for idx
• projectToConstraints

```public abstract void projectToConstraints(RigidTransform3d TGD,
RigidTransform3d TCD,
VectorNd coords)```
Project a transform TCD onto the nearest transform TGD that is legal given this coupling's bilateral constraints. In more detail, given a transform TCD from frame C to D, find the frame G that is nearest to C while lying on the constraint surface defined by the bilateral constraints. Subclasses should implement this method as needed.

Optionally, the coupling may also extend the projection to include unilateral constraints that are not associated with coordinate limits. In particular, this should be done for constraints for which is it desired to have the constraint error included in the `errC` argument that is passed to `updateConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.spatialmotion.Twist, maspack.spatialmotion.Twist, boolean)`.

If this coupling supports coordinates and `coords` is non-`null`, then the coordinate values corresponding to `TGD` should also be computed and returned in `coords`. The easiest way to do this is to simply call `TCDToCoordinates(maspack.matrix.VectorNd, maspack.matrix.RigidTransform3d)`, although in some cases it may be computationally cheaper to compute both the coordinates and the projection at the same time. The method should not clip the resulting coordinates to their range limits.

Parameters:
`TGD` - returns the nearest transform to `TCD` that is legal with respect to the bilateral (and possibly some unilateral) constraints
`TCD` - transform from frame C to D to be projected
`coords` - if non-`null`, should be used to return coordinate values
• projectAndUpdateCoordinates

```public void projectAndUpdateCoordinates(RigidTransform3d TGD,
RigidTransform3d TCD)```
• numBilaterals

`public int numBilaterals()`
Returns the number of bilateral constraints associated with this coupling.
Returns:
number of bilateral constraints
• numUnilaterals

`public int numUnilaterals()`
Returns the number of unilateral constraints associated with this coupling, engaged or otherwise.
Returns:
number of unilateral constraints
• updateConstraints

```public abstract void updateConstraints(RigidTransform3d TGD,
RigidTransform3d TCD,
Twist errC,
Twist velGD,
boolean updateEngaged)```
Called from `updateBodyStates(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.spatialmotion.Twist, boolean)` to update the constraints, usually once per simulation step. This method is responsible for:
• Updating the values of all non-constant constraint wrenches, along with their derivatives;
• If `updateEngaged` is `true`, updating the `engaged` and `distance` attributes for all unilateral constraints not associated with a joint limit.

Wrenches and their derivatives should be computed with respect to frame G, which is frame C with the constraint errors removed.

If the coupling supports coordinates, their values will be updated before this method is called so as to correspond to `TGD`.

Parameters:
`TGD` - idealized joint transform from frame G to D, obtained by calling `projectToConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.matrix.VectorNd)` on `TCD`
`TCD` - actual joint transform from frame C to D; included for legacy reasons and not normally used
`errC` - error transform from frame C to G, represented as a `Twist`
`velGD` - velocity of frame G with respect to D, as seen in frame G
`updateEngaged` - if `true`, requests the updating of unilateral `engaged` and `distance` attributes as describe above.
• initializeConstraints

`public abstract void initializeConstraints()`
Called inside the `RigidBodyCoupling` constructor to allocate constraint and coordinate information, using calls to the `addConstraint` and `addCoordinate` methods. The method may be called at other times if the constraints need to be reconfigured (such as when switching constraints between bilateral and unilateral). Subclasses should implement this method as needed.
• transformGeometry

```public void transformGeometry(GeometryTransformer gtr,
RigidTransform3d TCW,
RigidTransform3d TDW)```
Transforms the geometry of this coupling. Subclasses should reimplement this as needed.
Parameters:
`gtr` - transformer implementing the transformation
`TCW` - current transform from C to world
`TDW` - current transform from D to world
• scaleDistance

`public void scaleDistance(double s)`
Scales the distance units of this constraint. Subclasses should reimplement this as needed.
Parameters:
`s` - scale factor
• getState

`public void getState(DataBuffer data)`
Stores the state for this coupling. Used by the `BodyConnector` class in the ArtiSynth core code.
Parameters:
`data` - buffer in which to store state
• setState

`public void setState(DataBuffer data)`
Loads the state for the coupling. Used by the `BodyConnector` class in the ArtiSynth core code.
Parameters:
`data` - buffer from which state is loaded
• 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`.
• numCoordinates

`public int numCoordinates()`
Returns the number of coordinates associated with this coupling. If coordinates are not supported, this method returns 0.
Returns:
number of associated coordinates
• getConstraint

`public RigidBodyConstraint getConstraint(int idx)`
Returns the `idx`-th constraint that was added inside `initializeConstraintInfo()`.
Parameters:
`idx` - constraint index
Returns:
`idx`-th constraint
• getCoordinates

```public void getCoordinates(VectorNd coords,
RigidTransform3d TGD)```
Returns (and possibly updates) the current coordinate values for this coupling, if coordinates are supported. If `TGD` is non-`null`, it supplies the current value of the TCD transform, which is then projected to the constraint surface to form TGD and update the coordinate values, with TGD returned in `TCD`. Otherwise, if `TGD` is `null`, the method simply returns the currently stored coordinate values. If `numCoordinates()` returns 0, this method does nothing.
Parameters:
`coords` - returns the coordinates. Its size will be set to `numCoordinates()`.
`TGD` - if non-`null`, provides TCD on input and TGD on output.
• setCoordinateValues

```public void setCoordinateValues(VectorNd coords,
RigidTransform3d TGD)```
Sets the coordinate values for this coupling, if coordinates are supported. Otherwise this method does nothing.
Parameters:
`coords` - new coordinate values. Must have a length >= `numCoordinates()`.
`TGD` - if non-`null`, returns the corresponding TGD transform
• coordinatesToTCD

```public abstract void coordinatesToTCD(RigidTransform3d TCD,
VectorNd coords)```
Computes the TCD transform for a set of coordinates, if coordinates are supported. Otherwise this method does nothing. Subclasses should implement this method as needed.
Parameters:
`TCD` - returns the TCD transform
`coords` - supplies the coordinate values and must have a length >= `numCoordinates()`.
• coordinatesToTCD

`public void coordinatesToTCD(RigidTransform3d TCD)`
Computes the TCD transform for the connector's current coordinate values, if coordinates are supported. Otherwise `TCD` is set to the identity.
Parameters:
`TCD` - returns the TCD transform
• setCoordinateValue

```public void setCoordinateValue(int idx,
double value,
RigidTransform3d TGD)```
Sets the value for the `idx`-ith coordinate supported by this coupling. An exception will be generated if coordinates are not supported.

If `TGD` is non-`null`, it is assumed to contain a transform TCD that this method will first use to update the other coordinate values, after projecting it onto the nearest bilateral-legal transform TGD using `projectToConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.matrix.VectorNd)`. The specified coordinate value is then set, and `TGD` is recomputed from the updated coordinates.

Parameters:
`idx` - coordinate index
`value` - new coordinate value
`TGD` - if non-`null`, is used to update the other coordinate values, and then return the final TGD resulting from the new coordinates
• getCoordinate

```public double getCoordinate(int idx,
RigidTransform3d TGD)```
Returns the current value for the `idx`-ith coordinate supported by this coupling. An exception will be generated if coordinates are not supported.

If `TGD` is non-`null`, it is assumed to contain a transform TCD that this method will first use to update the coordinate values, after projecting it onto the nearest bilateral-legal transform TGD using `projectToConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.matrix.VectorNd)`. The projected value is returned in `TGD`.

Parameters:
`idx` - coordinate index
`TGD` - if non-`null`, is projected onto the contraints and used to update the coordinate values
Returns:
current value
• getCoordinateRange

`public DoubleInterval getCoordinateRange(int idx)`
Returns the range for the `idx`-ith coordinate supported by this coupling. An exception will be generated if coordinates are not supported.
Parameters:
`idx` - coordinate index
Returns:
range for the coordinate
• clipCoordinate

```public double clipCoordinate(int idx,
double value)```
Clips `value` to the range for the `idx`-ith coordinate supported by this coupling. An exception will be generated if coordinates are not supported.
Parameters:
`idx` - idx coordinate index
`value` - value to be clipped
Returns:
clipped value
• setCoordinateRange

```public void setCoordinateRange(int idx,
DoubleInterval range)```
Sets the range for the `idx`-ith coordinate supported by this coupling. An exception will be generated if coordinates are not supported.
Parameters:
`idx` - coordinate index
`range` - range for the coordinate
• clone

`public RigidBodyCoupling clone()`
Overrides:
`clone` in class `java.lang.Object`
• main

`public static void main(java.lang.String[] args)`