3 Mechanical Models I 3.2 Rigid bodies 3.4 Joint components

3.3 Joints and connectors

In a typical mechanical model, many of the rigid bodies are interconnected, either using spring-type components that exert binding forces on the bodies, or through joints and connectors that enforce the connection using hard constraints. This section describes the latter. While the discussion focuses on rigid bodies, joints and connectors can be used more generally with any body that implements the ConnectableBody interface. In particular, this allows joints to also interconnect finite element models, as described in Section 6.6.2.

3.3.1 Joints and coordinate frames

Consider two rigid bodies A and B. The pose of body B with respect to body A can be described by the 6 DOF rigid transform ${\bf T}_{BA}$ . If A and B are unconnected, ${\bf T}_{BA}$ may assume any possible value and has a full six degrees of freedom. A joint between A and B constrains the set of poses that are possible between the two bodies and reduces the degrees of freedom available to ${\bf T}_{BA}$ . For ease of use, the constraining action of a joint is described with respect to a pair of local coordinate frames C and D that are connected to frames A and B, respectively, by auxiliary transformations. This allows joints to be placed at locations that do not correspond directly to frames A or B.

Figure 3.8: Coordinate frames D and C for a hinge joint.

The joint frames C and D move with respect to each other as the joint moves. The allowed joint motions therefore correspond to the allowed values of the joint transform ${\bf T}_{CD}$ . Although both frames typically move with their attached bodies, D is considered the base frame and C the motion frame (this is because when a joint is used to connect a single body to ground, body B is set to null and the world frame takes its place). As an example of a joint’s constraining effect, consider a hinge joint (Figure 3.8), which allows C to move with respect to D only by rotating about the axis while the origins of C and D remain coincident. Other motions are prohibited. If we let $\theta$ describe the counter-clockwise rotation angle of C about the axis, then ${\bf T}_{CD}$ should always have the form

${\bf T}_{CD}=\left(\begin{matrix}\cos(\theta)&-\sin(\theta)&0&0\\ \sin(\theta)&\cos(\theta)&0&0\\ 0&0&1&0\\ 0&0&0&1\end{matrix}\right).$

(3.7)

Figure 3.9: Transforms connecting joint coordinate frames C and D with rigid bodies A and B.

When a joint is attached to bodies A and B, frame C is fixed to body A and frame D is fixed to body B. Except in special cases, the joint frames C and D are not coincident with the body frames A and B. Instead, they are located relative to A and B by the transforms ${\bf T}_{CA}$ and ${\bf T}_{DB}$ , respectively (Figure 3.9). Since ${\bf T}_{CA}$ and ${\bf T}_{DB}$ are both fixed, the joint constraints on ${\bf T}_{CD}$ constrain the relative poses of A and B, with ${\bf T}_{AB}$ determined from

${\bf T}_{AB}={\bf T}_{DB}\,{\bf T}_{CD}\,{\bf T}_{CA}^{-1}.$

(3.8)

(See Section A.2 for a discussion of determining transforms between related coordinate frames).

3.3.2 Joint coordinates, constraints, and errors

Each different joint and connector type restricts the motion between two bodies to degrees of freedom, for some . Sometimes, the joint also defines a set of coordinates that parameterize these DOFs. For example, the hinge joint described above is parameterized by $\theta$ . Other examples are given in Section 3.4: a 2 DOF cylindrical has coordinates and $\theta$ , a 3 DOF gimbal joint is parameterized by the roll-pitch-yaw angles $\theta$ , $\phi$ , and $\psi$ , etc. When ${\bf T}_{CD}={\bf I}$ (where ${\bf I}$ is the identity transform), the coordinates are usually all equal to zero, and the joint is said to be in the zero state.

As explained in Section 1.2, ArtiSynth uses a full coordinate formulation for dynamic simulation. That means that instead of using joint coordinates to describe system state, it uses the combined full coordinates ${\bf q}$ of all dynamic components. For example, a model consisting of a single rigid body connected to ground by a hinge joint will have 6 DOF (corresponding to the 6 DOF of the body), rather than the 1 DOF implied by the hinge joint. The DOF restrictions imposed by the joints are then enforced by a set of linearized constraint relationships

${\bf G}({\bf q}){\bf u}={\bf g},\qquad{\bf N}({\bf q}){\bf u}\geq{\bf n}$

(3.9)

that restrict the body velocities ${\bf u}$ computed at each simulation step, usually by solving an MLCP like (1.6). As explained in Section 1.2, the right side vectors ${\bf g}$ and ${\bf n}$ in (3.9) contain time derivative terms, which for simplicity much of the following presentation will assume to be 0.

Each joint contributes its own set of constraint equations to (3.9). Typically these take the form of bilateral, or equality, constraints

${\bf G}_{\text{J}}({\bf q}){\bf u}=0$

(3.10)

which are added to the system’s global bilateral constraint matrix ${\bf G}$ . ${\bf G}_{\text{J}}$ contains rows providing individual constraints ${\bf G}_{k}$ . During simulation, these give rise to constraint forces (corresponding to $\boldsymbol{\lambda}$ in (1.8)) which enforce the constraints.

In some cases, the joint also maintains unilateral, or inequality constraints, to keep ${\bf T}_{CD}$ out of inadmissible regions. These take the form

${\bf N}_{\text{J}}({\bf q}){\bf u}\geq 0$

(3.11)

