10 Inverse Simulation

10.1 Overview

ArtiSynth supports an inverse simulation capability that allows the computation of the excitation signals (for muscles and other actuators) needed to track target trajectories for a set of source components within the model (typically points or frames). This is achieved using a specialized controller component known as a TrackingController, defined in the package

  artisynth.core.inverse

An application model creates the tracking controller and configures it with the source components, the exciter components needed to effect the tracking, and probes or controllers to specify the target trajectories. Then at each simulation time step the controller computes the excitations (i.e., signals to the exciter components) that allow the trajectories to be followed as closely as possible, in a manner conceptually similar to computed muscle control in OpenSim [7].

In earlier versions of ArtiSynth it was necessary to run inverse simulation with hybrid solving disabled. That limitation has been removed.

10.1.1 Tracking controller operation

Let {\bf q}, {\bf u} and {\bf f} be composite vectors describing the positions, velocities and forces of all the components in the application model, and let {\bf a} be a vector of excitation signals for all the excitation components available to the controller. {\bf f} can be decomposed into

{\bf f}={\bf f}_{p}({\bf q},{\bf u})+{\bf f}_{a}({\bf q},{\bf u},{\bf a}) (10.1)

where {\bf f}_{p} are the passive forces corresponding to zero excitation, and {\bf f}_{a} are the active forces. During simulation, the controller computes values for {\bf a} to best track the target trajectories.

In the case of motion tracking, let {\bf x} and {\bf v} be the subvectors of {\bf q} and {\bf u} containing the positions and velocities of the source components, and let {\bf x}_{t} and {\bf v}_{t} be the corresponding target trajectories. At the beginning of each time step, the controller compares the current source velocity state ({\bf x},{\bf v}) with the desired target state ({\bf x}_{t},{\bf v}_{t}) and uses this to determine a desired source velocity {\bf v}_{*} for the next time step; this computation is done by the motion tracker (Figure 10.1, Section 10.1.2). An excitation generator then computes {\bf a} in order to try and make {\bf v} match {\bf v}_{*} (Section 10.1.3).

The target state ({\bf x}_{t},{\bf v}_{t}) is maintained within a set of target objects that the controller creates to mirror each source component (Section 10.2.2), and the target positions and velocities of these objects are typically specified using probes or controllers. Target velocities do not have to be explicitly set; if they are not, then they are either estimated from the position targets (see the property estimateTargetVelocity in Section 10.2.2), or assumed to be 0.

The excitation generator accepts a desired velocity {\bf v}_{*} instead of a desired acceleration because ArtiSynth’s physics engine computes velocity changes directly.

Figure 10.1: Tracking controller operation for motion targets.

10.1.2 Motion tracking

As mentioned above, the motion tracker computes a desired source velocity {\bf v}_{*} for the next time step based on the current source state ({\bf x},{\bf v}) and desired target state ({\bf x}_{t},{\bf v}_{t}). This can be done in two ways: chase control (the default), and PD control.

Both methods take account of the fact that the target and source states are known at different times. Let h be the step size, and let {\bf x}^{k} and {\bf v}^{k} denote the source position and velocity at the start of the step. Since input probes and other controllers are applied before the tracking controller, the target state has already been advanced to the end of the step when the tracking controller is called, and so is given by {\bf x}_{t}^{k+1} and {\bf v}_{t}^{k+1}. The position error at the start of the step is then estimated as

{\bf e}\equiv{\bf x}_{t}^{k+1}-h{\bf v}_{t}^{k+1}-{\bf x}^{k}. (10.2)

If {\bf v}_{t} is consistent with the motion of {\bf x}_{t}, then {\bf e} approximates {\bf x}_{t}^{k}-{\bf x}^{k}. If it is not (for instance, if {\bf v}_{t} is assumed to be 0 because it is neither specified nor estimated), then {\bf e} also contains a velocity correction that helps compensate for this.

10.1.2.1 Chase control

Chase control sets {\bf v}_{*} to {\bf v}_{t}, plus an additional velocity that tries to reduce the position error over a specified time interval T, called the chase time:

{\bf v}_{*}={\bf v}_{t}^{k+1}+\frac{{\bf e}}{T}. (10.3)

If the source attains {\bf v}_{*}, the position error decays as e^{-t/T}. When T=h, (10.3) reduces to

