Date: Sat, 13 Aug 2022 23:32:35 -0700 (PDT) Message-ID: <1103433599.6891.1660458755658@simtk-confluence.stanford.edu> Subject: Exported From Confluence MIME-Version: 1.0 Content-Type: multipart/related; boundary="----=_Part_6890_412933442.1660458755658" ------=_Part_6890_412933442.1660458755658 Content-Type: text/html; charset=UTF-8 Content-Transfer-Encoding: quoted-printable Content-Location: file:///C:/exported.html Creating a Controller Part One

# Creating a Controller Part One

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 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 ControllerExample.cpp file that calculates the value of 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).

## Designing a controller to track a desired trajectory

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 F =3D xFopt at both ends of the actuator (direct= ed 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 maximu= m force an actuator can produce when given a control value x =3D 1= . We set Fopt for each actuator equal to the maximum is= ometric force (specified in the model's .osim file) for the corres= ponding muscle. Unlike idealized actuators, muscles have activation and con= traction 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 co= nsists of muscle actuators (which cannot instantaneously produce a desired = force from a given excitation value) instead of idealized actuators, we exp= ect 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 simp= le controller!

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 zdes(t), velocity zdes'(t), and acceleration z= des''(t) of the model. First, we compute the total= desired acceleration:

ades(t) =3D = [zdes''(t) + kv[z= des'(t) - z'(t)] + kp<= /em>[zdes(t) - z(t)]],

where kp and kv are constants ca= lled the position and velocity gains, respectively. In dynamics, k= p and kv represent the "stiffness" (force re= sponse due to position change) and "damping" (force response due to velocit= y change) properties of a system. If kv is too high, th= e system will be overdamped, and the system damping will dominate = the response slowing down the time to reach the equilibrium state. If k= v is too low, the controller will be underdamped a= nd the system will overshoot and oscillate about an equilibrium state which= also slows the settling time. It is common practice to choose kv<= /sub> so that the system is critically damped, i.e., the syst= em settles quickly to a state but without oscillating about the equilibrium= state. Similarly, a PD controller is considered critically damped if k= v =3D 2*sqrt(kp) for a second-order lin= ear system. Thus, we choose kp =3D 1600 and kv= =3D 80 in our implementation of this PD controller.

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

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 Fdes(t) < 0, then we want to pull t= he block to the left, so we will excite the left muscle at time t.= If Fdes(t) > 0, then we want to pull the b= lock to the right, so we will excite the right muscle at time t. I= n any case, the non-zero control value x(t) that we send = to a muscle at any time t will be:

x =3D |Fdes(t)| / Fopt,

where Fopt is the maximum isometric force of the mus= cle being excited at time t. This equation is the control law<= /em> we will implement for our PD controller below.

## Implementin= g 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:

=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
=09// Specify control for other actuator (muscle) controlled by this contro=
ller
=09muscleControl =3D rightControl;
}```
=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

Previous: Creating a = Controller

=20
------=_Part_6890_412933442.1660458755658--