and are added to the system’s global unilateral constraint matrix ${\bf N}$ . They give rise to constraint forces corresponding to $\boldsymbol{\theta}$ in (1.8). A common use of unilateral constraints is to enforce range limits of the joint coordinates (Section 3.3.5), such as

$\theta_{\text{min}}\leq\theta\leq\theta_{\text{max}}.$

(3.12)

A specific unilateral constraint is added to ${\bf N}_{\text{J}}$ only when ${\bf T}_{CD}$ is on or within the boundary of the inadmissible region associated with that constraint. The constraint is then said to be engaged. The combined number of bilateral and engaged unilateral constraints for a particular joint should not exceed 6; otherwise, the joint would be overconstrained.

Joint coordinates, when supported for a particular joint, can be both read and set. Setting a coordinate causes the joint transform ${\bf T}_{CD}$ to change. To accommodate this, the system adjusts the poses of one or both bodies connected to the joint, along with adjacent bodies connected to them, with preference given to bodies that are not attached to “ground”. However, if this is done during simulation, and particularly if one or both of the bodies connected to the joint are moving dynamically, the results will be unpredictable and will likely conflict with the simulation.

Joint coordinates are also often exported as properties. For example, the HingeJoint class (Section 3.4) exports its $\theta$ coordinate as the property theta, which can be accessed in the GUI, or via the accessor methods

⬇

double getTheta() // get theta in degrees

void setTheta (double deg) // set theta in degrees

Since joint constraints are generally nonlinear, their linearized enforcement at the velocity level by (3.9) will usually produce small errors as the simulation proceeds. These errors are reduced using a position correction step described in Section 4.9.1 and [11]. Errors can also be caused by joint compliance (Section 3.3.8). Both effects mean that the joint transform ${\bf T}_{CD}$ may deviate from the allowed values dictated by the joint type. In ArtiSynth, this is accounted for by introducing an additional constraint frame G between D and C (Figure 3.10). G is computed to be the nearest frame to C that lies exactly in the joint constraint space. ${\bf T}_{GD}$ is therefore a valid joint transform, ${\bf T}_{GC}$ accommodates the error, and the whole joint transform is given by the composition

${\bf T}_{CD}={\bf T}_{GD}\;{\bf T}_{CG}.$

(3.13)

If there is no compliance or joint error, then frames G and C are identical, ${\bf T}_{CG}={\bf I}$ , and ${\bf T}_{CD}={\bf T}_{GD}$ . Because ${\bf T}_{CG}$ describes the joint error, we sometimes refer to it as ${\bf T}_{err}={\bf T}_{CG}$ .

Figure 3.10: 2D schematic showing the joint frames D and C, along with the intermediate frame G that accounts for numeric error and complaint motion.

3.3.3 Creating joints

Joint and connector components in ArtiSynth are both derived from the superclass BodyConnector, with joints being further derived from JointBase, which provides support for coordinates. Some of the commonly used joints and connectors are described in Section 3.4.

An application creates a joint by constructing it and adding it to a MechModel. Many joints have constructors of the form

⬇

XXXJoint (bodyA, bodyB, TDW)

which specifies the bodies A and B which the joint connects, along with the transform ${\bf T}_{DW}$ giving the pose of the joint base frame D in world coordinates. The constructor then assumes that the joint is in the zero state, so that C and D are the same and ${\bf T}_{CD}={\bf I}$ and ${\bf T}_{CW}={\bf T}_{DW}$ , and then computes ${\bf T}_{CA}$ and ${\bf T}_{DB}$ from

	$\displaystyle{\bf T}_{CA}$	$\displaystyle={\bf T}_{AW}^{-1}\;{\bf T}_{CW}$		(3.14)
	$\displaystyle{\bf T}_{DB}$	$\displaystyle={\bf T}_{BW}^{-1}\;{\bf T}_{DW},$		(3.15)

where ${\bf T}_{AW}$ and ${\bf T}_{BW}$ are the current poses of A and B.

After the joint is created, it should be added to the system’s MechModel using addBodyConnector(), as shown in the following code fragment:

⬇

MechModel mech;

RigidBody bodyA, bodyB;

RigidTransform3d TDW;

... initialize mech, bodyA, bodyB, and TDW ...

HingeJoint joint = new HingeJoint (bodyA, bodyB, TDW);

mech.addBodyConnector (joint);

It is also possible to create a joint using its default constructor and attach it to the bodies afterward, using the method setBodies(bodyA,bodyB,TDW), as in the following:

⬇

HingeJoint joint = new HingeJoint ();

joint.setBodies (bodyA, bodyB, TDW);

mech.addBodyConnector (joint);

One reason for doing this is that it allows the joint transform ${\bf T}_{CD}$ to be modified (by setting coordinate values) before setBodies() is called; this is discussed further in Section 3.3.4.

Joints usually offer a number of other constructors that let its world location and body relationships to be specified in different ways. These may include:

⬇

XXXJoint (bodyA, TCA, bodyB, TDB)

XXXJoint (bodyA, bodyB, TCW, TDW)

The first, which is restricted to rigid bodies, allows the application to explicitly specify transforms ${\bf T}_{CA}$ and ${\bf T}_{DB}$ connecting frames C and D to the body frames A and B, and is useful when ${\bf T}_{CA}$ and ${\bf T}_{DB}$ are explicitly known, or the initial value of ${\bf T}_{CD}$ is not the identity. Likewise, the second constructor allows ${\bf T}_{CW}$ and ${\bf T}_{DW}$ to be explicitly specified, with ${\bf T}_{CD}\neq{\bf I}$ if ${\bf T}_{CW}\neq{\bf T}_{DW}$ . For instance, suppose ${\bf T}_{CD}$ and ${\bf T}_{DW}$ are both known. Then we can use the relationship

${\bf T}_{CW}={\bf T}_{DW}\,{\bf T}_{CD}$

(3.16)

to create the joint as in the following code fragment:

⬇

MechModel mech;

RigidBody bodyA, bodyB;

RigidTransform3d TDW, TCD;

... initialize mech, bodyA, bodyB, TDW, and TCD ...

// compute TCW:

RigidTransform3d TCW = new RigidTransform3d();

TCW.mul (TDW, TCD);

HingeJoint joint = new HingeJoint (bodyA, bodyB, TCW, TDW);

mech.addBodyConnector (joint);

As an alternative to specifying ${\bf T}_{DW}$ or its equivalents, some joint types provide constructors that let the application locate specific joint features. These may be easier to use in some cases. For instance, HingeJoint provides a constructor

⬇

HingeJoint (bodyA, bodyB, originD, zaxis)

that specifies origin of D and its axis (which is the rotation axis), with the remaining orientation of D aligned as closely as possible with the world. SphericalJoint provides a constructor

⬇

SphericalJoint (bodyA, bodyB, originD)

that specifies origin of D and aligns its orientation with the world. Users should consult the source code or API documentation for specific joints to see what special constructors may be available.

Finally, it is possible to use joints to connect a single body to ground (by convention, this is the A body). Most joints provide a constructor of the form

⬇

XXXJoint (bodyA, TDW)

which allows this to be done explicitly. Alternatively, most joint constructors which supply body B will allow this to be specified as null, so that body A will be connected to ground by default.

3.3.4 Working with coordinates

As mentioned in Section 3.3.2, some joints support coordinates that parameterize the valid motions within the joint transform ${\bf T}_{CD}$ . All such joints are subclasses of JointBase, which provides some generic methods for querying and setting coordinate values (JointBase is in turn a subclass of BodyConnector).

The number of coordinates is returned by the method numCoordinates(); if this returns 0, then coordinates are not supported. Each coordinate has an index in the range $0\ldots M-1$ , where is the number of coordinates. Coordinate values can be queried or set using the following methods:

⬇

getCoordinate (int idx) // get coordinate value with index idx

getCoordinates (VectorNd coords) // get all coordinates values

setCoordinate (int idx, double value) // set coordinate value with index idx

setCoordinates (VectorNd values) // set all coordinates values

Specific joint types usually also provide names for their joint coordinates, along with integer constants describing their indices and methods for accessing their values. For example, CylindricalJoint supports two coordinates, and $\theta$ , along with the following:

⬇

// coordinate indices

static final int Z_IDX = 0;

static final int THETA_IDX = 1;

// set/get z value and range

double getZ()

void setZ (double z)

// set/get theta value and range in degrees

double getTheta()

void setTheta (double theta)

The coordinate values are also exported as the properties z and theta, allowing them to be set in the GUI. For convenience, particularly in GUI applications, the properties and methods for controlling specific angular coordinates generally use degrees instead of radians.

As discussed in Section 3.3.2, unlike in some multibody simulation systems (such as OpenSim), joint coordinates are not fundamental quantities that describe system state. As such, then, coordinates can usually only be set in specific circumstances that avoid simulation conflicts. In general, when joint coordinates are set, the system adjusts the poses of one or both bodies connected to this joint, along with adjacent bodies connected to them, with preference given to bodies that are not attached to “ground”. However, if this is done during simulation, and particularly if one or both of the bodies connected to the joint are moving dynamically, the results will be unpredictable and will likely conflict with the simulation.

If a joint has been created with its default constructor and not yet attached to any bodies, then setting joint values will simply set the joint transform ${\bf T}_{CD}$ . This can be useful in situations where one needs to initialize a joint’s ${\bf T}_{CD}$ to a non-identity value corresponding to a particular set of joint coordinates:

⬇

RigidTransform3d TDW; // known location for D frame

double z, theta; // desired initial coordinate values

...

CylindricalJoint joint = new CylindricalJoint();

joint.setZ (z);

joint.setTheta (thetaDeg);

joint.setBodies (bodyA, bodyB, TDW);

This can also be done in vector form:

⬇

RigidTransform3d TDW; // known location for D frame

VectorNd coordValues; // desired initial coordinate values

...

CylindricalJoint joint = new CylindricalJoint();

joint.setCoordinates (coordValues);

joint.setBodies (bodyA, bodyB, TDW);

In either of these cases, setBodies() will not use ${\bf T}_{CD}={\bf I}$ but instead use the value determined by the initial coordinate values.

To determine the ${\bf T}_{CD}$ corresponding to a particular set of coordinates, one may use the method

⬇

void coordinatesToTCD (RigidTransform3d TCD, VectorNd coords)

In some cases, within a model’s build() method, one may wish to set initial coordinates after a joint has been attached to its bodies, in order to move those bodies (along with the bodies attached to them) into an initial configuration without having to explicitly calculate the poses from the joint coordinates. As mentioned above, the system will make a decision about which attached bodies are most “free” and adjust their poses accordingly. This is done in the example of the next section.

3.3.5 Coordinate limits and locking

It is possible to set limits on a joint coordinate’s range, and also to lock a coordinate in place at its current value.

When a joint coordinate hits either an upper or lower range limit, a unilateral constraint is invoked to prevent it from violating the limit, and remains engaged until the joint moves away from the limit. Each range constraint that is engaged reduces the number of joint DOFs by one.

By default, joint range limits are usually disabled (i.e., they are set to $(-\inf,\inf)$ ). They can be queried and set, for a given joint with index idx, using the methods:

⬇

DoubleInterval getCoordinateRange (int idx)

double getMinCoordinate (int idx)

double getMaxCoordinate (int idx)

void setCoordinateRange (idx, DoubleInterval rng)

where range limits for angular coordinates are specified in radians. For convenience, the following methods are also provided which use degrees instead of radians for angular coordinates:

⬇

DoubleInterval getCoordinateRangeDeg (int idx)

double getMinCoordinateDeg (int idx)

double getMaxCoordinateDeg (int idx)

void setCoordinateRangeDeg (idx, DoubleInterval rng)

Range checking can be disabled by setting the range to $(-\inf,\inf)$ , or by specifying rng as null, which implicitly does the same thing.

Ranges for angular coordinates are not limited to $\pm 180^{\circ}$ but can instead be set to larger values; the joint will continue to wrap until the limit is reached.

Joint coordinates can also be locked, so that they hold their current value and don’t move. A joint is locked using a bilateral constraint that prevents motion in either direction and reduces the joint’s DOF count by one. The following methods are available for querying or setting a coordinate’s locked status:

⬇

boolean isLocked (int idx)

void setLocked (int idx, boolean locked)

As with coordinate values, specific joint types usually provide methods for controlling the ranges and locking status of individual coordinates, with ranges for angular coordinates specified in degrees instead of radians. For example, CylindricalJoint supplies the methods

⬇

// set/get z range

DoubleInterval getZRange()

void setZRange (double min, double max)

// control z locking

boolean isZLocked()

void setZLocked (boolean locked)

// set/get theta range in degrees

DoubleInterval getThetaRange()

void setThetaRange (double min, double max)

void setThetaRange (DoubleInterval rng)

// control theta locking

boolean isThetaLocked()

void setThetaLocked (boolean locked)

The range and locking information is also exported as the properties zRange, thetaRange, zLocked, and thetaLocked, allowing them to be set in the GUI.

3.3.6 Example: a simple hinge joint

Figure 3.11: RigidBodyJoint model loaded into ArtiSynth.

A simple model showing two rigid bodies connected by a joint is defined in

  artisynth.demos.tutorial.RigidBodyJoint

The build method for this model is given below:

⬇

1 public void build (String[] args) {

3 // create MechModel and add to RootModel

4 mech = new MechModel ("mech");

5 mech.setGravity (0, 0, -98);

6 mech.setFrameDamping (1.0);

7 mech.setRotaryDamping (4.0);

8 addModel (mech);

10 PolygonalMesh mesh; // bodies will be defined using a mesh

12 // create first body and set its pose

13 mesh = MeshFactory.createRoundedBox (lenx1, leny1, lenz1, /*nslices=*/8);

14 RigidTransform3d TMB =

15 new RigidTransform3d (0, 0, 0, /*axisAng=*/1, 1, 1, 2*Math.PI/3);

16 mesh.transform (TMB);

17 bodyB = RigidBody.createFromMesh ("bodyB", mesh, /*density=*/0.2, 1.0);

18 bodyB.setPose (new RigidTransform3d (0, 0, 1.5*lenx1, 1, 0, 0, Math.PI/2));

19 bodyB.setDynamic (false);

21 // create second body and set its pose

22 mesh = MeshFactory.createRoundedCylinder (

23 leny2/2, lenx2, /*nslices=*/16, /*nsegs=*/1, /*flatBottom=*/false);

24 mesh.transform (TMB);

25 bodyA = RigidBody.createFromMesh ("bodyA", mesh, 0.2, 1.0);

26 bodyA.setPose (new RigidTransform3d (

27 (lenx1+lenx2)/2, 0, 1.5*lenx1, 1, 0, 0, Math.PI/2));

29 // create the joint

30 RigidTransform3d TDW =

31 new RigidTransform3d (lenx1/2, 0, 1.5*lenx1, 1, 0, 0, Math.PI/2);

32 HingeJoint joint = new HingeJoint (bodyA, bodyB, TDW);

34 // add components to the mech model

35 mech.addRigidBody (bodyB);

36 mech.addRigidBody (bodyA);

37 mech.addBodyConnector (joint);

39 joint.setTheta (35); // set joint position

41 // set render properties for components

42 RenderProps.setFaceColor (joint, Color.BLUE);

43 joint.setShaftLength (4);

44 joint.setShaftRadius (0.2);

45 }

A MechModel is created as usual at line 4. However, in this example, we also set some parameters for it: setGravity() is used to set the gravity acceleration vector to $(0,0,-98)^{T}$ instead of the default value of $(0,0,-9.8)^{T}$ , and the frameDamping and rotaryDamping properties (Section 3.2.7) are set to provide appropriate damping.

Each of the two rigid bodies are created from a mesh and a density. The meshes themselves are created using the factory methods MeshFactory.createRoundedBox() and MeshFactory.createRoundedCylinder() (lines 13 and 22), and then RigidBody.createFromMesh() is used to turn these into rigid bodies with a density of 0.2 (lines 17 and 25). The pose of the two bodies is set using RigidTransform3d objects created with x, y, z translation and axis-angle orientation values (lines 18 and 26).

