I'm currently working (VERY slowly) on a project about biped balance control. My architecture is meant to work as follows:
- trajectory for each leg is planned on Cartesian space, and expressed as a matrix of via points
- the algorithm, written in C, calculates at each time interval the inverse kinematics by transposing the Jacobian matrix, multiplying it by a weighted error vector, and finally integrating the result (using Euler's method)
- a global vector containing the future robot stance is updated and commands are sent to the servos (at a much slower rate than that of the calculations)
- additionally, I plan to use Huv's pressure sensor to mesure the position of the ZMP, and use it to balance the biped, but this is not important for now.
In pseudo code:
- Code:
while ( time_counter < max )
{
for (cycle=0; cycle < end_cycle; cycle++)
{
//present target position by linear spline interpolation
x_d_dx = lin_spline(time_counter, via_points);
e_dx =K*( x_dx - x_d_dx);
qdot_dx = traspJac(e_dx, q_old_dx);
q_dx = euler(qdot_dx, q_old_dx);
q_old_dx = q_dx;
x_dx = direct_kin(q_dx);
//same for the left
_delay_ms(DELTA);
}
//update the portion of the global stance vector corresponding to that leg
update(RIGHT_LEG);
//same for the left
send_next_stance();
time_counter++;
}
Everything works fine in some situations, (and the logic is tested on Matlab), but other commands lead to strange behaviours, which I believe are related to temporization (in fact I can't determine the frequency at which I'm integrating, because calculatioins are slower than _delay_ms() ), and above all on the tracking precision of the algorithm, since to obtain small errors you need larger gains K, but numerical stability limits the distance you can travel. And again, larger frequencies allow for larger gains, but you can't increase it as you like ( I'm assuming from the CM-5 schematic that F_CPU = 16 MHz).
Has anyone already tried something similar?
Thanks, and sorry for bothering you!Statistics: Posted by Mirko — Tue Oct 12, 2010 4:08 pm
]]>
