maspack.spatialmotion
Class RigidBodyCoupling

java.lang.Object
  extended by maspack.spatialmotion.RigidBodyCoupling
Direct Known Subclasses:
CustomSphericalCoupling, FullPlanarCoupling, ParameterizedCoupling, PlanarCoupling, RevoluteCoupling, RollPitchCoupling, SegmentedPlanarCoupling, 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 XDB. Body A is associated with an operation frame F, described with respect to A by the fixed transform XFA. Enforcement of the constraint involves trying to keep F on a constraint surface whose location is constant with respect to D. The constraint frame C is the frame on the constraint surface which is nearest to F. Wrenches to enforce the constraint are defined (initially) with repsect to C and then transformed into the body frames A and B. Ideally, F and C coincide; otherwise, the transform XFC defines the constraint error and a linearization of this, projected onto the constraint wrenches in C, gives the linearized distance associated with each constraint.


Field Summary
static int BILATERAL
          Constraint is bilateral.
static int LINEAR
          Constraint is linear.
static int ROTARY
          Constraint is rotary.
 
Method Summary
 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 getBilateralForceF()
           
 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 XCD, RigidTransform3d XFD, RigidTransform3d XERR, boolean setEngaged)
          Computes the constraint frame C 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 getUnilateralForceF()
           
 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.
abstract  void projectToConstraint(RigidTransform3d XCD, RigidTransform3d XFD)
          Computes the frame C on the constraint surface which is closest to a given frame F.
 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(AffineTransform3dBase X, RotationMatrix3d Ra, RigidTransform3d XFW, RigidTransform3d XDW)
          Transforms the geometry of this coupling, in response to an affine transform X applied in world coordinates.
 void updateBodyStates(RigidTransform3d XFD, RigidTransform3d XCD, RigidTransform3d XERR, Twist velA, Twist velB, 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

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

getBilateralForceF

public Wrench getBilateralForceF()

getUnilateralForceF

public Wrench getUnilateralForceF()

updateConstraintsFromC

public void updateConstraintsFromC()

updateBodyStates

public void updateBodyStates(RigidTransform3d XFD,
                             RigidTransform3d XCD,
                             RigidTransform3d XERR,
                             Twist velA,
                             Twist velB,
                             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 XCD,
                                         RigidTransform3d XFD)
Computes the frame C on the constraint surface which is closest to a given frame F. The input consists of the transform from F to D.

Parameters:
XCD - returns the transform from C to D
XFD - transform from frame F 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 XCD,
                                       RigidTransform3d XFD,
                                       RigidTransform3d XERR,
                                       boolean setEngaged)
Computes the constraint frame C and the associated constraint information. C is determined by projecting F 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 C) 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 XFC. 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
XCD - returns the transform from C to D
XFD - transform from operation frame F to D
XERR - 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(AffineTransform3dBase X,
                              RotationMatrix3d Ra,
                              RigidTransform3d XFW,
                              RigidTransform3d XDW)
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 F 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' = XDW.R^T Ra^T X.M XDW.R p
 
which is equivalent to transforming p to world coordinates (using XDW), applying X, and then transforming back to D using the modified XDW produced by applying X.

Parameters:
X - affine transform applied to the coupling in world coordinates
Ra - rotational component of the matrix part of X.
XFW - current transform from F to world
XDW - 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.