4 Mechanical Models II 4.8 General component arrangements 5 Simulation Control

4.9 Custom Joints

If desired, it is also possible for applications to create their own custom joints. This involves creating two custom classes: a coupling class that does the constraint computations, and a joint class that wraps around it and allows it to connect connectable bodies. Details on how to create these classes are given in Sections 4.9.3 and 4.9.4, after some explanation of the constraint mechanism that underlies joint operation.

This section assumes that the reader is highly familiar with spatial kinematics and dynamics.

4.9.1 Joint constraints

To create a custom joint, it is necessary to understand how joints are implemented. The basic function of a joint is to constraint the set of poses allowed by the joint transform ${\bf T}_{CD}$ that relates frame C to D. To do this, the joint imposes restrictions on the six-dimensional spatial velocity $\hat{\bf v}_{CD}$ that describes how frame C is moving with respect to D. This restriction is done using a set of bilateral constraints ${\bf G}_{k}$ and (in some cases) unilateral constraints ${\bf N}_{l}$ , each of which is a $1\times 6$ matrix that acts to restrict a single degree of freedom in $\hat{\bf v}_{CD}$ (see Section A.5 for a review of spatial velocities and forces). Bilateral constraints take the form of an equality,

${\bf G}_{k}\,\hat{\bf v}_{CD}=0,$

(4.23)

while unilateral constraints take the form of an inequality:

${\bf N}_{l}\,\hat{\bf v}_{CD}\geq 0.$

(4.24)

These constraints are defined with respect to frame C, and their total number equals the number of DOFs that the joint removes. A joint’s main computational task is to specify these constraints. ArtiSynth then uses its own knowledge of how frames C and D are connected to bodies A and B (or ground, if there is no body B) to map the individual ${\bf G}_{k}$ and ${\bf N}_{l}$ onto the joint’s full bilateral and unilateral constraint matrices ${\bf G}_{\text{J}}$ and ${\bf N}_{\text{J}}$ (see (3.10) and (3.11)) that restrict the body velocities.

As a simple example, consider a cylindrical joint, in which C is free to rotate about and translate along the axis of D but other motions are restricted. Letting ${\bf v}$ and $\boldsymbol{\omega}$ denote the translational and rotational components of $\hat{\bf v}_{CD}$ , such that

$\hat{\bf v}_{CD}=\left(\begin{matrix}{\bf v}\\ \boldsymbol{\omega}\end{matrix}\right),$

we see that the constraints must enforce

$v_{x}=v_{y}=\omega_{x}=\omega_{y}=0.$

(4.25)

This can be accomplished using four constraints defined as follows:

	$\displaystyle{\bf G}_{0}=$	$\displaystyle(1,0,0,0,0,0)$
	$\displaystyle{\bf G}_{1}=$	$\displaystyle(0,1,0,0,0,0)$
	$\displaystyle{\bf G}_{2}=$	$\displaystyle(0,0,0,1,0,0)$
	$\displaystyle{\bf G}_{3}=$	$\displaystyle(0,0,0,0,1,0).$

Constraining velocities is a necessary but insufficient condition for constraint enforcement. Because of numerical errors, as well as the fact that constraints are often nonlinear, the joint transform ${\bf T}_{CD}$ will tend to drift away from the joint restrictions as the simulation proceeds, leading to the error ${\bf T}_{err}$ described at the end of Section 3.3.1. These errors are corrected during a position correction at the end of every simulation time step: the joint first projects ${\bf T}_{CD}$ onto the nearest valid constraint surface to form ${\bf T}_{GD}$ , and ${\bf T}_{err}$ is then computed from

${\bf T}_{err}={\bf T}_{CG}={\bf T}_{GD}^{-1}{\bf T}_{CD}.$

(4.26)

Because ${\bf T}_{err}$ is (usually) small, we can approximate it as a twist $\hat{\boldsymbol{\delta}}_{err}$ representing a small displacement from frame G (which lies on the constraint surface) to frame C. During the position correction, ArtiSynth adjusts the pose of C relative to D in order to try and bring $\hat{\boldsymbol{\delta}}_{err}$ to zero. To do this, it uses an estimate of the distance $d_{k}$ along each constraint to the constraint surface, which it computes from the dot product of ${\bf G}_{k}$ and $\hat{\boldsymbol{\delta}}_{err}$ :

