The steps covered in part one are:

=20
=20

The desired trajectory for the model is a sinusoid that starts out exact=
ly halfway in-between the left and right walls (at the origin *z* =
=3D 0), moves toward the right wall (to *z* =3D +0.15 m) then toward=
the left wall (to *z* =3D -0.15 m), and then moves back to the star=
ting position. The whole motion will last from *t* =3D 0 seconds to =
*t* =3D 2 seconds. The equation for the *z*-coordinate of the=
model to follow this motion is *z*(*t*) =3D 0.15 sin(2*=
=EF=81=B0ft*), where the frequency is *f* =3D =C2=BD Hz, which s=
implifies to *z*(*t*) =3D 0.15 sin(*=EF=81=B0t*). We w=
ill 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 *z*(<=
em>t) given a value of *t*, according to the equation above.

=20

double d= esiredModelZPosition( double t ) { // z(t) =3D 0.15 sin( pi * t ) return 0.15 * sin( Pi * t ); }=20

The velocity of the model's *z*-coordinate is just the time-deriv=
ative of its position: *z*'(*t*) =3D 0.15*=EF=81=B0* c=
os(*=EF=81=B0t*). We will implement this as a function as well. =
;

=20

double d= esiredModelZVelocity( double t ) { // z'(t) =3D (0.15*pi) cos( pi * t ) return 0.15 * Pi * cos( Pi * t ); }=20

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 th=
e *ControllerExample.cpp* file).

We will design a controller that computes control values (excitations) f=
or the model's two muscles in an effort to make the model follow the desire=
d trajectory we implemented above. The controller will be a *proportiona=
l-derivative (PD) controller*: we will compute excitations based on dev=
iations 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 *xF _{opt}* at both ends of the actuator (direct=
ed from each end to the middle of the actuator), given an input excitation =
value 0 >

Continuing to pretend that our model contains idealized actuators instea=
d of muscles, we will now implement a PD controller that computes control v=
alues that would cause the actuators to produce the forces that would make =
the model follow the desired motion. At time *t*, we know the curren=
t position *z*(*t*) and velocity *z*'(*t*) of t=
he model, as well as the desired position *z _{des}*(

*a _{des}*(

where *k _{p}* and

Next, we compute the net desired force on the block in the model:

*F _{des}*(

where *m* is the mass of the block. Since muscles only pull and d=
o not push, we will only excite (i.e., send a non-zero control value to) on=
e muscle at a time (we will set the control value of the other muscle to ze=
ro). If *F _{des}*(

*x* =3D |*F _{des}*(

where *F _{opt}* is the maximum isometric force of the mus=
cle being excited at time

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:

=20

class Tu= gOfWarPDController : public Controller { OpenSim_DECLARE_CONCRETE_OBJECT(TugOfWarController, Controller); public: =09TugOfWarController(double aKp, double aKv) : Controller(), kp( aKp ), kv= ( aKv )=20 =09{ =09} ...=20

The constructor above says that when the controller is created, it shoul= d have all the properties of its parent Controller (i.e., it knows what mod= el 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 func= tion, which implements the intended control law. Two arguments are passed i= nto this function: the current state, s, of the system and a vector of cont= rols, which are the model controls to be computed. In our model, index 0 re= fers to the left muscle and index 1 refers to the right muscle. The compute= Controls function computes and adds in the values for controls for each of = its actuators based on the current state and desired position, velocity, an= d acceleration. You will need to fill out the computation of velocity and a= cceleration:

=20

void com= puteControls( const SimTK::State& s, SimTK::Vector controls ) const=20 { =09// Get the current time in the simulation. =09double t =3D s.getTime(); =09// Read the mass of the block. =09double blockMass =3D getModel().getBodySet().get( "block" ).getMass(); =09// Get pointers to each of the muscles in the model. =09Muscle* leftMuscle =3D dynamic_cast<Muscle*>=09( &getActuatorS= et().get(0) ); =09Muscle* rightMuscle =3D dynamic_cast<Muscle*> ( &getActuatorSe= t().get(1) ); =09// Compute the desired position of the block in the tug-of-war =09// model. =09double zdes =3D desiredModelZPosition(t); =09////////////////////////////////////////////////////////////// =09// 3) Compute the desired velocity of the block in the tug- // =09// of-war model. Create a new variable zdesv to hold // =09// this value. // =09////////////////////////////////////////////////////////////// =09// Compute the desired acceleration of the block in the tug- =09// of-war model. =09double zdesa =3D desiredModelZAcceleration(t); =09// Get the z translation coordinate in the model. =09const Coordinate& zCoord =3D _model->getCoordinateSet().get( "blo= ckToGround_zTranslation" ); =09// Get the current position of the block in the tug-of-war =09// model. =09double z =3D zCoord.getValue(s); =09////////////////////////////////////////////////////////////// =09// 4) Get the current velocity of the block in the tug-of- // =09// war model. Create a new variable zv to hold this // =09// value. // =09////////////////////////////////////////////////////////////// =09// Compute the correction to the desired acceleration arising =09// from the deviation of the block's current position from its =09// desired position (this deviation is the "position error"). =09double pErrTerm =3D kp * ( zdes - z ); =09////////////////////////////////////////////////////////////// =09// 5) Compute the correction to the desired acceleration // =09// arising from the deviation of the block's current // =09// velocity from its desired velocity (this deviation // =09// is the "velocity error"). Create a new variable // =09// vErrTerm to hold this value. // =09////////////////////////////////////////////////////////////// =09////////////////////////////////////////////////////////////// =09// 6) In the computation of desAcc below, add the velocity // =09// error term you created in item #5 above. Please // =09// update the comment for desAcc below to reflect this // =09// change. // =09////////////////////////////////////////////////////////////// =09// Compute the total desired acceleration based on the initial =09// desired acceleration plus the position error term we =09// computed above. =09double desAcc =3D zdesa + pErrTerm; =09// Compute the desired force on the block as the mass of the =09// block times the total desired acceleration of the block. =09double desFrc =3D desAcc * blockMass; =09// Get the maximum isometric force for the left muscle. =09double FoptL =3D leftMuscle->getMaxIsometricForce(); =09// Get the maximum isometric force for the right muscle. =09double FoptR =3D rightMuscle->getMaxIsometricForce(); =09// If desired force is in direction of one muscle's pull =09// direction, then set that muscle's control based on desired =09// force. Otherwise, set the muscle's control to zero. =09double leftControl =3D 0.0, rightControl =3D 0.0; =09if( desFrc < 0 ) { =09=09leftControl =3D abs( desFrc ) / FoptL; =09=09rightControl =3D 0.0; =09} =09else if( desFrc > 0 ) { =09=09leftControl =3D 0.0; =09=09rightControl =3D abs( desFrc ) / FoptR; =09} =09// Don't allow any control value to be greater than one. =09if( leftControl > 1.0 ) leftControl =3D 1.0; =09if( rightControl > 1.0 ) rightControl =3D 1.0; =09// Thelen muscle has only one control =09Vector muscleControl(1, leftControl); =09// Add in the controls computed for this muscle to the set of all model = controls =09leftMuscle->addInControls(muscleControl, controls); =09// Specify control for other actuator (muscle) controlled by this contro= ller =09muscleControl[0] =3D rightControl; =09rightMuscle->addInControls(muscleControl, controls); }=20

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 s= tate (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 de= claration of the member variables, kp, kv, and blockMass:

=20

private: /** Position gain for this controller */ double kp; /** Velocity gain for this controller */ double kv; };=20

=20

Next: Creati= ng a Controller Part Two

Previous: Creating a = Controller

Home: Scripting a= nd Development | Devel= oper's Guide | Ad= ding New Functionality

=20