[From Bill Powers (970107.0400 MST)]
This is a note to clarify for the record my reasons for doubting that Hans
Blom's model is the proper one for modeling the lower-level behavior of
human organisms.
Appended to the end of this post is a module, armdynam.c, from Little Man
Version 2. I particularly want to call attention to two functions in it, the
one called "evaluate" and the one called "control".
The control function models the spinal reflexes (tendon and stretch) plus
the properties of the muscles. Within the function called "control" there is
a section labeled "MUSCLE MODELS". The steps that follow are a reasonably
good model of the way a muscle contraction, proportional to the driving
signal from a motor neuron, is converted into a torque at a joint. The
driving signal, by shortening the contractile portion of the muscle,
stretches the passive series spring portion, changing the resting length of
the muscle. If the joint angle corresponds to this "adjustable resting
length," there is no torque. If the angle is different from the resting
angle (corresponding to the resting muscle length), the series muscle spring
is stretched, generating a torque tending to bring the joint angle toward
the resting angle (this part corresponds to the whole of Bizzi's model of
motor behavior, and is faithful to the accepted model of muscle behavior).
This model assumes that each muscle is a pair of muscles opposed to each
other, with driving signals that provide a balanced drive to the pair. The
model includes only the net unbalanced effects. Overall, this function
converts three reference signals into three torques at the three joints in
this arm model (shoulder x, shoulder y, and elbow angles).
The "control" function calls the "evaluate" function, which converts the
three torques into angular acceleration, angular velocity, and angle at each
of the three joints. This is a FORWARD dynamic model of the physical
response of the arm when torques are applied at the joints. As can be seen,
this C function is evaluating a set of nonlinear second-order differential
equations in which sines, cosines, and products of sines and cosines are
involved, as well as angles themselves and their first and second time
derivatives. The final result is an angular acceleration at each joint which
is integrated twice to give the angular velocity and finally the angle. The
dynamic physical interactions among the various motions (such as Coriolis
effect) are included. The control function calls the evaluate function four
times for each iteration of the main program, which represents the changes
that take place in one unit of time, here about 0.01 sec. The spinal model
is evaluated faster than the main model to approximate the relative delays
at the two primary levels of organization, spinal and higher-than-spinal.
A simulation-based control system representing only the spinal level of
organization would require an internal world-model that does what the
combined "control" and "evaluate" functions do: convert the driving signals
into resulting (simulated) angles, velocities, and accelerations. In
addition, the INVERSE of this same model would have to exist between the
incoming reference signals and the torque outputs of the control system. All
of the constants in the model and its inverse, such as the lengths of arm
segments and their masses and moments of inertia, as well as the properties
of the muscle such as its sensitivity to driving signals and its series
spring constant, would have to be represented correctly in the model and its
inverse. The simulation would have to perform a double time integration
which does not drift with time in order to produce the correct simulated
representations of the joint angles, corresponding to the actual joint
angles of the real arm.
I maintain that it is extremely improbable that the calculations in the
control and evaluate functions, AND THEIR INVERSES, are performed in the
spinal cord, _or anywhere else in the brain_. Further, I point out that the
calculations required in the Little Man model control systems are of extreme
simplicity in comparison and do not involve the calculation of any inverses
of functions. The muscles themselves and the arm itself, simply through
having the physical properties they have, perform all the "calculations"
shown here in the muscle model and the "evaluate" function.
These calculations do not take place in the nervous system at all. They
represent the "plant", not the controlling system.
I think that the mind-set that seems evident in "modern control theory" is
reflected in the robotics books from which Greg Williams obtained the
dynamical equations used here. The forward equations could not be found in
any of the three texts that Greg consulted. Instead, the derivation, in
every case, produced the INVERSE dynamical equations -- the equations that
allow one to calculate the torques required to produce a predetermined set
of accelerations, velocities and angles in the arm. (In addition, one text
included a typographical error in the derivation, which forced Greg to do
the entire derivation from scratch to find out which of the texts was in
error). In order to find the forward equations, we were forced to solve the
given equations simultaneously in order to obtain the angular accelerations
as a function of the applied torques -- in other words, to obtain the
_forward_ equations describing the arm's response to applied torques.
I think that if the _forward_ equations for the whole body were available,
it would be quite feasible to model the entire body and the simple PCT-type
control systems that control its postures. From this we could progress to
control of bodily configuration, to balancing, and to locomotion, level by
level. I can't produce the dynamical equations myself. If anyone has derived
them, I don't know about it. From time to time I've tried to get the
attention of engineers and mathematicians who might have the skills
required, but they are uniformly immersed in far more complex, and I think
wrong, approaches to the problem and apparently have no interest in pursuing
a different approach.
Best,
Bill P.
···
===========================================================================
/* Module armdynam.c */
#include "stdio.h"
#include "conio.h"
#include "math.h"
#include "string.h"
#include "graphics.h"
extern int hsize,
vsize,
hcenter,
vcenter,
interval,
time,
testnum,
test1,
usediagram,
r2e,p2e,o2e,
r2a,p2a,o2a,
r2d,p2d,o2d;
#ifndef numkparams
#define numkparams 15
#endif
#ifndef numvparams
#define numvparams 8
#endif
FILE *dfile;
float kg0,kt0,kd0,ko0,ks0;
float kg1,kt1,kd1,ko1,ks1;
float kg2,kt2,kd2,ko2,ks2;
float kparams[numkparams];
float m1,m2,l1,l2,l1c,l2c,gg,g1,g2;
float I0,I1,I2,q1,q2,q3;
float t0,t1,t2;
float a0,b0,a1,b1,c1,a2,b2,c2;
float t0dot,t0ddot,t1dot,t1ddot,t2dot,t2ddot;
float denom;
float c1c1,c3c1,c3c3,s1c1,s1c3,s3c3,c1s3;
float co1,si1,co2,si2,co3,si3,s1p1,s3p3,s1p3;
float pl0,pl1,pl2;
float loopgain0,loopgain1,loopgain2;
float p0ref,p1ref,p2ref;
float torque0,torque1,torque2;
float t0ref,t1ref,t2ref;
float pt0,pt1,pt2;
float dt,dt0;
static float e0,e1,e2;
static float t0d = 0.0, t1d = 3.0e-6, t2d = -5.0e-6;
int gravity,initialized;
int mark;
int base1,base2,base3,base4,base5,base6;
void armcont(int *a, int *e, int *f, int aref,int eref,int fref);
void initdynam(void);
void setgravity(void);
void makeconstants(void);
void evaluate(float torque0,float torque1,float torque2);
void initialize(void);
void control(void);
void showangles(int a1,int a2,int a3);
void showfloat(char *s, int x, int y, float n);
void showcontrol(int level, int which);
/* for debugging */
void showfloat(char *s, int x, int y, float n)
{
char nstr[200];
sprintf(nstr,"%15s %8.3f",s,n);
setfillstyle(0,0);
bar(x,y,x+textwidth(nstr),y+textheight("1"));
outtextxy(x,y,nstr);
}
void makeconstants()
{
base1 = 2 * vsize/14; /* y - offsets of plots */
base2 = 4 * vsize/14;
base3 = 6 * vsize/14;
base4 = 8 * vsize/14;
base5 = 10 *vsize/14 ;
base6 = 12*vsize/14;
/* everything in MKS units, from measurements of my arm */
m1 = 2.73; m2 = 1.25; /* -- masses, kg */
I0 = 1.0;
l1 = 0.36; l2 = 0.33; /*-- link lengths*/
l1c = l1/2.0; l2c = l2/2.0; /* centers of mass */
I1 = 0.52; I2 = 0.07; /* moments of inertia from measured geometry */
q1 = m1*l1c*l1c + I1 + m2*l1*l1; /* constants for calculations */
q2 = m2*l1*l2c;
q3 = m2*l2c*l2c + I2;
if (gravity) gg = 9.8;
else gg = 0.0;
g1 = (m1*l1c + m2*l1) * gg;
g2 = m2*l2c*gg;
loopgain0 = kt0*ko0*ks0;
loopgain1 = kt1*ko1*ks1;
loopgain2 = kt2*ko2*ks2;
}
/* convert three torques to angular accel, angular velocity, angle */
void evaluate(float torque0,float torque1,float torque2)
{
co1 = cos(t1); /*prepare constants for repeated use */
co2 = cos(t2);
si1 = sin(t1);
si2 = sin(t2);
si3 = si1*co2 + co1*si2;
co3 = co1*co2 - si1*si2;
c1c1 = co1 * co1;
c1s3 = co1 * si3;
c3c1 = co3 * co1;
c3c3 = co3 * co3;
s1c1 = si1 * co1;
s1c3 = si1 * co3;
s3c3 = si3 * co3;
/*make coefficients*/
/* ddot = second derivative
dot = first derivative */
a0 = ( I0 + q1*c1c1 + 2.0*q2*c3c1 + q3*c3c3 ); /*t0ddot*/
b0 = -2.0 * t0dot * (
(q1*s1c1 + q2*s1c3 + q3*s3c3)* t1dot
+(q2*c1s3 + q3*s3c3) * t2dot
);
a1 = q1 + 2.0*q2*co2 + q3;
b1 = q2*co2 + q3;
c1 = (q1*s1c1 + q2*(s1c3-c1s3) + q3*s3c3) * t0dot * t0dot
-q2*si2*t2dot*(t2dot + 2.0 * t1dot)
+ g1*co1 + g2*co3;
a2 = b1;
b2 = q3;
c2 = (q2*c1s3 + q3*s3c3) * t0dot * t0dot
+ q2 * si2 * t1dot * t1dot
+ g2*co3;
denom = (a1*b2 - a2*b1);
/* Shoulder x */
t0ddot = (torque0 - b0) / a0; /* simple double integrals */
t0dot += t0ddot * dt0;
t0 += t0dot * dt0;
/* Shoulder y */
t1ddot = ( (b2*torque1 - b1*torque2) - (b2*c1 - b1*c2) ) / denom;
t1dot += t1ddot * dt0;
t1 += t1dot* dt0;
/* Elbow */
t2ddot = ( (a1*torque2 - a2*torque1) - (a1*c2 - a2*c1) ) / denom;
t2dot += t2ddot * dt0;
t2 += t2dot * dt0;
if(t2 < 0.0) t2 = 0.0; /* limit elbow angle to full extension */
}
void initialize()
{
t0ref = 0.0; t1ref = -0.3; t2ref = 0.5;
t0ddot = 0.0; t0dot = 0.0; t0 = -0.340;
t1ddot = 0.0; t1dot = 0.0; t1 = -0.824;
t2ddot = 0.0; t2dot = 0.0; t2 = 1.639;
pl0 = 0.0; pl1 = 0.0; pl2 = 0.0;
pt0 = 0.0063; pt1 = -.1185; pt2 = 0.0;
}
void initdynam()
{
initialize();
makeconstants();
initialized = !1;
pt0 = t0ref;
pt1 = t1ref;
pt2 = t2ref;
gravity = 0;
dt = 0.010;
dt0 = dt/4.0; /* run arm dynamics 4 times as fast */
}
void setgravity()
{
gravity = !gravity;
makeconstants();
}
/* plot joint angle acceleration, velocity, angle */
/* shx = shoulder x; shy = shoulder y; elbow = external elbow angle */
void showangles(int a1,int a2,int a3)
{
static int t = 0;
static int old0,old1,old2;
int i;
int s0,s1,s2;
if(test1) s0=s1=s2=testnum;
else { s0 = 0; s1 = 1; s2 = 2; }
mark = (t+1) % interval;
if(mark == 0)
for(i=-2;i< 3; ++i)
{
if(s0 == 0) putpixel(t,base1 - a1/2 + i,WHITE);
if(s1 == 1) putpixel(t,base2 - a2/2 + i,WHITE);
if(s2 == 2) putpixel(t,base3 - a3/2 + i,WHITE);
if(s0 == 0) putpixel(t,base4 - torque0/4.0 + i,WHITE);
if(s1 == 1) putpixel(t,base5 - torque1/4.0 + i,WHITE);
if(s2 == 2) putpixel(t,base6 - 3.0*torque2 + i,WHITE);
}
if(s0 == 0) putpixel(t,base1 - a1/2,WHITE);
if(s1 == 1) putpixel(t,base2 - a2/2,WHITE);
if(s2 == 2) putpixel(t,base3 - a3/2,WHITE);
setwritemode(0);
moveto(t-1,old0);
old0 = base4 - torque0/4.0;
if(s0 == 0) lineto(t,old0);
moveto(t-1,old1);
old1 = base5 - torque1/4.0;
if(s1 == 1) lineto(t,old1);
moveto(t-1,old2);
old2 = base6 - 3.0*torque2;
if(s2 == 2) lineto(t,old2);
setwritemode(1);
++t;
if((t % (20*interval)) == 0)
{
t = 0; time = 0;
setviewport(0,0,200,vsize - 1,0);
clearviewport();
setviewport(0,0,hsize-1, vsize-1,0);
outtextxy(0,1,"ANGLES:");
if(s0 == 0) outtextxy(0,base1 - a1/2 - 12,"Sh. X");
if(s1 == 1) outtextxy(0,base2 - a2/2 - 12,"Sh. Y");
if(s2 == 2) outtextxy(0,base3 - a3/2 - 12,"Elbow");
outtextxy(0,vcenter,"TORQUES:");
if(s0 == 0) outtextxy(0,base4 - torque0/5 - 12,"Sh. X");
if(s1 == 1) outtextxy(0,base5 - torque1/5 - 12,"Sh. Y");
if(s2 == 2) outtextxy(0,base6 - torque2/5 - 12,"Elbow");
}
};
void showcontrol(int level,int which) /* using lineto for plots */
{
static int t = 0;
static int oldt = 0;
static int oldaccel;
static int oldvel;
static int oldangle;
float timescale;
int i,accel,vel,x,angle,hscale;
if (t == 0) oldt = 0;
if (level == 2) timescale = .5; else timescale = 2.0;
if(level == 1)
switch (which)
{
case 0: accel = t0ddot/4.0;
vel = 5.0*t0dot;
angle = 100*t0 - 50;
break;
case 1: accel = t1ddot/4.0;
vel = 5.0*t1dot;
angle = 100*t1 - 50;
break;
case 2: accel = t2ddot/4.0;
vel = 5.0*t2dot;
angle = 100*t2 - 50;
break;
}
else
switch (which)
{
case 1: accel = r2e/10.0;
vel = p2e/10.0;
angle = o2e/10.0 + 30;
break;
case 0: accel = r2a/10.0;
vel = p2a/10.0;
angle = o2a/10.0;
break;
case 2: accel = r2d/10.0 - 100;
vel = p2d/10.0 - 100;
angle = o2d/10.0 - 150;
break;
}
x = timescale * t;
if(t==1) oldt = 0;
if(t == 0) oldaccel = accel;
moveto(oldt,vsize/4 - oldaccel);
lineto(x,vsize/4 - accel);
if(t == 0) oldvel = vel;
moveto(oldt,2*vsize/4 - oldvel);
lineto(x,2 * vsize/4 - vel);
if(t == 0) oldangle = angle;
moveto(oldt,3*vsize/4 - oldangle);
lineto(x,3 * vsize/4 - angle);
oldt = x;
oldaccel = accel; oldvel = vel; oldangle = angle;
mark = (t) % interval;
if(mark == 0)
for(i=-2;i< 3; ++i)
{
putpixel(timescale * t, vsize/4 - accel+ i,WHITE);
putpixel(timescale * t,2 * vsize/4 - vel+ i,WHITE);
putpixel(timescale * t,3 * vsize/4 - angle + i,WHITE);
}
hscale = 20*interval/timescale;
if((t % hscale) == 0)
{
t = 0; time = 0;
setviewport(0,0,200,vsize - 1,0);
clearviewport();
setviewport(0,0,hsize-1, vsize-1,0);
if(level == 1)
{
switch(which)
{
case 1: outtextxy(hcenter/4,0,"SHOULDER Y"); break;
case 0: outtextxy(hcenter/4,0,"SHOULDER X"); break;
case 2: outtextxy(hcenter/4,0,"ELBOW"); break;
}
outtextxy(50,1*vsize/4-20,"ACCEL");
outtextxy(50,2*vsize/4-20,"VEL");
outtextxy(50,3*vsize/4-20,"ANGLE");
}
else
{
switch(which)
{
case 1: outtextxy(hcenter/4,0,"ELEVATION"); break;
case 0: outtextxy(hcenter/4,0,"AZIMUTH"); break;
case 2: outtextxy(hcenter/4,0,"RADIUS"); break;
}
outtextxy(0,1*vsize/4-60,"REF SIG");
outtextxy(0,2*vsize/4-45,"PERCEP");
outtextxy(0,3*vsize/4-20,"OUTPUT");
}
}
++t;
};
/* SPINAL CONTROL SYSTEMS */
void control()
{
static float oldpl0 = 0.0, oldpl1 = 0.0,oldpl2 = 0.0;
/* Perception of tendon tension, smoothed. Global variables */
pt0 = pt0 + (torque0*kt0 - pt0)/ (1.0 + loopgain0)/5.0 ; /* azimuth*/
pt1 = pt1 + (torque1*kt1 - pt1)/ (1.0 + loopgain1)/5.0 ; /* elevation */
pt2 = pt2 + (torque2*kt2 - pt2)/ (1.0 + loopgain2)/5.0 ; /* distance */
/* "distance" = elbow angle */
/* Perception of muscle spindle length */
pl0 = kg0 * (t0ref - t0);
pl1 = kg1 * (t1ref - t1);
pl2 = kg2 * (t2ref - t2);
/* slightly smoothed derivatives of length, for damping */
t0d = t0d + 0.7*((pl0 - oldpl0)*dt - t0d);
t1d = t1d + 0.7*((pl1 - oldpl1)*dt - t1d);
t2d = t2d + 0.7*((pl2 - oldpl2)*dt - t2d);
oldpl0 = pl0; oldpl1 = pl1; oldpl2 = pl2;
/* error signal = ref + length signal - tension signal - damping */
/* ref = alpha input to motor neuron (also gamma efferent)
plx = feedback signal from muscle spindle length detector
ptx = tendon receptor feedback signal
txd = rate component of muscle spindle signal, smoothed */
e0 = t0ref + pl0 - pt0 + kd0*t0d; /* damping terms added to error sig */
e1 = t1ref + pl1 - pt1 + kd1*t1d; /* error sig = motor neuron output */
e2 = t2ref + pl2 - pt2 + kd2*t2d;
/* M U S C L E M O D E L S */
/* (EACH MUSCLE MODEL REPRESENTS A BALANCED OPPOSED PAIR */
/* torque = spring constant times difference between angle and resting
angle, where resting angle is determined by the driving signal e */
torque0 = -ks0 * (t0 - ko0*e0);
torque2 = -ks2 * (t2 - ko2*e2);
/* elbow torque produced in part by muscle that spans elbow and
shoulder joint -- hence contribution from shoulder y torque */
torque1 = -ks1 * (t1 - ko1*e1) + torque2/2;
}
/* enter with azimuth, elevation, reach reference signals */
/* returns corresponding angles via pointer variables */
void armcont(int *a,int *e,int *r, int azimuth,int elevation,int reach)
{
t0ref = azimuth/651.9; /* convert from integer to floating pt. */
t1ref = elevation/651.9;
t2ref = reach/651.9;
control(); /* calculates torques at each joint */
evaluate(torque0,torque1,torque2); /* do four times per iteration */
evaluate(torque0,torque1,torque2); /* of main program */
evaluate(torque0,torque1,torque2);
evaluate(torque0,torque1,torque2);
*a = t0 * 651.9; /* return angles in proper units */
*e = t1 * 651.9;
*r = t2 * 651.9;
}