The hinge joint is implemented using HingeJoint, which is constructed at line 32 with the joint coordinate frame D being located in world coordinates by TDW as described in Section 3.3.3.

Once the joint is created and added to the MechModel, the method setTheta() is used to explicitly set the joint parameter to 35 degrees. The joint transform ${\bf T}_{CD}$ is then set appropriately and bodyA is moved to accommodate this (bodyA being chosen since it is the most free to move).

Finally, joint rendering properties are set starting at line 42. We render the joint as a cylindrical shaft about the rotation axis, using its shaftLength and shaftRadius properties. Joint rendering is discussed in more detail in Section 3.3.10).

To run this example in ArtiSynth, select All demos > tutorial > RigidBodyJoint from the Models menu. The model should load and initially appear as in Figure 3.11. Running the model (Section 1.5.3) will cause bodyA to fall and swing under gravity.

3.3.7 Constraint forces

During each simulation solve step, the joint velocity constraints described by (3.10) and (3.11) are enforced by bilateral and unilateral constraint forces ${\bf f}_{g}$ and ${\bf f}_{n}$ :

${\bf f}_{g}={\bf G}_{\text{J}}^{T}\boldsymbol{\lambda}_{J},\quad{\bf f}_{n}={% \bf N}_{\text{J}}^{T}\boldsymbol{\theta}_{J}.$

(3.17)