$d_{k}={\bf G}_{k}\,\hat{\boldsymbol{\delta}}_{err}.$

(4.27)

ArtiSynth assembles these distances into a composite distance vector ${\bf d}_{g}$ for all bilateral constraints, and then uses the system solver to find a displacement $\boldsymbol{\delta}{\bf q}$ of the system coordinates that satisfies

${\bf G}({\bf q})\,\boldsymbol{\delta}{\bf q}=-{\bf d}_{g}.$

Adding $\boldsymbol{\delta}{\bf q}$ to the system coordinates ${\bf q}$ then reduces the constraint errors. While for nonlinear constraints several steps may be required to bring the error to 0, the process usually converges quickly.

Unlike bilateral constraints, unilateral constraints are one-sided, and take effect, or are engaged, only when ${\bf T}_{CD}$ encounters an inadmissible region. The constraint then acts to prevent further penetration into the region, via the velocity restriction (4.24), and also to push ${\bf T}_{CD}$ out of the inadmissible region, using a position correction analogous to that used for bilateral constraints.

Whether or not a unilateral constraint is engaged is determined by its engaged value $E_{l}$ , which takes one of the three values: $\{0,1,-1\}$ , and is updated by the joint implementation as the simulation proceeds. A value of 0 means that the constraint is not engaged, and will not be included in the joint’s unilateral constraint matrix ${\bf N}_{\text{J}}$ . Otherwise, if $E_{l}$ is or , then the constraint is engaged and will be included in ${\bf N}_{\text{J}}$ , using ${\bf N}_{l}$ if $E_{l}=1$ , or its negative $-{\bf N}_{l}$ if $E_{l}=-1$ . $E_{l}$ therefore defines a sign for the constraint. General details on how unilateral constraints should be engaged or disengaged are discussed in Section 4.9.2.

A common use of unilateral constraints is to implement limits on joint coordinate values; this also illustrates the utility of $E_{l}$ . For example, the cylindrical joint mentioned above may have two coordinates, and $\theta$ , describing the translation and rotation along and about the D frame’s axis. Now suppose we wish to bound , such that

$z_{\text{min}}\leq z\leq z_{\text{max}}.$

(4.28)

When these limits are violated, a unilateral constraint can be engaged to limit motion along the axis. A constraint ${\bf N}_{z}$ that will do this is

${\bf N}_{z}=(0,0,1,0,0,0).$

Whenever $z\leq z_{\text{min}}$ , using ${\bf N}_{z}$ in (4.24) will ensure that $\dot{\bf z}\geq 0$ and hence will not fall further below the lower bound. On the other hand, when $z\geq z_{\text{max}}$ , we want to employ $-{\bf N}_{z}$ in (4.24) to ensure that $\dot{\bf z}\leq 0$ . In other words, lower bounds can be enforced by engaging ${\bf N}_{l}$ with $E_{l}=1$ , while upper bounds can be enforced with $E_{l}=-1$ .

As with bilateral constraints, constraining velocities is not sufficient; it is also necessary to correct position errors, particularly as unilateral constraints are typically not engaged until the inadmissible region is violated. The position correction procedure is the same: for each engaged unilateral constraint, find a distance $d_{l}$ along its constraint direction that indicates the distance to the inadmissible region boundary. ArtiSynth will then assemble these $d_{l}$ into a composite distance vector ${\bf d}_{n}$ for all unilataral constraints, and solve for a system coordinate displacement $\boldsymbol{\delta}{\bf q}$ that satisfies

${\bf N}({\bf q})\,\boldsymbol{\delta}{\bf q}\geq-{\bf d}_{n}.$

(4.29)

Because of the inequality direction in (4.29), distances $d_{l}$ representing penetration into a inadmissible region must be negative. For coordinate bounds such as (4.28), we need to use $d_{l}=z-z_{\text{min}}$ for the lower bound and $d_{l}=z_{\text{max}}-z$ for the upper bound. Alternatively, if the unilateral constraint has been included into the projection of C onto G and hence into the error term $\hat{\boldsymbol{\delta}}_{err}$ , $d_{l}$ can be computed from

$d_{l}=E_{l}{\bf N}_{l}\,\hat{\boldsymbol{\delta}}_{err}.$

(4.30)

Note that unilateral constraints for coordinate limits are not usually incorporated into the projection; more on this details are given in Section 4.9.4.