{\bf v}_{*}=\frac{{\bf x}_{t}^{k+1}-{\bf x}^{k}}{h},

which places the source on the target at the end of each step, regardless of {\bf v}_{t}. This most closely reproduces the target trajectory, and is often the appropriate choice for inverse simulation.

T should not be less than h, since the source would then overshoot the target. Larger values of T produce smoother excitations and reduce sensitivity to roughness in the target trajectory and to system nonlinearities, but introduce a tracking lag of about (T-h)\|{\bf v}_{t}\| if {\bf v}_{t} is neither specified nor estimated (Section 10.2.2). With {\bf v}_{t} specified or estimated, a chase time of a few steps can give smoother excitations with little or no loss of accuracy.

T is controlled by the chaseTime property of the motion target term (Section 10.3.3). In accordance with the need to keep T\geq h, T is determined from

T=\max(h,\text{\sf chaseTime}).

Setting chaseTime to 0 (the default) then ensures that T=h.

10.1.2.2 PD control

PD control computes a desired source acceleration according to

\dot{\bf v}_{*}=\dot{\bf v}_{t}+K_{p}({\bf x}_{t}-{\bf x})+K_{d}({\bf v}_{t}-{%
\bf v}), (10.4)

which, if attained, causes the position error {\bf x}_{t}-{\bf x} to obey

\ddot{\bf e}+K_{d}\dot{\bf e}+K_{p}{\bf e}=0. (10.5)

This is then integrated over the step to determine {\bf v}_{*}:

{\bf v}_{*}={\bf v}_{t}^{k+1}-\boldsymbol{\eta}+h(K_{p}{\bf e}+K_{d}%
\boldsymbol{\eta}),\quad\boldsymbol{\eta}\equiv{\bf v}_{t}^{k}-{\bf v}^{k}, (10.6)

where {\bf e} is given by (10.2) and {\bf v}_{t}^{k} is the target velocity from the previous step. (If this is unavailable, as on the first step or after a reset, {\bf v}_{t}^{k+1} is used instead.) Equation (10.6) is equivalent to {\bf v}_{*}={\bf v}^{k}+h\dot{\bf v}_{*}, with the target acceleration \dot{\bf v}_{t} approximated by ({\bf v}_{t}^{k+1}-{\bf v}_{t}^{k})/h.

From (10.5), the gains determine a natural frequency \sqrt{K_{p}} and a damping ratio \zeta=K_{d}/(2\sqrt{K_{p}}). It is often easier to specify the gains in terms of \zeta and a time constant T=1/\sqrt{K_{p}}, using

K_{p}=\frac{1}{T^{2}},\quad K_{d}=\frac{2\zeta}{T}.

For critical damping (\zeta=1), the position error following a step change decays as (1+t/T)e^{-t/T}, falling to 10% of its initial value after about 3.9T. Values of \zeta less than 1 lead to overshoot, while values greater than 1 lead to a slower response. Unlike chase control, the resulting behavior does not depend on the step size h, provided that hK_{d} and h\sqrt{K_{p}} are both less than about 1; otherwise, the discrete control law no longer approximates (10.5) and a warning is printed. The default values for K_{p} and K_{d} are 2500 and 100, corresponding to T=0.02 and \zeta=1.

PD control is enabled by setting the usePDControl property of the motion target term to true, and adjusted by setting its Kp and Kd properties. The gains can also be set using the pdTimeConstant and pdDampingRatio properties, which set Kp and Kd according to T and \zeta (Section 10.3.3).

Chase control is a special case of PD control: setting K_{p}=1/(hT) and K_{d}=1/h in (10.6) yields (10.3). That is, chase control eliminates any velocity error within a single step, so that its derivative gain depends on h. PD control offers a greater ability to adjust the tracking behavior, but works best when {\bf v}_{t} is specified or estimated (Section 10.2.2). Otherwise, the derivative term will damp the source velocity toward 0, producing a tracking lag of about 2\zeta T\|{\bf v}\|.

With either method, target trajectories should be smooth. For example, marker data sampled at a fixed rate and interpolated linearly has velocity discontinuities at each sample. If the step size is smaller than the sample interval, these can cause the computed excitations to oscillate at the sample rate. Using cubic interpolation for the target probe avoids this.