Here, ${\bf f}_{g}$ and ${\bf f}_{n}$ are spatial forces (or wrenches, Section A.5) acting in the joint coordinate frame C, and $\boldsymbol{\lambda}_{J}$ and $\boldsymbol{\theta}_{J}$ are the Lagrange multipliers computed as part of the mechanical system solve (see (1.6) and (1.8)). The sizes of $\boldsymbol{\lambda}_{J}$ and $\boldsymbol{\theta}_{J}$ equal the number of bilateral and engaged unilateral constraints in the joint; these numbers can be queried for a particular joint using the methods numBilateralConstraints() and numEngagedUnilateralConstraints(). (The number of engaged unilateral constraints may be less than the total number of unilateral constraints; the latter may be queried with numUnilateralConstraints(), while the total number of constraints is returned by numConstraints().

Applications may sometimes need to query the current constraint force values, typically from within a controller or monitor (Section 5.3). The Lagrange multipliers themselves may be obtained with

⬇

void getBilateralForces (VectorNd lam)

void getUnilateralForces (VectorNd the)

which load the multipliers into lam or the and set their sizes to the number of bilateral or engaged unilateral constraints. Alternatively, one can retrieve the individual multiplier for the constraint indexed by idx using

⬇

double getConstraintForce (int idx);

Typically, it is more useful to find the spatial constraint forces ${\bf f}_{g}$ and ${\bf f}_{n}$ , which can be obtained with respect to frame C:

⬇

// place the forces in the wrench f

void getBilateralForcesInC (Wrench f)

void getUnilateralForcesInC (Wrench f)

// convenience methods that allocate the wrench and return it

Wrench getBilateralForcesInC();

Wrench getUnilateralForcesInC();

If the attached bodies A and B are rigid bodies, it is also possible to obtain the constraint wrenches experienced by those bodies:

⬇

// place the forces in the wrench f

void getBilateralForcesInA (Wrench f)

void getUnilateralForcesInA (Wrench f)

void getBilateralForcesInB (Wrench f)

void getUnilateralForcesInB (Wrench f)

// convenience methods that allocate the wrench and return it

Wrench getBilateralForcesInA();

Wrench getUnilateralForcesInA();

Wrench getBilateralForcesInB();

Wrench getUnilateralForcesInB();

Constraint wrenches obtained for bodies A or B are given in world coordinates, which is consistent with the forces reported by rigid bodies via their getForce() method. To orient the forces into body coordinates, one may use the inverse of the rotation matrix ${\bf R}$ of the body’s pose. For example:

⬇

RigidBody bodyA;

// ... body A initialized, etc. ...

Wrench force = joint.getBilateralForceInA();

force.inverseTransform (bodyA.getPose().R);

3.3.8 Compliance and regularization

By default, the constraints used to implement joints and couplings are treated as hard, so that the system tries to respect the constraint conditions (3.9) as exactly as possible as the simulation proceeds. Sometimes, however, it is desirable to introduce some “softness” into the constraints, whereby constraint forces are determined as a linear function of their distance from the constraint. Adding compliance also allows an application to regularize a system of joint constraints that would otherwise be overconstrained, as illustrated in Section 3.3.9.

To describe compliance precisely, consider the bilateral constraint portion of the MLCP in (1.6), which solves for the updated system velocities ${\bf u}^{k+1}$ at each time step:

$\left(\begin{matrix}\hat{\bf M}^{k}&-{\bf G}^{T}\\ {\bf G}&0\end{matrix}\right)\left(\begin{matrix}{\bf u}^{k+1}\\ \tilde{\boldsymbol{\lambda}}\end{matrix}\right)=\left(\begin{matrix}{\bf M}{% \bf u}^{k}-h\hat{\bf f}^{k}\\ 0\end{matrix}\right).$

(3.18)

Here ${\bf G}$ is the system’s bilateral constraint matrix, $\tilde{\boldsymbol{\lambda}}$ denotes the constraint impulses (from which the constraint forces $\boldsymbol{\lambda}$ can be determined by $\boldsymbol{\lambda}=\tilde{\boldsymbol{\lambda}}/h$ ), and for simplicity we have assumed that ${\bf G}$ is constant and so the ${\bf g}$ term on the lower right side is .

Solving (3.18) results in constraint forces that satisfy ${\bf G}{\bf u}^{k+1}=0$ precisely, corresponding to hard constraints. To implement soft constraints, start by defining a function $\boldsymbol{\phi}({\bf q})$ that defines the distances from each constraint, where ${\bf q}$ is the vector of system positions; these distances are the local translational and rotational deviations from each constraint’s correct position and are discussed in more detail in Section 4.9.1. Then assume that the constraint forces are a linear function of these distances:

$\boldsymbol{\lambda}=-{\bf C}^{-1}\boldsymbol{\phi}({\bf q}),$

(3.19)

where ${\bf C}$ is a diagonal compliance matrix that is equivalent to an inverse stiffness matrix. We also note that $\boldsymbol{\phi}$ will be time varying, and that we can approximate its change between time steps as

$\boldsymbol{\phi}^{k+1}\approx\boldsymbol{\phi}^{k}+h\dot{\boldsymbol{\phi}}^{% k+1},\quad\text{with}\quad\dot{\boldsymbol{\phi}}^{k+1}={\bf G}{\bf u}^{k+1}.$

(3.20)

Next, assume that in using (3.19) to determine $\boldsymbol{\lambda}$ for a particular time step, we use the average value of $\boldsymbol{\phi}$ over the step, represented by $\bar{\boldsymbol{\phi}}=(\boldsymbol{\phi}^{k+1}+\boldsymbol{\phi}^{k})/2$ . Substituting this and (3.20) into (3.19), multiplying by ${\bf C}$ , and rearranging yields:

${\bf G}{\bf u}^{k+1}+\frac{2{\bf C}}{h}\boldsymbol{\lambda}=-\frac{2}{h}% \boldsymbol{\phi}^{k}.$

(3.21)

Then noting that $\tilde{\boldsymbol{\lambda}}=h\boldsymbol{\lambda}$ , we obtain a revised form of (3.18),

$\left(\begin{matrix}\hat{\bf M}^{k}&-{\bf G}^{T}\\ {\bf G}&2{\bf C}/h^{2}\end{matrix}\right)\left(\begin{matrix}{\bf u}^{k+1}\\ \tilde{\boldsymbol{\lambda}}\end{matrix}\right)=\left(\begin{matrix}{\bf M}{% \bf u}^{k}-h\hat{\bf f}^{k}\\ -2\boldsymbol{\phi}^{k}/h\end{matrix}\right),$

(3.22)

in the which the zeros in the matrix and right hand side have been replaced by compliance terms. The resulting constraint behavior is different from that of (3.18) in two important ways:

1.

The joint now allows 6 DOF, with motion along the constrained directions limited by restoring spring constants given by the reciprocals of the diagonal entries of ${\bf C}$ .
2.

If ${\bf C}$ has no zero diagonal entries, then the system (3.22) is regularized by the $2{\bf C}/h^{2}$ term in the lower right matrix block. This means that the matrix is always non-singular, even if ${\bf G}$ is rank deficient, and so compliance offers a way to handle overconstrained models, as discussed further in Section 3.3.9.

Unilateral constraints can be regularized using the same approach, with a distance function defined such that $\boldsymbol{\phi}({\bf q})\leq 0$ .

The reason for specifying soft constraints using compliance instead of stiffness is that by setting ${\bf C}=0$ we can easily handle the case of infinite stiffness where the constraints are strictly enforced. The ArtiSynth compliance implementation uses a slightly more complex version of (3.22) that accounts for non-constant ${\bf G}$ and also allows for a damping term $-{\bf D}\dot{\boldsymbol{\phi}}$ , where ${\bf D}$ is again a diagonal matrix. For more details, see [9] and [21].

When using compliance, damping is often needed for stability, and, in the case of unilateral constraints, to prevent “bouncing”. A good choice for damping is usually critical damping, which is discussed further below.

Any joint which is a subclass of BodyConnector allows individual compliance values $C_{i}$ and damping values $D_{i}$ to be set for each of the joint’s constraints. These values comprise the diagonal entries in the compliance and damping matrices ${\bf C}$ and ${\bf D}$ , and can be queried and set using the methods

⬇

VectorNd getCompliance()

void setCompliance (VectorNd compliance)

VectorNd getDamping()

void setCompliance (VectorNd damping)

The vectors supplied to the above set methods contain the requested compliance or damping values. If their size is less than numConstraints(), then compliance or damping will be set for the first constraints. Damping for a specific constraint only has an effect if the compliance for that constraint is nonzero.

What compliance and damping values should be specified? Compliance is usually relatively easy to figure out. Each of the joint’s individual constraints corresponds to a row in its bilateral constraint matrix ${\bf G}_{\text{J}}$ or unilateral constraint matrix ${\bf N}_{\text{J}}$ , and represents a specific 6 DOF direction along which the spatial velocity $\hat{\bf v}_{CD}$ (of frame C with respect to D) is restricted (more details on this are given in Section 4.9.1). Each of these constraint directions is usually predominantly linear or rotational; specific descriptions for the constraints of different joints are provided in Section 3.4. To determine compliance for a constraint , estimate the typical force likely to act along its direction, decide how much displacement $\delta q$ (translational or rotational) along that constraint is desirable, and then set the compliance $C_{i}$ to the associated inverse stiffness:

$C_{k}=\delta q/f.$

(3.23)

Once $C_{k}$ is determined, the damping $D_{k}$ can be estimated based on the desired damping ratio $\zeta$ , using the formula

$D_{k}=2\zeta\sqrt{M/C_{k}}$

(3.24)

where is total mass of the bodies attached to the joint. Typically, the desired damping will be close to critical damping, for which $\zeta=1$ .

Constraints associated with linear motion will typically require different compliance values from those associated with rotation. To make this process easier, joint components allow the setting of collective compliance values for their linear and rotary constraints, using the methods

⬇

void setLinearCompliance (double c)

double getLinearCompliance()

void setRotaryCompliance (double c)

double getRotaryCompliance()

The set() methods will set a uniform compliance for all linear or rotary constraints, except for unilateral constraints associated with coordinate limits. At the same time, they will also set an automatically computed critical damping value. Likewise, the get() methods query these linear or rotary constraints for uniform compliance values (with the corresponding critical damping), and return either that value, or -1 if it does not exist.

Most of the demonstration models for the joints described in Section 3.4 allow these linear and rotary compliance settings to be adjusted interactively using a control panel, enabling users to experimentally gain a feel for their behavior.

To determine programmatically whether a particular constraint is linear or rotary, one can use the joint method

⬇

VectorNi getConstraintFlags()

which returns a vector of information flags for all its constraints. Linear and rotary constraints are indicated by the flags LINEAR and ROTARY, defined in RigidBodyConstraint.

3.3.9 Example: an overconstrained linkage

Situations may occasionally arise in which a model is overconstrained, which means that the rows of the bilateral constraint matrix ${\bf G}$ in (3.9) are not all linearly dependent, or in other words, ${\bf G}$ does not have full row rank. At present, the ArtiSynth solver has difficultly handling overconstrained models, but these situations can often be handled by adding a small amount of compliance to the constraints. (Overconstraining is not a problem with unilateral constraints ${\bf N}$ , because of the way they are handled by the solver.)

One possible symptom of an overconstrained system is a error message in the application’s terminal output, such as

Pardiso: num perturbed pivots=12

Overconstraining frequently occurs in closed-chain linkages, involving loops in which a jointed sequence of links is connected back on itself. Depending on how the constraints are configured and how redundant they are, the system may still be able to move. A classical example is the four-bar linkage, a common version of which consists of four links, or “bars”, arranged as a parallelogram and connected by hinge joints at the corners. One link is usually connected to ground, and so the remaining three links together have 18 DOF, while the four hinge joints together remove 20 DOF, overconstraining the system. However, the constraints are redundant in such as way that the linkage still actually has 1 DOF.

Figure 3.12: FourBarLinkage model, several steps into the simulation.

To model a four-bar in ArtiSynth presently requires adding compliance to the hinge joints. An example of this is defined by the demo program

  artisynth.demos.tutorial.FourBarLinkage

shown in Figure 3.12. The code for the build() method and a couple of supporting methods is given below:

⬇

1 /**

2 * Create a link with a length of 1.0, width of 0.25, and specified depth

3 * and add it to the mech model. The parameters x, z, and deg specify the

4 * link’s position and orientation (in degrees) in the x-z plane.

5 */

6 protected RigidBody createLink (

7 MechModel mech, String name,

8 double depth, double x, double z, double deg) {

9 int nslices = 20; // num slices on the rounded mesh ends

10 PolygonalMesh mesh =

11 MeshFactory.createRoundedBox (1.0, 0.25, depth, nslices);

12 RigidBody body = RigidBody.createFromMesh (

13 name, mesh, /*density=*/1000.0, /*scale=*/1.0);

14 body.setPose (new RigidTransform3d (x, 0, z, 0, Math.toRadians(deg), 0));

15 mech.addRigidBody (body);

16 return body;

17 }

19 /**

20 * Create a hinge joint connecting one end of link0 with the other end of

21 * link1, and add it to the mech model.

22 */

23 protected HingeJoint createJoint (

24 MechModel mech, String name, RigidBody link0, RigidBody link1) {

25 // easier to locate the link using TCA and TDB since we know where frames

26 // C and D are with respect the link0 and link1

27 RigidTransform3d TCA = new RigidTransform3d (0, 0, 0.5, 0, 0, Math.PI/2);

28 RigidTransform3d TDB = new RigidTransform3d (0, 0, -0.5, 0, 0, Math.PI/2);

29 HingeJoint joint = new HingeJoint (link0, TCA, link1, TDB);

30 joint.setName (name);

31 mech.addBodyConnector (joint);

32 // set joint render properties

33 joint.setAxisLength (0.4);

34 RenderProps.setLineRadius (joint, 0.03);

35 return joint;

36 }

38 public void build (String[] args) {

39 // create a mech model and set rigid body damping parameters

40 MechModel mech = new MechModel ("mech");

41 addModel (mech);

42 mech.setFrameDamping (1.0);

43 mech.setRotaryDamping (4.0);

45 // create four ’bars’ from which to construct the linkage

46 RigidBody[] bars = new RigidBody[4];

47 bars[0] = createLink (mech, "link0", 0.2, -0.5, 0.0, 0);

48 bars[1] = createLink (mech, "link1", 0.3, 0.0, 0.5, 90);

49 bars[2] = createLink (mech, "link2", 0.2, 0.5, 0.0, 180);

50 bars[3] = createLink (mech, "link3", 0.3, 0.0, -0.5, 270);

51 // ground the left bar

52 bars[0].setDynamic (false);

54 // connect the bars using four hinge joints

55 HingeJoint[] joints = new HingeJoint[4];

56 joints[0] = createJoint (mech, "joint0", bars[0], bars[1]);

57 joints[1] = createJoint (mech, "joint1", bars[1], bars[2]);

58 joints[2] = createJoint (mech, "joint2", bars[2], bars[3]);

59 joints[3] = createJoint (mech, "joint3", bars[3], bars[0]);

61 // Set uniform compliance and damping for all bilateral constraints,

62 // which are the first 5 constraints of each joint

63 VectorNd compliance = new VectorNd(5);

64 VectorNd damping = new VectorNd(5);

65 for (int i=0; i<5; i++) {

66 compliance.set (i, 0.000001);

67 damping.set (i, 25000);

68 }

69 for (int i=0; i<joints.length; i++) {

70 joints[i].setCompliance (compliance);

71 joints[i].setDamping (damping);

72 }

73 }

Two helper methods are used to construct the model: createLink() (lines 6-17), and createJoint() (lines 23-36). createLink() makes the individual rigid bodies used to build the linkage: a mesh is produced defining the body’s shape (a box with rounded ends), and then passed to the RigidBody createFromMesh() method which creates the body and sets its inertia according to a specified density. The body’s pose is then set so as to center it at while rotating it about the axis by the angle deg (in degrees). The completed body is then added to the MechModel mech and returned.

The second helper method, createJoint(), connects two rigid bodies (link0 and link1) together using a HingeJoint. Because we know the location of the joint in body-relative coordinates, it is easier to create the joint using the transforms ${\bf T}_{CA}$ and ${\bf T}_{DB}$ instead of ${\bf T}_{DW}$ : ${\bf T}_{CA}$ locates the joint at the top end of link0, at , with the axis parallel to the body’s axis, while ${\bf T}_{DB}$ similarly locates the joint at the bottom of link1. After the joint is created and added to the MechModel, its render properties are set so that its axis drawn as a blue cylinder.

The build() method itself begins by creating a MechModel and setting damping parameters for the rigid bodies (lines 40-43). Next, createLink() is used to create and store the four links (lines 46-50), and the left bar is attached to ground by making it non-dynamic (line 52). The links are then connected together using joints created by createJoint() (lines 55-59). Finally, uniform compliance and damping values are set for each of the joint’s bilateral constraints, using the setCompliance() and setDamping() methods (lines 63-72). Values are set for the first five constraints, since for a HingeJoint these are the bilateral constraints. The compliance value of $C=10^{-6}$ was found experimentally to be low enough so as to not cause noticeable deflections in the joints. Given and an average mass of around for each link pair, (3.24) suggests the damping factor of . Note that for this example, very similar settings could be achieved by simply calling

⬇

for (int i=0; i<joints.length; i++) {

joints[i].setLinearCompliance (0.000001);

joints[i].setRotaryCompliance (0.000001);

}

In principle, we only need to set compliance for the constraints that are redundant, but it can sometimes be difficult to determine exactly which these are. Also, different values are often needed for linear and rotary constraints; that is not necessary here because the links have unit length and so the linear and rotary units have similar scales.

3.3.10 Rendering joints

Most joints provide a means to render themselves in order to provide a graphical representation of their position and configuration. Control over this is achieved by setting various properties in the joint component, including both specialized properties and the standard render properties (Section 4.3) used by all renderable components.

All joints which are subclasses of JointBase support rendering of both their C and D coordinate frames, through the properties drawFrameC, drawFrameD, and axisLength. The first two properties are of the type Renderer.AxisDrawStyle (described in detail in Section 3.2.8), and can be set to LINE or ARROW to enable the coordinate axes to be drawn either as lines or solid arrows. The axisLength property has type double and specifies the length with which the axes are drawn. As with all properties, these properties can be set either in the GUI, or in code using accessor methods supplied by the joint:

⬇

void setAxisLength (double l)

double getAxisLength()

void setDrawFrameC (AxisDrawStyle style)

(AxisDrawStyle getDrawFrameC()

void setDrawFrameD (AxisDrawStyle style)

(AxisDrawStyle getDrawFrameD()

Another pair of properties used by several joints is shaftLength and shaftRadius, which specify the length and radius used to draw shaft or axis structures associated with the joint. These are rendered as solid cylinders, using the color indicated by the faceColor rendering property. The default value of both properties is 0; if shaftLength is 0, then the structures are not drawn, while if shaftRadius is 0, a default value proportional to shaftLength is used. For example, to enable rendering of a blue shaft along the rotation axis of a hinge joint, one may use the code fragment

⬇

HingeJoint joint;

...

joint.setShaftLength (0.5); // set shaft dimensions

joint.setShaftRadius (0.05);

RenderProps.setFaceColor (joint, Color.BLUE); // set the color

As another example, to enable rendering of a green ball about the center of a spherical joint, one may use the fragment

⬇

SphericalJoint joint;

...

joint.setJointRadius (0.02); // set the ball size

RenderProps.setFaceColor (joint, Color.GREEN); // set the color

Specific joints may define additional properties to control how they are rendered.

3.2 Rigid bodies Bibliography 3.4 Joint components

Generated on Wed Jul 24 21:23:12 2024 by LaTeXML [LOGO]