As simulation proceeds, the velocity limits imposed by (4.23) and (4.24) are enforced by bilateral and unilateral constraint forces ${\bf f}_{k}$ and ${\bf f}_{l}$ whose magnitudes are given by

${\bf f}_{k}={\bf G}_{k}^{T}\,\lambda_{k},\qquad{\bf f}_{l}={\bf N}_{l}^{T}\,% \theta_{l},$

(4.31)

where $\lambda_{k}$ and $\theta_{l}$ are the Lagrange multipliers computed by the mechanical system solver (and are components of $\boldsymbol{\lambda}$ or $\boldsymbol{\theta}$ in (1.8) and (1.6)). ${\bf f}_{k}$ and ${\bf f}_{l}$ are 6 DOF spatial force vectors, or wrenches (Section A.5), which like ${\bf G}_{k}$ and ${\bf N}_{l}$ are expressed in frame C. Because ${\bf G}_{k}^{T}$ and ${\bf N}_{l}^{T}$ are proportional to spatial wrenches, they are often themselves referred to as constraint wrenches, and within the ArtiSynth codebase are described by a Wrench object.

4.9.2 Unilateral constraint engagement

As mentioned above, joints which implement unilateral constraints must monitor ${\bf T}_{CD}$ and the joint coordinates as the simulation proceeds and decide when to engage or disengage them.

Engagement is usually easy: a constraint is engaged whenever ${\bf T}_{CD}$ or a joint coordinate hits an inadmissible region. The constraint ${\bf N}_{l}$ is itself a spatial vector that is (locally) perpendicular to the inadmissible region boundary, and $E_{l}$ is chosen to be either or so that $E_{l}{\bf N}_{l}$ is directed away from the inadmissible region. In the remainder of this section, we shall assume $E_{l}=1$ .

Figure 4.22: Left: A joint configuration at A inside an inadmissible region (gray) is pushed further outside the region than intended B, so that it reenters the region during the next simulation step, resulting in chattering. Right: a deadband solution, in which the position correction is reduced sufficiently so that the joint configuration remains inside the region.

To disengage, we usually want to ensure that the joint configuration is out of the inadmissible region. If we have a constraint ${\bf N}_{l}$ , with a local distance $d_{l}$ defined such that $d_{l}<0$ implies the joint is inside the region, then we are out of the region when $d_{l}>0$ . However, if we use only this as the disengagement criterion, we may encounter a problem known as chattering, illustrated in Figure 4.22 (left). An inadmissible region is shown in gray, with a unilateral constraint $N_{1}$ perpendicular to its boundary. As simulation proceeds, the joint lands inside the region at an initial point A at the lower left, at a (negative) distance $d_{1}$ from the boundary. Ideally the position correction step will move the configuration by $-d_{1}$ so that it lands right on the region boundary. However, numerical errors and nonlinearities may mean that in fact it lands outside the region, at point B. Then on the next step it reenters the region, only to again be pushed out, etc.

Figure 4.23: Left: constraint oscillation, in which a joint configuration starting at A oscillates between two overlapping inadmissible regions 1 and 2 whose boundaries are not perpendicular. Right: ensuring that constraints stay engaged for more than one simulation step allows the solver to quickly determine a stable solution at F, where the region boundaries intersect.

ArtiSynth implements two solutions to chattering. One is to implement a deadband, so that instead of correcting the position by $-d_{1}$ , we correct it by $-d_{1}-p_{\text{tol}}$ , where $p_{\text{tol}}$ is a penetration tolerance. This means that the correction will try to leave the joint inside the region by a small amount (Figure 4.22, right) so that chattering is suppressed. The penetration tolerance used depends on the constraint type. Those that are primarily linear use the value of the penetrationTol property, while those that are primarily rotary use the value of the rotaryLimitTol property; both of these are exported as inheritable properties by both MechModel and BodyConnector, with default values computed from the model’s overall dimensions.

The second chattering solution is to disengage only when the joint is actively moving away from the region, as determined by $\dot{d_{l}}>0$ . The disengagement criteria then become

$d_{l}>0\quad\text{and}\quad\dot{d_{l}}>0.$

(4.32)

$\dot{d_{l}}$ is called the contact speed and can be computed from

$\dot{d_{l}}={\bf N}_{l}\,\hat{\bf v}_{CD}.$

(4.33)

