maspack.spatialmotion

Class SegmentedPlanarCoupling

java.lang.Object

maspack.spatialmotion.RigidBodyCoupling

maspack.spatialmotion.SegmentedPlanarCoupling

All Implemented Interfaces:

java.lang.Cloneable

public class SegmentedPlanarCoupling
extends RigidBodyCoupling

Field Summary

Fields inherited from class maspack.spatialmotion.RigidBodyCoupling

debug

Constructor Summary

Constructors

Constructor and Description
SegmentedPlanarCoupling()
SegmentedPlanarCoupling(double[] segs)

Method Summary

Modifier and Type	Method and Description
SegmentedPlanarCoupling	clone()
Plane	closestPlane(Point3d p) Returns the plane closest to a specified point in the D coordinate frame.
void	coordinatesToTCD(RigidTransform3d TCD, VectorNd coords) Computes the TCD transform for a set of coordinates, if coordinates are supported.
java.util.ArrayList<Plane>	getPlanes() Returns a list of the planes in frame D.
java.util.ArrayList<Point3d>	getSegmentPoints() Returns a list of points describing the piecewise linear curve in the D frame x-z plane which defines the plane segments.
void	initializeConstraints() Called inside the RigidBodyCoupling constructor to allocate constraint and coordinate information, using calls to the addConstraint and addCoordinate methods.
boolean	isUnilateral()
int	numBilaterals() Returns the number of bilateral constraints associated with this coupling.
int	numPlanes() Returns the number of planar segments associated with this coupling.
int	numUnilaterals() Returns the number of unilateral constraints associated with this coupling, engaged or otherwise.
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.
void	setSegments(double[] segs) Sets the plane segments associated with this SegmentedPlanarCoupling.
void	setUnilateral(boolean unilateral)
void	transformGeometry(GeometryTransformer gt, RigidTransform3d TCW, RigidTransform3d TDW) Transforms the geometry of this coupling.
void	updateConstraints(RigidTransform3d TGD, RigidTransform3d TCD, Twist errC, Twist velGD, boolean updateEngaged) Called from RigidBodyCoupling.updateBodyStates(maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.matrix.RigidTransform3d, maspack.spatialmotion.Twist, boolean) to update the constraints, usually once per simulation step.

Methods inherited from class maspack.spatialmotion.RigidBodyCoupling

clipCoordinate, coordinatesToTCD, findNearestAngle, getBilateralConstraints, getBilateralForceG, getBilateralForces, getBreakSpeed, getCompliance, getConstraint, getConstraintFlags, getConstraintForce, getCoordinate, getCoordinateIndex, getCoordinateMotionType, getCoordinateName, getCoordinateRange, getCoordinates, getCoordinateSpeed, getCoordinateWrench, getDamping, getLinearLimitTol, getRotaryLimitTol, getState, getUnilateralConstraints, getUnilateralForceG, getUnilateralForces, getUnilateralState, isCoordinateLocked, main, numConstraints, numCoordinates, numEngagedUnilaterals, printConstraintInfo, projectAndUpdateCoordinates, setBilateralForces, setBreakSpeed, setCompliance, setCoordinateLocked, setCoordinateName, setCoordinateRange, setCoordinateValue, setCoordinateValues, setDamping, setLinearLimitTol, setRotaryLimitTol, setState, setUnilateralForces, setUnilateralState, TCDToCoordinates, updateBodyStates, updateUnilateralConstraints, zeroForces

Methods inherited from class java.lang.Object

equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Detail

SegmentedPlanarCoupling

public SegmentedPlanarCoupling()

SegmentedPlanarCoupling

public SegmentedPlanarCoupling(double[] segs)

Method Detail

getSegmentPoints

public java.util.ArrayList<Point3d> getSegmentPoints()

Returns a list of points describing the piecewise linear curve in the D frame x-z plane which defines the plane segments. These returned points are read-only and should not be modified by the caller.

Returns:

points defining the plane segments (should not be modified)

getPlanes

public java.util.ArrayList<Plane> getPlanes()

Returns a list of the planes in frame D. The returned planes are read-only and should not be modified by the caller.

Returns:

plane segments (should not be modified)

numPlanes

public int numPlanes()

Returns the number of planar segments associated with this coupling.

Returns:

number of planes

setUnilateral

public void setUnilateral(boolean unilateral)

isUnilateral

public boolean isUnilateral()

setSegments

public void setSegments(double[] segs)

Sets the plane segments associated with this SegmentedPlanarCoupling. The segments are defined by a sequence of x-z coordinate pairs in the D coordinate frame. The number of segments equals the number of pairs minus one, so there must be at least two pairs (which would define a single plane). Segments are assumed to be contiguous, and the normal for each is defined by u X y, where u is a vector in the direction of the segment (from first to last coordinate pair) and y is the direction of the D frame y axis. The first and last plane segments are assumed to extend to infinity.

Parameters:

segs - x-z coordinate pairs defining the segments

closestPlane

public Plane closestPlane(Point3d p)

Returns the plane closest to a specified point in the D coordinate frame.

Parameters:

p - point to find nearest plane to

numUnilaterals

public int numUnilaterals()

Description copied from class: RigidBodyCoupling

Returns the number of unilateral constraints associated with this coupling, engaged or otherwise.

Overrides:

numUnilaterals in class RigidBodyCoupling

Returns:

number of unilateral constraints

numBilaterals

public int numBilaterals()

Description copied from class: RigidBodyCoupling

Returns the number of bilateral constraints associated with this coupling.

Overrides:

numBilaterals in class RigidBodyCoupling

Returns:

number of bilateral constraints

projectToConstraints

public void projectToConstraints(RigidTransform3d TGD,
                                 RigidTransform3d TCD,
                                 VectorNd coords)

Description copied from class: RigidBodyCoupling

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

Specified by:

projectToConstraints in class RigidBodyCoupling

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

initializeConstraints

public void initializeConstraints()

Description copied from class: RigidBodyCoupling

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.

Specified by:

initializeConstraints in class RigidBodyCoupling

updateConstraints

public void updateConstraints(RigidTransform3d TGD,
                              RigidTransform3d TCD,
                              Twist errC,
                              Twist velGD,
                              boolean updateEngaged)

Description copied from class: RigidBodyCoupling

Called from RigidBodyCoupling.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.

Specified by:

updateConstraints in class RigidBodyCoupling

Parameters:

TGD - idealized joint transform from frame G to D, obtained by calling RigidBodyCoupling.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.

scaleDistance

public void scaleDistance(double s)

Description copied from class: RigidBodyCoupling

Scales the distance units of this constraint. Subclasses should reimplement this as needed.

Overrides:

scaleDistance in class RigidBodyCoupling

Parameters:

s - scale factor

transformGeometry

public void transformGeometry(GeometryTransformer gt,
                              RigidTransform3d TCW,
                              RigidTransform3d TDW)

Description copied from class: RigidBodyCoupling

Transforms the geometry of this coupling. Subclasses should reimplement this as needed.

Overrides:

transformGeometry in class RigidBodyCoupling

Parameters:

gt - transformer implementing the transformation

TCW - current transform from C to world

TDW - current transform from D to world

coordinatesToTCD

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

Specified by:

coordinatesToTCD in class RigidBodyCoupling

Parameters:

TCD - returns the TCD transform

coords - supplies the coordinate values and must have a length >= RigidBodyCoupling.numCoordinates().

clone

public SegmentedPlanarCoupling clone()

Overrides:

clone in class RigidBodyCoupling