10.1.3 Generating excitations using a quadratic program

Given {\bf v}_{*}, the excitation generator computes {\bf a} to try and ensure that the velocity at the end of the subsequent time step, {\bf v}^{k+1}, satisfies

{\bf v}^{k+1}\approx{\bf v}_{*}. (10.7)

This is accomplished using a quadratic program.

First, we linearize (10.1) with respect to changes in {\bf a} given by {\bf a}={\bf a}_{0}+\Delta{\bf a}, where {\bf a}_{0} is the current set of excitation values and \partial{\bf f}_{a}/\partial{\bf a} is evaluated at {\bf a}_{0}:

{\bf f}({\bf a}_{0}+\Delta{\bf a})\approx{\bf f}_{p}({\bf q},{\bf u})+{\bf f}_%
{a}({\bf q},{\bf u},{\bf a}_{0})+\frac{\partial{\bf f}_{a}}{\partial{\bf a}}%
\Delta{\bf a}, (10.8)

Given this, it is possible to show that

{\bf v}^{k+1}={\bf v}_{0}+{\bf H}_{m}\Delta{\bf a},

where {\bf v}_{0} is the velocity that will result if {\bf a} is unchanged and {\bf H}_{m} is a motion excitation response matrix. To best follow the trajectory, we can compute \Delta{\bf a} to minimize the quadratic cost function

\phi_{m}(\Delta{\bf a})\equiv\frac{1}{2}\|{\bf v}_{*}-{\bf v}^{k+1}\|^{2}=%
\frac{1}{2}\|\bar{\bf v}-{\bf H}_{m}\Delta{\bf a}\|^{2},\quad\bar{\bf v}\equiv%
{\bf v}_{*}-{\bf v}_{0}. (10.9)

If we add in the constraint that excitations lie in the range [0,1], the problem takes the form of a quadratic program (QP)

\displaystyle\min_{\Delta{\bf a}}\;\phi_{m}(\Delta{\bf a})
\displaystyle\text{subject to}\quad-{\bf a}_{0}\leq\Delta{\bf a}\leq{\bf 1}-{%
\bf a}_{0}. (10.10)

In order to prioritize some target terms over others, \phi_{m}(\Delta{\bf a}) can be modified to include weights, according to

\phi_{m}(\Delta{\bf a})\equiv\frac{1}{2}\|{\bf W}_{m}(\bar{\bf v}-{\bf H}_{m}%
\Delta{\bf a})\|^{2}, (10.11)

where {\bf W}_{m} is a diagonal weighting matrix. To handle excitation redundancy, where the size of {\bf a} exceeds the size of {\bf v}, we can add a regularization term

\phi_{a}(\Delta{\bf a})\equiv\frac{1}{2}({\bf a}_{0}+\Delta{\bf a})^{T}{\bf W}%
_{a}({\bf a}_{0}+\Delta{\bf a}) (10.12)

that resolves redundancy in favour of lowering a weighted norm of {\bf a}. {\bf W}_{a} is a diagonal weight matrix that can be used to adjust the relative importance of specific excitations.

The matrix {\bf W}_{a} described here is the inverse of the matrix {\bf W} presented in [11]. In particular, increasing an element of {\bf W}_{a} will tend to decrease the size of the corresponding excitation. Also, For legacy reasons, {\bf W}_{a} appears unsquared in \phi_{a}, whereas {\bf W}_{m} (and similar matrices in other cost functions) are squared.

The motion and regularization terms are combined to form an overall cost function

\displaystyle\min_{\Delta{\bf a}}\;w_{m}\phi_{m}(\Delta{\bf a})+w_{a}\phi_{a}(%
\Delta{\bf a})
\displaystyle\text{subject to}\quad-{\bf a}_{0}\leq\Delta{\bf a}\leq{\bf 1}-{%
\bf a}_{0}, (10.13)

where w_{m} and w_{a} are additional scalar weights that balance the different cost terms.

If the controller property normalizeCostTerms is true (the default), then the cost terms in (10.13) are normalized so that the effect of the weights w_{m} and w_{a} more accurately reflects their actual value. See normalizeCostTerms in Section 10.3.2 for more details.

10.1.4 Force tracking

Other cost terms can be added to the tracking controller. For instance, we can request a force trajectory {\bf f}_{et} for a selected set of force effectors. As with motions, the forces of these effectors {\bf f}_{e} at the end of step k+1 are linearly related to \Delta{\bf a} via

{\bf f}_{e}^{k+1}={\bf f}_{e0}+{\bf H}_{e}\Delta{\bf a}

where {\bf f}_{e0} is the force if {\bf a} is unchanged and {\bf H}_{e} is the force excitation response matrix. The force trajectory can then be tracked by minimizing

\phi_{e}(\Delta{\bf a})\equiv\frac{1}{2}\|{\bf W}_{e}(\bar{\bf f}_{e}-{\bf H}_%
{e}\Delta{\bf a})\|^{2},\quad\bar{\bf f}_{e}\equiv{\bf f}_{et}-{\bf f}_{e0}, (10.14)

where {\bf W}_{e} is a diagonal weighting matrix. When tracking force targets, the target force trajectory {\bf f}_{et} is fed directly into the excitation generator (Figure 10.2); there is no equivalent of the motion tracker.

Figure 10.2: Tracking controller operation for force targets.

With force tracking included, the full QP takes the form

\displaystyle\min_{\Delta{\bf a}}\;w_{m}\phi_{m}(\Delta{\bf a})+w_{e}\phi_{e}(%
\Delta{\bf a})+w_{a}\phi_{a}(\Delta{\bf a})
\displaystyle\text{subject to}\quad-{\bf a}_{0}\leq\Delta{\bf a}\leq{\bf 1}-{%
\bf a}_{0}. (10.15)

10.1.5 Non-incremental computation

In many cases, the system forces are actually linear with respect to {\bf a}, such that {\bf f}_{a} in (10.1) takes the form

{\bf f}_{a}=\Lambda({\bf q},{\bf u}){\bf a}, (10.16)

When that is true,

\frac{\partial{\bf f}_{a}}{\partial{\bf a}}=\Lambda({\bf q},{\bf u}),

and the cost terms and QP can be defined with respect to the full excitation values {\bf a} instead of \Delta{\bf a}. The motion term, for example, becomes

\phi_{m}({\bf a})\equiv\frac{1}{2}\|{\bf W}_{m}(\bar{\bf v}-{\bf H}_{m}{\bf a}%
)\|^{2},\quad\bar{\bf v}\equiv{\bf v}_{*}-{\bf v}_{0},

where {\bf v}_{0} is now the velocity that results when {\bf a}={\bf 0} (and similarly for the force term, with {\bf f}_{e0} the force when {\bf a}={\bf 0}), while the regularization term becomes simply \phi_{a}({\bf a})=\frac{1}{2}{\bf a}^{T}{\bf W}_{a}{\bf a}. The QP then takes the form

\displaystyle\min_{{\bf a}}\;w_{m}\phi_{m}({\bf a})+w_{e}\phi_{e}({\bf a})+w_{%
a}\phi_{a}({\bf a})
\displaystyle\text{subject to}\quad{\bf 0}\leq{\bf a}\leq{\bf 1}. (10.17)

The original tracking controller assumed that forces are linear in {\bf a} and used (10.17) in place of (10.15). Non-incremental computation can still be enabled by setting the tracking controller property computeIncrementally to false (Section 10.3.2), but it should only be used when the forces are (approximately) linear in {\bf a}. Even then, incremental computation usually gives equal or better tracking at the same computational cost. The main reasons to enable non-incremental computation are:

  • •

    To reproduce the results of models that were developed using non-incremental computation, which was the default in earlier versions of ArtiSynth.

  • •

    Non-incremental computation can sometimes give slightly smoother excitations, at the expense of tracking accuracy. For example, in an arm model with a soft FEM bone, mean excitation changes per step were about 45% lower with non-incremental computation, while the tracking error was about 12% higher.

10.1.6 Setting up the tracking controller

Applications will generally set up a tracking controller using the following steps inside the application’s build() method:

  1. 1.

    Create an instance of TrackingController and add it to the application using the root model’s addController() method.

  2. 2.

    Configure the controller with the available excitation components using its addExciter(ex) and addExciter(weight,ex) methods.

  3. 3.

    Configure the controller to track position or force trajectories for selected source components, which may include Point, Frame, or ForceEffector components. These are specified to the controller using methods such as addPointTarget(point), addFrameTarget(frame), and addForceEffectorTarget(forceEffector). These methods return a target object, which is allocated and maintained by the controller, and which is used for specifying the target positions or forces.

  4. 4.

    Create input probes or controllers to specify the desired trajectories for the sources added in Step 3. Trajectories are specified by using probes or controllers to set the appropriate properties of the target objects returned by the addXXXTarget() methods. Likewise, output probes or monitors can be created to record the excitations and the tracked positions or forces of the sources.

  5. 5.

    Add other cost terms as needed. These may include an L2 regularization term, added using the controller’s addL2RegularizationTerm() method. Regularization attempts to minimize the norm of the excitation values and is needed to resolve redundancy if the excitation set has more degrees of freedom than the target space.

    For historical reasons, the target position and target velocity properties described in Section 5.3.1.1 are not used by the tracking controller for target components; therefore, probes controlling target positions and velocities should be bound to the position and velocity properties.

  6. 6.

    Set controller configuration parameters.

Once the tracking controller has been set up, during subsequent simulation it will compute excitation values, using the quadratic program described in Sections 10.1.3 and 10.1.4, so as to best follow the prescribed target trajectories.

10.1.7 Example: moving a point with multiple Muscles

Figure 10.3: InverseParticle when first loaded (left), and during simulation with the muscles activated to move the center particle (right).
Figure 10.4: Probes showing the target position and computed excitations for InverseParticle.

A simple example illustrating the steps of Section 10.1.6 is given by

  artisynth.demos.tutorial.InverseParticle

which uses a set of point-to-point muscles to drive the position of a single particle. The model consists of a single dynamic particle, initially located at the origin, attached to a set of 16 point-to-point muscles arranged radially around it. A tracking controller is created to control the excitations of these muscles so as to enable the particle to follow a trajectory specified by a probe. The code for the model, without the package and import directives, is listed below:

1 public class InverseParticle extends RootModel {
2
3    int numMuscles = 16; // num radial muscles surrounding the dynamic particle
4    double muscleStiffness = 200; // passive muscle stiffness
5    double muscleDamping = 1.0; // passive muscle damping
6    double muscleFmax = 1000; // max active force at excitation = 1
7    double dist = 1.0; // distance of anchor point from world origin
8
9    /**
10     * Create a muscle excitable spring extending out from the origin
11     * {@code part} at an angle {@code ang}.
12     */
13    void createMuscle (MechModel mech, Particle part, double ang) {
14       // create and add non-dynamic particle as fixed end point for muscle
15       Particle fixed = new Particle(
16          /*mass=*/1d, new Point3d(dist*Math.sin(ang),0.0,dist*Math.cos(ang)));
17       fixed.setDynamic(false);
18       mech.addParticle(fixed);
19       // create muscle and set its material
20       Muscle muscle = new Muscle (fixed, part);
21       muscle.setName (  // name the muscle using its angle
22          "muscle_"+Integer.toString((int)Math.round(Math.toDegrees(ang))));
23       muscle.setMaterial (
24          new SimpleAxialMuscle (muscleStiffness, muscleDamping, muscleFmax));
25       mech.addAxialSpring (muscle);
26
27       // make muscles red when activated
28       muscle.setExcitationColor (Color.RED);
29       muscle.setMaxColoredExcitation (0.5);
30       muscle.setRestLength (muscle.getLength());
31    }
32
33    public void build (String[] args) {
34       // create MechModel and add to RootModel
35       MechModel mech = new MechModel ("mech");
36       addModel (mech);
37       mech.setGravity (Vector3d.ZERO); // disable gravity
38
39       // create and add particle whose position is to be controlled
40       Particle part = new Particle ("center", /*mass=*/0.1, /*x,y,z=*/0, 0, 0);
41       part.setPointDamping (0.1); // add damping
42       mech.addParticle(part);
43
44       // create radial muscles connected to center particle
45       for (int i = 0; i < numMuscles; i++) {
46          double angle = 2*Math.PI*((double)i/numMuscles);
47          createMuscle (mech, part, angle);
48       }
49
50       // create the tracking controller and add it to the root model
51       TrackingController tcon = new TrackingController(mech, "tcon");
52       addController(tcon);
53       // set all muscles to be "exciters" for the controller to control
54       for (AxialSpring s : mech.axialSprings()) {
55          if (s instanceof Muscle) {
56             tcon.addExciter((Muscle)s);
57          }
58       }
59       // set the center dynamic particle to be the component that is tracked
60       TargetPoint target = tcon.addPointTarget(part);
61       // add an L-2 regularization term, since there are more muscles than
62       // target degrees-of-freedom
63       tcon.setL2Regularization(/*weight=*/0.1);
64
65       // Render properties: make points gray spheres, central particle white,
66       // and muscles as blue cylinders.
67       RenderProps.setSphericalPoints (this, dist/25, Color.LIGHT_GRAY);
68       RenderProps.setPointColor (part, Color.WHITE);
69       RenderProps.setCylindricalLines (mech, dist/50, Color.BLUE.darker ());
70
71       // add an input probe to control the position of the target:
72       NumericInputProbe targetprobe = new NumericInputProbe (
73          target, "position", /*startTime=*/0, /*stopTime=*/1);
74       targetprobe.setName ("target positions");
75       targetprobe.addData (
76          new double[] {0d,0d,0d, 0.5,0d,0.5, 0d,0d,0d}, // three knot points
77          /*timestep=*/0.5);
78       targetprobe.setInterpolationOrder (Interpolation.Order.Cubic);
79       addInputProbe (targetprobe);
80
81       // add an output probe to record the excitations:
82       NumericOutputProbe exprobe = InverseManager.createOutputProbe (
83          tcon, ProbeID.COMPUTED_EXCITATIONS, /*fileName=*/null,
84          /*startTime=*/0, /*stopTime=*/1, /*interval=*/-1);
85       addOutputProbe (exprobe);
86    }
87 }

The build() method begins by creating a MechModel in the usual way and disabling gravity (lines 35-37). The particle to be controlled is then created and placed at the origin and with a damping factor of 0.1 (lines 40-42). Next, the particle is attached to a set of point-to-point muscles that are arranged radially around it (lines 45-48). These are created using the method createMuscle() (lines 13-31), which attaches each to a fixed non-dynamic particle located a distance of dist from the origin, and sets its material to a SimpleAxialMuscle (Section 4.5.1.1) with a passive stiffness, damping and maximum force (at excitation 1) defined by muscleStiffness, muscleDamping, and muscleFmax (lines 4-6). Each muscle’s excitationColor and maxColoredExcitation property is set so that its color transitions to red as its excitation value varies from 0 to 0.5 (lines 28-29, Section 10.2.1.1), and its rest length is initialized to its current length (line 30).

After the model components have been built, a TrackingController is created and added to the root model’s controller set (lines 51-52). All of the muscles are then added to the controller as exciters to be controlled (lines 54-58). The particle is added to the controller as a motion source using the addPointTarget() method (line 60), which returns a target object in the form of a TargetPoint. Since the number of exciters exceeds the degrees-of-freedom of the target space, an L2-regularization term is added (line 63) to resolve the redundancy.

Rendering properties are set at lines 67-69: points in the model are rendered as gray spheres, except for the dynamic particle which is colored white, and the muscles are drawn as blue cylinders. (The target particle, which is contained within the controller, is displayed using its default color of cyan.)

Probes are created to specify the target trajectory and record the computed excitations. The first, targetprobe, is an input probe attached to the position property of the target component, running from time 0 to 1 (lines 72-79). Its data is specified in code using addData(), which specifies three knot points with a time step of 0.5. Interpolation is set to cubic (line 78) for greater smoothness. The second, exprobe, is a probe that records all the excitations computed by the controller (lines 82-85). It is created by the utility method createOutputProbe() supplied by the InverseManager class (Section 10.4.1).

To run this example, select All demos > tutorial > InverseParticle from the Models menu. The model should load and initially appear as in Figure 10.3 (left). When run, the controller computes excitations to move the particle along the trajectory specified by the probe, which is upward and to the right (Figure 10.3, right) and back again.

The target point created by the controller is rendered separately from the source point as a cyan-colored sphere (see Section 10.2.2.2). As the simulation proceeds and certain muscles are excited, their coloring changes from blue to red, in proportion to their excitation, in accordance with the properties excitationColor and maxColoredExcitation. Recorded data for both the trajectory and the computed excitation probes are shown in Figure 10.4.