Another problem, which we call constraint oscillation, can occur when we are near two or more overlapping inadmissible regions whose boundaries are not perpendicular. See Figure 4.23 (left), which shows two overlapping regions 1 and 2. The joint starts at point A, inside region 1 but just outside region 2. Since only constraint 1 is engaged, the position correction moves it toward the boundary of 1, overshooting and landing at point B outside of 1 but inside region 2. Constraint 2 now engages, moving the joint to C, where it is past the boundary of 2 but inside 1 again. While the example in the figure converges to the corner where the boundaries of 1 and 2 meet, convergence may be slow and may be prevented entirely by external forcing. While the mechanisms that prevent chattering may also prevent oscillation, we find that an additional measure is useful, which is to simply require that a constraint must be engaged for at least two simulation steps. The result is shown in Figure 4.23 (right), where after the joint arrives at B, constraint 1 remains engaged along with constraint 2, and the subsequent solution takes the joint directly to point F at the corner where 1 and 2 meet.

4.9.3 Implementing a custom joint

All of the work of computing joint constraints and coordinates, as described in the previous sections, is done within a “coupling” class which is a subclass of RigidBodyCoupling. An instance of this is then embedded within a “joint” class (which is a subclass of JointBase) that supports connections with other bodies, provides rendering, exports various properties, and allows the joint to be attached to a MechModel.

For purposes of this discussion, we will assume that these two custom classes are called CustomCoupling and CustomJoint, respectively. The implementation of CustomJoint can be as simple as this:

⬇

import artisynth.core.mechmodels.ConnectableBody;

import artisynth.core.mechmodels.JointBase;

import maspack.matrix.RigidTransform3d;

