The steps covered in part one are:
Defining the desired trajectory of the model
The desired trajectory for the model is a sinusoid that starts out exactly halfway in-between the left and right walls (at the origin z = 0), moves toward the right wall (to z = +0.15 m) then toward the left wall (to z = -0.15 m), and then moves back to the starting position. The whole motion will last from t = 0 seconds to t = 2 seconds. The equation for the z-coordinate of the model to follow this motion is z(t) = 0.15 sin(2ft), where the frequency is f = ½ Hz, which simplifies to z(t) = 0.15 sin(t). We will keep the desired values for all other coordinates of the model at zero.
To implement this desired trajectory, we will write a function in our ControllerExample.cpp file that calculates the value of z(t) given a value of t, according to the equation above.
The velocity of the model's z-coordinate is just the time-derivative of its position: z'(t) = 0.15 cos(t). We will implement this as a function as well.
The velocities of all of the other coordinates in the model shall be set to zero (the derivative of their position values, which are also zero). A function implementing the desired acceleration is written similarly (see the ControllerExample.cpp file).
Designing a controller to track a desired trajectory
We will design a controller that computes control values (excitations) for the model's two muscles in an effort to make the model follow the desired trajectory we implemented above. The controller will be a proportional-derivative (PD) controller: we will compute excitations based on deviations of the model's current position from its desired position, as well as on deviations of the model's current velocity from its desired velocity.
We will pretend that each of the model's two muscles is an idealized linear actuator that instantaneously applies forces with magnitude F = xFopt at both ends of the actuator (directed from each end to the middle of the actuator), given an input excitation value 0 > x > 1. Fopt may be a different number for each actuator and is a constant indicating the maximum force an actuator can produce when given a control value x = 1. We set Fopt for each actuator equal to the maximum isometric force (specified in the model's .osim file) for the corresponding muscle. Unlike idealized actuators, muscles have activation and contraction dynamics that transform an input control (excitation) value into a muscle force, and this force production is not an instantaneous process. A model containing idealized actuators instead of muscles that is controlled by a PD controller can instantaneously produce the necessary forces needed to make the model follow a desired trajectory. However, since our model consists of muscle actuators (which cannot instantaneously produce a desired force from a given excitation value) instead of idealized actuators, we expect that the controller we implement will not track the desired trajectory perfectly. But, we are curious to see just how close we can get with a simple controller!
Continuing to pretend that our model contains idealized actuators instead of muscles, we will now implement a PD controller that computes control values that would cause the actuators to produce the forces that would make the model follow the desired motion. At time t, we know the current position z(t) and velocity z'(t) of the model, as well as the desired position zdes(t), velocity zdes'(t), and acceleration zdes''(t) of the model. First, we compute the total desired acceleration:
ades(t) = [zdes''(t) + kv[zdes'(t) - z'(t)] + kp[zdes(t) - z(t)]],
where kp and kv are constants called the position and velocity gains, respectively. In dynamics, kp and kv represent the "stiffness" (force response due to position change) and "damping" (force response due to velocity change) properties of a system. If kv is too high, the system will be overdamped, and the system damping will dominate the response slowing down the time to reach the equilibrium state. If kv is too low, the controller will be underdamped and the system will overshoot and oscillate about an equilibrium state which also slows the settling time. It is common practice to choose kv so that the system is critically damped, i.e., the system settles quickly to a state but without oscillating about the equilibrium state. Similarly, a PD controller is considered critically damped if kv = 2*sqrt(kp) for a second-order linear system. Thus, we choose kp = 1600 and kv = 80 in our implementation of this PD controller.
Next, we compute the net desired force on the block in the model:
Fdes(t) = m ades(t),
where m is the mass of the block. Since muscles only pull and do not push, we will only excite (i.e., send a non-zero control value to) one muscle at a time (we will set the control value of the other muscle to zero). If Fdes(t) < 0, then we want to pull the block to the left, so we will excite the left muscle at time t. If Fdes(t) > 0, then we want to pull the block to the right, so we will excite the right muscle at time t. In any case, the non-zero control value x(t) that we send to a muscle at any time t will be:
x = |Fdes(t)| / Fopt,
where Fopt is the maximum isometric force of the muscle being excited at time t. This equation is the control law we will implement for our PD controller below.
Implementing the controller
In this example, we will write a class called TugOfWarPDController that implements the PD controller we designed above. To implement our controller with the desired control law, we derive our controller from Controller:
The constructor above says that when the controller is created, it should have all the properties of its parent Controller (i.e., it knows what model it will be controlling) and set its member variables kp and kv equal to the input values aKp and aKv, respectively.
The behavior of the controller is determined by its computeControls function, which implements the intended control law. Two arguments are passed into this function: the current state, s, of the system and a vector of controls, which are the model controls to be computed. In our model, index 0 refers to the left muscle and index 1 refers to the right muscle. The computeControls function computes and adds in the values for controls for each of its actuators based on the current state and desired position, velocity, and acceleration. You will need to fill out the computation of velocity and acceleration:
This function returns a control value based on deviation of the current state (position and velocity of the block) of the system from the desired state (position and velocity of the block). This is an implementation of the control law we described earlier.
Finishing off the definition of the TugOfWarPDController class is the declaration of the member variables, kp, kv, and blockMass: