public abstract class RigidBodyCoupling
extends java.lang.Object
implements java.lang.Cloneable
BodyConnector
in the
core ArtiSynth code.
Couplings for specific types of constraints should subclass this
base class and implement the abstract 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.

static 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)
Updates the current constraint information.

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. 
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.

public double getLinearLimitTol()
setLinearLimitTol(double)
.public void setLinearLimitTol(double tol)
tol
 penetration tolerance for linear limitspublic double getRotaryLimitTol()
setRotaryLimitTol(double)
.public void setRotaryLimitTol(double tol)
tol
 penetration tolerance for rotary limitspublic void setBreakSpeed(double v)
v
 minimum normal speed for breaking unilateral constraintspublic double getBreakSpeed()
public VectorNd getCompliance()
public void setCompliance(VectorNd c)
c
 new compliance valuespublic VectorNd getDamping()
public void setDamping(VectorNd c)
c
 new damping valuespublic VectorNi getConstraintFlags()
RigidBodyConstraint.BILATERAL
,
RigidBodyConstraint.LINEAR
,
RigidBodyConstraint.ROTARY
, and
RigidBodyConstraint.CONSTANT
.public void printConstraintInfo()
public Wrench getBilateralForceG()
public Wrench getUnilateralForceG()
public void updateBodyStates(RigidTransform3d TCD, RigidTransform3d TGD, RigidTransform3d TERR, Twist velGD, boolean updateEngaged)
BodyConnector
in the ArtiSynth code after the position state of the
mechanical system has changed. It calls the couplingspecific
method updateConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.spatialmotion.Twist, maspack.spatialmotion.Twist, boolean)
and also performs
the following default calculations:
updateConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.spatialmotion.Twist, maspack.spatialmotion.Twist, boolean)
, it updates the engaged
and distance
attributes for all unilateral constraints
associated with coordinate value limits, if updateEngaged
is
true
.
updateConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.spatialmotion.Twist, maspack.spatialmotion.Twist, boolean)
, it computes the
distance
attribute for all bilateral constraints.
TCD
, TGD
, velCD
and updateEngaged
are passed through to updateConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.spatialmotion.Twist, maspack.spatialmotion.Twist, boolean)
, along
with TERR
represented as a Twist
.TCD
 transform from frame C to DTGD
 transform from frame G to DTERR
 error transform from frame C to GvelGD
 velocity of frame G wuth respect to D, as seen in frame GupdateEngaged
 if true
, update engaged
for the
unilateral constraintspublic double getConstraintForce(int idx)
idx
th
constraint.idx
 constraint indexpublic int getBilateralConstraints(java.util.ArrayList<RigidBodyConstraint> bilaterals)
bilterals
. The collected constraints are used for the
mechanical system solve.bilaterals
 collects the bilateral constraintspublic int setBilateralForces(VectorNd lam, double h, int idx)
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.lam
 supplies the bilateral impulse valuesh
 time step value to scale impulses into forcesidx
 starting location for impulses in lam
public void zeroForces()
public int getBilateralForces(VectorNd lam, int idx)
lam
, starting at location idx
. The method should return idx
incremented by the number of bilateral constraints.lam
 returns the bilateral forcesidx
 starting location for impulses in lam
public int numEngagedUnilaterals()
public double getUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals, boolean updateEngaged)
unilterals
. If updatedEngaged
is true
, the method will first check the engagement of each
constraint and break it if certain conditions are met.unilaterals
 collects the unilateral constraintsupdateEngaged
 if true
, update constraint engagementpublic void updateUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals, int idx, int numc)
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.
unilaterals
 constraints to be updatedidx
 starting index in unilaterals
numc
 number of constraints to updatepublic int setUnilateralForces(VectorNd the, double h, int idx)
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.the
 supplies the unilateral impulse valuesh
 time step value to scale impulses into forcesidx
 starting location for impulses in the
public int getUnilateralForces(VectorNd the, int idx)
the
, starting
at location idx
. The method should return idx
incremented
by the number of currently engaged unilateral constraints.the
 returns the unilateral forcesidx
 starting location for impulses in the
public int numConstraints()
numBilaterals()
and numUnilaterals()
.public abstract void projectToConstraints(RigidTransform3d TGD, RigidTransform3d TCD, VectorNd coords)
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 nonnull
, 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.
TGD
 returns the nearest transform to TCD
that is legal with
respect to the bilateral (and possibly some unilateral) constraintsTCD
 transform from frame C to D to be projectedcoords
 if nonnull
, should be used to return coordinate
valuespublic void projectAndUpdateCoordinates(RigidTransform3d TGD, RigidTransform3d TCD)
public int numBilaterals()
public int numUnilaterals()
public abstract void updateConstraints(RigidTransform3d TGD, RigidTransform3d TCD, Twist errC, Twist velGD, boolean updateEngaged)
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:
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
.
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 usederrC
 error transform from frame C to G, represented as a
Twist
velGD
 velocity of frame G with respect to D, as seen in frame GupdateEngaged
 if true
, requests the updating of unilateral
engaged
and distance
attributes as describe above.public abstract void initializeConstraints()
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.public void transformGeometry(GeometryTransformer gtr, RigidTransform3d TCW, RigidTransform3d TDW)
gtr
 transformer implementing the transformationTCW
 current transform from C to worldTDW
 current transform from D to worldpublic void scaleDistance(double s)
s
 scale factorpublic void getState(DataBuffer data)
BodyConnector
class in the ArtiSynth core code.data
 buffer in which to store statepublic void setState(DataBuffer data)
BodyConnector
class
in the ArtiSynth core code.data
 buffer from which state is loadedpublic static double findNearestAngle(double ref, double ang)
ang
, find an equivalent angle that is within
+/ PI of a given reference angle ref
.ref
 reference angle (radians)ang
 initial angle (radians)ang
within +/ PI
of ref
.public int numCoordinates()
public RigidBodyConstraint getConstraint(int idx)
idx
th constraint that was added inside initializeConstraintInfo()
.idx
 constraint indexidx
th constraintpublic void getCoordinates(VectorNd coords, RigidTransform3d TGD)
TGD
is nonnull
, 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.coords
 returns the coordinates. Its size will be set to numCoordinates()
.TGD
 if nonnull
, provides TCD on input and TGD on output.public void setCoordinateValues(VectorNd coords, RigidTransform3d TGD)
coords
 new coordinate values. Must have a length >= numCoordinates()
.TGD
 if nonnull
, returns the corresponding TGD transformpublic abstract void coordinatesToTCD(RigidTransform3d TCD, VectorNd coords)
TCD
 returns the TCD transformcoords
 supplies the coordinate values and
must have a length >= numCoordinates()
.public void coordinatesToTCD(RigidTransform3d TCD)
TCD
is set to the
identity.TCD
 returns the TCD transformpublic void TCDToCoordinates(VectorNd coords, RigidTransform3d TCD)
coords
. Otherwise this method does nothing.
It is assumed that TCD
is legal with respect the coupling's
bilateral constraints, as defined by projectToConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.matrix.VectorNd)
;
otherwise, projectToConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.matrix.VectorNd)
should be used instead.
When setting coordinate values, they should not be clipped to
their maximum and minimum values (as defined by getCoordinateRange(int)
.
coords
 returns the coordinate valuesTCD
 transform from frame C to Dpublic void setCoordinateValue(int idx, double value, RigidTransform3d TGD)
idx
ith coordinate supported by this
coupling. An exception will be generated if coordinates are not
supported.
If TGD
is nonnull
, 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 bilaterallegal
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.
idx
 coordinate indexvalue
 new coordinate valueTGD
 if nonnull
, is used to update the other coordinate
values, and then return the final TGD resulting from the new coordinatespublic double getCoordinate(int idx, RigidTransform3d TGD)
idx
ith coordinate supported by
this coupling. An exception will be generated if coordinates are not
supported.
If TGD
is nonnull
, 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 bilaterallegal transform
TGD using projectToConstraints(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.matrix.VectorNd)
. The projected value is returned
in TGD
.
idx
 coordinate indexTGD
 if nonnull
, is projected onto the contraints and used
to update the coordinate valuespublic DoubleInterval getCoordinateRange(int idx)
idx
ith coordinate supported by this
coupling. An exception will be generated if coordinates are not
supported.idx
 coordinate indexpublic double clipCoordinate(int idx, double value)
value
to the range for the idx
ith coordinate
supported by this coupling. An exception will be generated if coordinates
are not supported.idx
 idx coordinate indexvalue
 value to be clippedpublic void setCoordinateRange(int idx, DoubleInterval range)
idx
ith coordinate supported by this
coupling. An exception will be generated if coordinates are not
supported.idx
 coordinate indexrange
 range for the coordinatepublic RigidBodyCoupling clone()
clone
in class java.lang.Object
public static void main(java.lang.String[] args)