public class CustomJoint extends JointBase {

public CustomJoint() {

setCoupling (new CustomCoupling());

}

public CustomJoint (

ConnectableBody bodyA, ConnectableBody bodyB, RigidTransform3d TDW) {

this(); // call the default constructor

setBodies(bodyA, bodyB, TDW);

}

This creates an instance of CustomCoupling and sets it to the (inherited) myCoupling attribute inside the default constructor (which is where this normally should be done). Another constructor is provided which uses setBodies() to create a joint that is attached to two bodies with the D frame specified in world coordinates. In practice, a joint may also export some properties (such as joint coordinates), provide additional constructors, and implement rendering; one should examine the source code for some existing joints.

4.9.4 Implementing a custom coupling

Implementing a custom coupling constitutes most of the effort in creating a custom joint, since the coupling is responsible for maintaining the constraints ${\bf G}_{k}$ and ${\bf N}_{l}$ that enforce the joint behavior.

Before proceeding, we discuss the coordinate frame in which these constraints are situated. It is often convenient to describe joint constraints with respect to frame C, since rotations are frequently centered there. However, the joint transform ${\bf T}_{CD}$ usually contains errors (Section 3.3.1) due to a combination of simulation error and possible joint compliance. To determine these errors, we project C onto another frame G, defined to be the nearest to C that is consistent with the bilateral (and possibly some unilateral) constraints. (This is done by the projectToConstraints() method, described below). The result is a joint transform ${\bf T}_{GD}$ that is “error free” with respect to bilateral constraints and also consistent with the coordinates (if supported). This makes it convenient to formulate constraints with respect to frame G instead of C, and so this is the convention ArtiSynth uses. In particular, the updateConstraints() method, described below, uses ${\bf T}_{GD}$ , together with the spatial velocity $\hat{\bf v}_{GD}$ describing the motion of G with respect to C.

An actual custom coupling implementation involves subclassing RigidBodyCoupling and then implementing five abstract methods, the outline of which looks like this:

⬇

import maspack.matrix.*;

import maspack.spatialmotion.*;

import maspack.util.*;

class CustomCoupling extends RigidBodyCoupling {

public CustomCoupling() {

super();

}

// Initialize the constraints and coordinates.

public void initializeConstraints () {

...

}

// If coordinates are implemented, set TCD from the supplied coordinates.

public void coordinatesToTCD (RigidTransform3d TCD, VectorNd coords) {

...

}

// If coordinates are implemented, set their values from TCD.

public void TCDToCoordinates (VectorNd coords, RigidTransform3d TCD) {

...

}

// Project TCD to the nearest transform TGD admissible to the

// bilateral constraints, and maybe some unilateral constraints.

public void projectToConstraints (

RigidTransform3d TGD, RigidTransform3d TCD, boolean updateCoords) {

...

}

// Update the constraint wrenches, and maybe the engaged

// and distance settings for some unilateral constraints.

public void updateConstraints (

RigidTransform3d TGD, RigidTransform3d TCD, Twist errC,

Twist velGD, boolean updateEngaged) {

...

}

The implementations of these methods are now described in detail.

initializeConstraints()

This method has the signature

⬇

public void initializeConstraints ()

and is called in the coupling’s superclass constructor (i.e., the constructor for RigidBodyCoupling). It is responsible for initializing the coupling’s constraints and (if supported) coordinates.

Constraints are added using one of the two superclass methods:

⬇

RigidBodyConstraint addConstraint (int flags)

RigidBodyConstraint addConstraint (int flags, Wrench wrench)

Each creates a new RigidBodyConstraint and adds it to the coupling’s constraint list. flags is an or-ed combination of the following flags defined in RigidBodyConstraint:

BILATERAL: Constraint is bilateral (i.e., an equality). If BILATERAL is not specified, the constraint is considered unilateral.
ROTARY: Constraint primarily restricts rotary motion. If it is unilateral, the joint’s rotaryLimitTol property is used for its penetration tolerance.
LINEAR: Constraint primarily restricts translational motion. If it is unilateral, the joint’s penetrationTol property is used for its penetration tolerance.
CONSTANT: Constraint is constant with respect to frame G. This flag is set automatically if the constraint is created using addConstraint(flags,wrench).
LIMIT: Constraint is used to enforce limits for a coordinate. This flag is set automatically if the constraint is specified as the limit constraint for a coordinate.

The method addConstraint(flags,wrench) takes an additional Wrench argument specifying the (presumed constant) value of the constraint with respect to frame G, and sets the CONSTANT flag just described.

Coordinates are added similarly using using one of the two superclass methods:

⬇

CoordinateInfo addCoordinate ()

CoordinateInfo addCoordinate (

double min, double max, int flags, RigidBodyConstraint limCon)

Each creates a new CoordinateInfo object (which is an inner class of RigidBodyCoupling), and adds it to the coupling’s coordinate list. In the second method, min and max give the initial range limits, and limCon, if non-null, specifies a unilateral constraint (previously created using addConstraint) for enforcing the limits and causes that constraint’s LIMIT to be set. The argument flags is reserved for future use and should be set to 0. If not specified, the default coordinate limits are $(-\inf,\inf)$ .

The implementation of initializeConstraints() for a coupling that implements a hinge type joint might look like this:

⬇

public void initializeConstraints () {

addConstraint (BILATERAL|LINEAR, new Wrench (1, 0, 0, 0, 0, 0));

addConstraint (BILATERAL|LINEAR, new Wrench (0, 1, 0, 0, 0, 0));

addConstraint (BILATERAL|LINEAR, new Wrench (0, 0, 1, 0, 0, 0));

addConstraint (BILATERAL|ROTARY, new Wrench (0, 0, 0, 1, 0, 0));

addConstraint (BILATERAL|ROTARY, new Wrench (0, 0, 0, 0, 1, 0));

addConstraint (ROTARY, new Wrench (0, 0, 0, 0, 0, 1));

addCoordinate (-Math.PI, Math.PI, 0, getConstraint(5));

}

Six constraints are specified, with the sixth being a unilateral constraint that enforces the limits on the single coordinate describing the rotation angle. Each constraint and coordinate has an integer index giving the location in its list, in the order it was added. This index can be used to later retrieve the RigidBodyConstraint or CoordinateInfo object for the constraint or coordinate, using the methods getConstraint(idx) or getCoordinateInfo(idx).

Because initializeConstraints() is called in the superclass constructor, member attributes for the custom coupling will not yet be initialized when it is first called. Therefore, the method should not depend on the initial values of non-static member variables. initializeConstraints() can also be called later to rebuild the constraints if some defining setting is changed.

coordinatesToTCD()

This method has the signature

⬇

public void coordinatesToTCD (RigidTransform3d TCD, VectorNd coords)

and is called when needed by the system. If coordinates are supported, then the transform ${\bf T}_{CD}$ should be set from the coordinate values supplied in coords, and returned in the argument TCD. Otherwise, this method should do nothing.

TCDToCoordinates()

This method has the signature

⬇

public void TCDToCoordinates (VectorNd coords, RigidTransform3d TCD)

and is called when needed by the system. It is the inverse of coordinatesToTCD(): if coordinates are supported, then their values should be set from the joint transform ${\bf T}_{CD}$ supplied by TCD and returned in coords. Otherwise, this method should do nothing.

When calling this method, it is assumed that TCD is “legal” with respect to the joint’s constraints (as defined by projectToConstraints(), described next). If this is not the case, then projectToConstraints() should be called instead.

One issue that can arise is when a coordinate represents an angle $\phi$ that has a range greater than $2\pi$ . In that case, a common strategy is to compute a nominal value for $\phi$ , and then add or subtract $2\pi$ from it until the resulting value is as close as possible to the current value for the angular coordinate. This allows the angle to wrap through its entire range. To implement this, one can use the method

⬇

double nearestAngle (double phi)

in the coordinate’s CoordinateInfo object, which finds the angle equivalent to phi that is nearest to the current coordinate value.

Coordinate values computed by this method should not be clipped to their ranges.

projectToConstraints()

This method has the signature

⬇

public void projectToConstraints (

RigidTransform3d TGD, RigidTransform3d TCD, VectorNd coords)

and is called when needed by the system. It is responsible for projecting the joint transform ${\bf T}_{CD}$ (supplied by TCD) onto the nearest transform ${\bf T}_{GD}$ that is valid for the bilateral constraints, and returning this in TGD. If coordinates are supported and coords is non-null, then the coordinate values corresponding to ${\bf T}_{GD}$ should also be computed and returned in coords. The easiest way to do this is to simply call TCDToCoordinates(TGD,coords), although in some cases it may be computationally cheaper to compute both the coordinates and the projection at the same time.

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 ${\bf T}_{err}$ and the corresponding argument errC that is passed to updateConstraints().

updateConstraints()

This method has the signature

⬇

public void updateConstraints (

RigidTransform3d TGD, RigidTransform3d TCD, Twist errC,

Twist velGD, boolean updateEngaged)

and is usually called once per simulation time step. It 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 coordinate limit.

The method supplies several arguments:

•

TGD, containing the idealized joint transform ${\bf T}_{GD}$ from frame G to D produced by calling projectToConstraints().
•

TCD, containing the joint transform ${\bf T}_{CD}$ from frame C to D and supplied for legacy reasons.
•

errC, representing the (hopefully small) error transform ${\bf T}_{err}$ from frame C to G as a spatial twist vector.
•

velGD, giving the spatial velocity $\hat{\bf v}_{GD}$ of frame G with respect to D, as seen in G; this is needed to compute wrench derivatives.
•

updateEngaged, which requests the updating of unilateral engaged and distance attributes as describe above.

If the coupling supports coordinates, their values will be updated before the method is called so as to correspond to ${\bf T}_{GD}$ . If needed, a coordinate’s value may be obtained from the value attribute of its CoordinateInfo object, which may in turn be obtained using getCoordinateInfo(idx). Likewise, ConstraintInfo objects for each constraint may be obtaining using getConstraint(idx).

Constraint wrenches correspond to ${\bf G}_{k}$ and ${\bf N}_{l}$ in Section 4.9.1. These, along with their derivatives $\dot{\bf G}_{k}$ and $\dot{\bf N}_{l}$ , are described by the wrenchG and dotWrenchG attributes of each constraint’s RigidBodyConstraint object, and may be managed by a variety of methods:

⬇

Wrench getWrenchG() // return the reference to wrenchG

void setWrenchG (

double fx, double fy, double fx, double mx, double my, double mx)

void setWrenchG (Vector3d f, Vector3d m) // either f or m may be null

void setWrenchG (Wrench wr)

void negateWrenchG()

void zeroWrenchG()

Wrench getDotWrenchG() // return the reference to dotWrenchG

void setDotWrenchG (

double fx, double fy, double fx, double mx, double my, double mx)

void setDotWrenchG (Vector3d f, Vector3d m) // either f or m may be null

void setDotWrenchG (Wrench wr)

void negateDotWrenchG()

void zeroDotWrenchG()

dotWrenchG is used in computing the time derivative terms ${\bf g}$ and ${\bf n}$ that appear in (3.9) and (1.6). While these improve the computational accuracy of the simulation, their effect is often small, and so in practice one may be able to omit computing dotWrenchG and instead leave its value as 0.

Wrench information must also be computed for unilateral constraints which implement coordinate limits. While it is not necessary to compute the distance and engaged attributes for these constraints (this is done automatically), it is necessary to ensure that the wrench’s magnitude is compatible with the coordinate’s speed. More precisely, if the coordinate is given by $\phi$ , then the limit wrench ${\bf N}_{l}$ must have a magnitude such that

$\dot{\phi}={\bf N}_{l}\hat{\bf v}_{GD}.$

(4.34)

As mentioned above, if updateEngaged is true, the engaged and distance attributes for unilateral constraints not associated with coordinate limits must be updated. These correspond to $E_{l}$ and $d_{l}$ in Section 4.9.1, and are contained in the constraint’s RigidBodyConstraint object and may be queried using the methods

⬇

double getDistance()

void setDistance (double d)

int getEngaged()

void setEngaged (int engaged)

It is up to updateConstraints() to compute the distance, with a negative value denoting penetration into the inadmissible region. If projectToConstraints() is implemented so as to account for the constraint, then ${\bf T}_{GD}$ will be projected out of the inadmissible region and the distance will be implicitly present ${\bf T}_{err}$ and so can be recovered by taking the dot product of the constraint wrench and velGD:

⬇

RigidBodyConstraint cons = getConstraint (3); // assume constraint index is 3

...

double dist = cons.getWrench().dot (velGD);

Otherwise, if the constraint is not accounted for in projectToConstraints(), the distance must be obtained by other means.

To update engaged, one may use the general convenience method

⬇

void updateEngaged (

RigidBodyConstraint cons, double dist,

double dmin, double dmax, Twist velGD)

which sets engaged according to the rules of Section 4.9.2, for an inadmissible region corresponding to dist < dmin or dist > dmax. The upper or lower bounds may be removed by setting dmin to -inf or max to inf, respectively.

4.9.5 Example: a simple custom joint

Figure 4.24: Coordinate frames for the CustomJoint (left) and the associated CustomJointDemo (right).

An simple model illustrating custom joint creation is provided by

