Balance Functions

  This set of functions are included in the simulator to easy the implementation of the balance control introduced in [4] and extended in [11]. These functions, however, are not useful if other control schemas are adopted.

A balance is a measure of the position of the robot's body with respect to feet positions along a given dof. This measures are evaluated by comparing each leg position at a given moment with respect to a reference position that is at a fixed place with respect to the body. Both the legs and the reference positions are expressed in the reference frame of reference introduced in section 3.2.1.

The difference of a leg position with respect to its current reference position along a given dof is called the tension  of this leg. There is a physical analogy that can illustrate the meaning of the tension. If we imagine that each leg is attached to the reference position by an ideal spring, then the tension correspond the the forces and moments that the spring connected to the leg exert on the robot's body.

Formally, if pⁱ=(pⁱ_x,pⁱ_y,pⁱ_z) is the current position of a leg in the reference frame and rⁱ=(rⁱ_x,rⁱ_y,rⁱ_z) is its reference position in this same frame of reference, we can compute and many different tensions as possible dof in the Euclidean space (i.e., Tx, Ty, Tz, Rx, Ry, Rz):

$$\begin{align*} &Tension^i_{Tx}=p^i_x-r^i_x\\ &Tension^i_{Ty}=p^i_y-r^i_y\\ &Tens... ...^i_x r^i_z - p^i_z r^i_x\\ &Tension^i_{Rz}=p^i_y r^i_x - p^i_x r^i_y\end{align*}$$

Normally, the reference position of a leg should be the central position of its workspace (the one reached when all motor angles are zero) but the user can select another initial reference position and he can on-line change the reference position for legs. Formally, if r_initⁱ is the initial reference position given by the user, then the reference position rⁱ at a given moment is

$$r^i=Rz(r_\psi) Ry(r_\theta) Rx(r_\phi) Tz(r_z) Ty(r_y) Tx(r_x) r_{init}^i$$

where $r_\psi$, $r_\theta$, $r_\phi$, r_z, r_y, and r_x are initially set to 0 and that be changed using the functions described below.

The values of the tensions (and consequently the balance values) can be modified by moving legs (either individually of performing gestures) or by moving the reference positions. In our controllers both methods are used in different circumstances.

double get_current_tension (unsigned int leg dof_R3, t_robot *r)

 

Returns the current tension produced by a given leg in the specified dof.

double calculate_tension (unsigned int leg dof_R3,double *position,t_robot *r)

 

Returns the tension that a leg would exert on the robot's body in a given dof if the leg was placed at position. The position is given in the body frame of reference.

double get_balance (unsigned int leg dof_R3,t_robot *r)

 

Returns the adding of tensions in a given dof for all legs.

void move_balance_reference (unsigned int dof_R3,double v,t_robot *r)

 

Sets the parameter $r_{dof\_R3} \leftarrow v$

void move_incr_balance_reference (unsigned int dof_R3,double v,t_robot *r)

 

Sets the parameter $r_{dof\_R3} \leftarrow r_{dof\_R3}+v$

void reset_balance_reference (t_robot *r)

 

Sets the values of the reference positions to its initial values (i.e., sets all reference movement parameters $r_\psi$, $r_\theta$, $r_\phi$, r_z, r_y, and r_x to 0).

void get_central_position (unsigned int leg,double *position,t_robot *r)

 

Returns the initial reference position for the given leg (r_init^leg).

void get_balance_reference (unsigned int leg,double *position,t_robot *r)

 

Returns the current reference position for the given leg (r^leg).