|
|||||||||
PREV CLASS NEXT CLASS | FRAMES NO FRAMES | ||||||||
SUMMARY: NESTED | FIELD | CONSTR | METHOD | DETAIL: FIELD | CONSTR | METHOD |
java.lang.Objectmaspack.spatialmotion.RigidBodyCoupling
public abstract class RigidBodyCoupling
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 |
---|
public static final int BILATERAL
public static final int LINEAR
public static final int ROTARY
Method Detail |
---|
public void setContactDistance(double d)
d
- contact distance for unilateral constraintspublic double getContactDistance()
public void setBreakSpeed(double v)
v
- minimum normal speed for breaking unilateral constraintspublic double getBreakSpeed()
public void setBreakAccel(double a)
a
- minimum normal acceleration for breaking unilateral constraintspublic double getBreakAccel()
public VectorNd getCompliance()
public void setCompliance(VectorNd c)
c
- new compliance valuespublic VectorNd getDamping()
public void setDamping(VectorNd c)
c
- new damping valuespublic VectorNi getConstraintInfo()
BILATERAL
, LINEAR
, and ROTARY
.
public Wrench getBilateralForceF()
public Wrench getUnilateralForceF()
public void updateConstraintsFromC()
public void updateBodyStates(RigidTransform3d XFD, RigidTransform3d XCD, RigidTransform3d XERR, Twist velA, Twist velB, boolean setEngaged)
public RigidBodyConstraint getConstraint(int idx)
public int getBilateralConstraints(java.util.ArrayList<RigidBodyConstraint> bilaterals)
public int setBilateralImpulses(VectorNd lam, double h, int idx)
public void zeroImpulses()
public int getBilateralImpulses(VectorNd lam, int idx)
public int numUnilaterals()
public double getUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals, boolean setEngaged)
public void updateUnilateralConstraints(java.util.ArrayList<RigidBodyConstraint> unilaterals, int offset, int numc)
public int setUnilateralImpulses(VectorNd the, double h, int idx)
public int getUnilateralImpulses(VectorNd the, int idx)
public int maxConstraints()
public abstract void projectToConstraint(RigidTransform3d XCD, RigidTransform3d XFD)
XCD
- returns the transform from C to DXFD
- transform from frame F to Dpublic abstract int numBilaterals()
public abstract int maxUnilaterals()
public abstract void getConstraintInfo(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info, RigidTransform3d XCD, RigidTransform3d XFD, RigidTransform3d XERR, boolean setEngaged)
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
.
info
- used to return information for each possible constraint wrenchesXCD
- returns the transform from C to DXFD
- transform from operation frame F to DXERR
- TODOsetEngaged
- if true
, this method should determine
if the constraint is engaged.public abstract void initializeConstraintInfo(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info)
public void transformGeometry(AffineTransform3dBase X, RotationMatrix3d Ra, RigidTransform3d XFW, RigidTransform3d XDW)
X.M = P RaGiven 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 pwhich 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.
X
- affine transform applied to the coupling in world coordinatesRa
- rotational component of the matrix part of X.XFW
- current transform from F to worldXDW
- current transform from D to worldpublic void scaleDistance(double s)
public void skipAuxState(DataBuffer data)
public void getAuxState(DataBuffer data)
public void setAuxState(DataBuffer data)
public void getInitialAuxState(DataBuffer newData, DataBuffer oldData)
public void setDistancesAndZeroDerivatives(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo[] info, int numc, Twist err)
distance
fields within the info
array to the dot product of wrenchC
with err
,
and sets the dotWrenchC
fields to 0.
public void setDistanceAndZeroDerivative(maspack.spatialmotion.RigidBodyCoupling.ConstraintInfo info, Twist err)
public 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
.
|
|||||||||
PREV CLASS NEXT CLASS | FRAMES NO FRAMES | ||||||||
SUMMARY: NESTED | FIELD | CONSTR | METHOD | DETAIL: FIELD | CONSTR | METHOD |