  artisynth.demos.tutorial.CustomJointDemo

This implements a joint class defined by CustomJoint (also in the package artisynth.demos.tutorial), which is actually just a simple implementation of SlottedHingeJoint (Section 3.4.4). Certain details are omitted, such as exporting coordinate values and ranges as properties, and other things are simplified, such as the rendering code. One may consult the source code for SlottedHingeJoint to obtain a more complete example.

This section will focus on the implementation of the joint coupling, which is created as an inner class of CustomJoint called CustomCoupling and which (like all couplings) extends RigidBodyCoupling. The joint itself creates an instance of the coupling in its default constructor, exactly as described in Section 4.9.3.

The coupling allows two DOFs (Figure 4.24, left): translation along the axis of D (described by the coordinate ), and rotation about the axis of D (described by the coordinate $\theta$ ), with ${\bf T}_{CD}$ related to the coordinates by (3.25). It implements initializeConstraints() as follows:

⬇

public void initializeConstraints () {

addConstraint (BILATERAL|LINEAR);

addConstraint (BILATERAL|LINEAR, new Wrench(0, 0, 1, 0, 0, 0));

addConstraint (BILATERAL|ROTARY, new Wrench(0, 0, 0, 1, 0, 0));

addConstraint (BILATERAL|ROTARY, new Wrench(0, 0, 0, 0, 1, 0));

addConstraint (LINEAR);

addConstraint (ROTARY, new Wrench(0, 0, 0, 0, 0, 1));

addCoordinate (-1, 1, 0, getConstraint(4)); // x

addCoordinate (-2*Math.PI, 2*Math.PI, 0, getConstraint(5)); // theta

}

Six constraints are added using addConstraint(): two linear bilaterals to restrict translation along the and axes of D, two rotary bilaterals to restrict rotation about the and axes of D, and two unilaterals to enforce limits on and $\theta$ . Four of the constraints are constant in frame G, and so are initialized with a wrench value. The other two are not constant in G and so will need to be updated in updateConstraints(). The coordinates for and $\theta$ are added at the end, using addCoordinate(), with default joint limits and a reference to the constraint that will enforce the limit.

The implementations for coordinatesToTCD() and TCDToCoordinates() simply use (3.25) to compute ${\bf T}_{CD}$ from the coordinates, or vice versa:

⬇

public void coordinatesToTCD (RigidTransform3d TCD, VectorNd coords) {

double x = coords.get (X_IDX);

double theta = coords.get (THETA_IDX);

TCD.setIdentity();

TCD.p.x = x;

double c = Math.cos (theta);

double s = Math.sin (theta);

TCD.R.m00 = c;

TCD.R.m11 = c;

TCD.R.m01 = -s;

TCD.R.m10 = s;

}

public void TCDToCoordinates (VectorNd coords, RigidTransform3d TGD) {

coords.set (X_IDX, TGD.p.x);

double theta = Math.atan2 (TGD.R.m10, TGD.R.m00);

coords.set (THETA_IDX, getCoordinateInfo(THETA_IDX).nearestAngle (theta));

}

X_IDX and THETA_IDX are constants defining the coordinate indices for and $\theta$ . In TCDToCoordinates(), note the use of the CoordinateInfo method nearestAngle(), as discussed in Section 4.9.4.

Projecting ${\bf T}_{CD}$ onto the error-free ${\bf T}_{GD}$ is done by projectToConstraints(), implemented as follows:

⬇

public void projectToConstraints (

RigidTransform3d TGD, RigidTransform3d TCD, VectorNd coords) {

TGD.R.set (TCD.R);

TGD.R.rotateZDirection (Vector3d.Z_UNIT);

TGD.p.x = TCD.p.x;

TGD.p.y = 0;

TGD.p.z = 0;

if (coords != null) {

TCDToCoordinates (coords, TGD);

}

The translational projection is easy - the y and z components of the translation vector p are simply zeroed out. To project the rotation R, we use its rotateZDirection() method, which applies the shortest rotation aligning its axis with . The residual rotation will be a rotation in the - plane. If coords is non-null and needs to be computed, we simply call TCDToCoordinates().

Lastly, the implementation for projectToConstraints() is as follows:

⬇

public void updateConstraints (

RigidTransform3d TGD, RigidTransform3d TCD, Twist errC,

Twist velGD, boolean updateEngaged) {

RigidBodyConstraint cons = getConstraint(0); // constraint along y

double s = TGD.R.m10; // sin (theta)

double c = TGD.R.m00; // cos (theta)

// constraint wrench along y is constant in D but needs to be

// transformed to G

cons.setWrenchG (s, c, 0, 0, 0, 0);

// derivative term:

double dotTheta = velGD.w.z;

cons.setDotWrenchG (c*dotTheta, -s*dotTheta, 0, 0, 0, 0);

// update x limit constraint if necessary

cons = getConstraint(4);

if (cons.getEngaged() != 0) {

// constraint wrench along x, transformed to G, is (-c, s, 0)

cons.setWrenchG (c, -s, 0, 0, 0, 0);

cons.setDotWrenchG (-s*dotTheta, -c*dotTheta, 0, 0, 0, 0);

}

// theta limit constraint is constant; no need to do anything

}

Only constraints 0 and 4 need to have their wrenches updated, since the rest are constant, and we obtain their constraint objects using getConstraint(idx). Constraint 0 restricts motion along the axis in D, and while this is constant in D, it is not constant in G, which is where the wrench must be situated. The axis of D as seen in G is the given by the second row of the rotation matrix of ${\bf T}_{GD}$ , which from (3.25) we see is $(s,c,0)^{T}$ , where $s\equiv\sin(\theta)$ and $c\equiv\cos(\theta)$ . We obtain and directly from TGD, since this has been projected to lie on the constraint surface; alternatively, we could compute them from $\theta$ . To obtain the wrench derivative, we note that $\dot{s}=c\dot{\theta}$ and $\dot{c}=-c\dot{\theta}$ , and that $\dot{\theta}$ is simply the component of the angular velocity of with respect to , or velGD.w.z. The wrench and its derivative are set using the constraint’s setWrenchG() and setDotWrenchG() methods.

The other non-constant constraint is the limit constraint for the coordinate, which is the axis of D as seen in G. This is updated similarly, although we only need to do so if the limit constraint is engaged. Since all unilateral constraints are coordinate limits, there is no need to update their distance or engaged attributes as this is done automatically by the system.

4.8 General component arrangements Bibliography 5 Simulation Control

Generated on Sun Jan 12 23:00:57 2025 by LaTeXML [LOGO]