Describing Constraints with gamma_q

This document explains how to write a function called gamma_q that imposes constraints on the joint position variables of a kinematic tree.  It can be used to implement gearing between joints, kinematic loops, etc.  This document first gives a bit of background, then explains how to write gamma_q, and finishes with an example.

Background

As explained in §3.2 of RBDA, there are two ways to describe a constraint on the vector of joint position variables, q.  One way is to define a function, φ, such that φ(q)=0 for every q that satisfies the constraint.  The other way is to identify a vector of independent position variables, y, and define a function γ such that q=γ(y).  Spatial_v2 takes the second approach.  Differentiating this equation once gives q' = G y', and differentiating it a second time gives

q" = G y" + g

H q" + C = τ + τ_c

G^T τ_c = 0 .

G^T H q" = G^T(τ − C)
G^T H (G y" + g) = G^T(τ − C)
y" = (G^T H G)⁻¹ G^T(τ − C − H g)

q" = G (G^T H G)⁻¹ G^T(τ − C − H g) + g

Defining gamma_q

gamma_q can now be defined as follows: it is a function to compute γ(y), G and g_s=g+g_stab from q and q', where g_stab is a stabilization term described below.  The calling convention for gamma_q is

[qn,qdn,G,gs] = gamma_q( model, qo, qdo )

where model is a system model data structure describing the kinematic tree that is to be subjected to constraint; qo and qdo are the given values of q and q'; qn and qdn are more accurate values for q and q'; and G and g_s are as described above.  Ideally, qn=γ(y), where y=γ*(qo) and γ* is a pseudoinverse of γ that first projects qo onto the range space of γ and then maps it to y.  However, some deviation from this ideal is possible, as explained below.

The purpose of supplying model as an argument is to allow gamma_q to have access to parameters contained within the system model data structure.  This gives you the option of writing a single generic gamma_q for a whole class of system models, rather than a separate gamma_q for each individual model in that class.  Note that there is nothing stopping you from adding your own private extra fields to your system model data structures, such as my_gamma_q_pars, to contain any parameters that your gamma_q might need.

It is assumed that qo and qdo do not satisfy the constraints accurately, e.g. because of integration truncation error.  For this reason, gamma_q returns the vectors qn and qdn, which are expected to satisfy the constraints more accurately than qo and qdo.  FDgq uses these vectors to calculate H and C, so that the equation of motion is not adversely affected by constraint-violation errors.  Ideally, qn and qdn should satisfy the constraints exactly.  However, it is enough that they be more accurate than qo and qdo.  The reason for accepting inexact values is this: some constraints cannot be expressed in the form of exact analytical expressions, but only as the roots of some nonlinear equation.  For such a constraint, qn can only be calculated via an iterative root-finding process, such as Newton-Raphson iteration.  Under such circumstances, it is convenient to allow qn to be the result of a single iteration with qo as the starting point.

One important consequence of integrating q" to obtain q' and q instead of integrating y" to obtain y' and y is that constraint-violation errors will gradually accumulate in q' and q.  This problem can be solved by including a constraint-stabilization term, g_stab, in the return value g_s.  This term is a function of the difference between qn and qo, and between qdn and qdo.  If both differences are zero then g_stab=0.  Otherwise, g_stab introduces a bias into q" that gradually reduces the differences as the integration proceeds.  Constraint stabilization is discussed in §8.3 of RBDA.

Writing gamma_q

The first step is to identify a vector of independent variables, y, and work out the formulae for γ(y), G and g.  Typically, y is a subset of the elements in q.  The next step is to decide how you are going to calculate y and y' from qo and qdo.  If y is a subset of q then this step is very easy: just extract the appropriate elements from qo and qdo.  Finally, you have to decide how you are going to stabilize constraint-violation errors.  If you choose to use the formula below, then the only decision you have to make is to pick a value for Tstab.  A value around 0.1 would be appropriate for a system that moves at the speed of a humanoid, and 0.01 for something that moves at the speed of a sewing machine.  The exact value is not critical—you only need to be roughly in the right ballpark.  You are now ready to write gamma_q.

Suggested Template

function  [q,qd,G,gs] = gamma_q( model, qo, qdo )

  y = formula for calculating y from qo;
  q = formula for gamma(y);

  G = Jacobian of gamma;

  yd = formula for calculating yd from qo (or y) and qdo;
  qd = G * yd;  (or a formula equivalent to this expression)

  g = formula for dG/dt * yd;

  Tstab = some suitable value, such as 0.1;
  gstab = 2/Tstab * (qd - qdo) + 1/Tstab^2 * (q - qo);

  gs = g + gstab;
end

An Example

The fuction below creates a modified version of the robot planar(2) in which q(2) is subject to the constraint q(2)=2*sin(q(1)).  The independent variable is chosen to be y=q(1).  If you want to try this example yourself, then try the following: set up a simulation in which the robot is subject to a constant joint torque tau=[1;0].  At the end of the simulation run, the work done by this torque will be equal to the final value of q(1); so you can check if the simulation is working properly by comparing this value with the calculated kinetic energy (e.g. use EnerMo).  The two should be the same modulo the integration truncation error.  Another example can be found in Simulink tutorial example 6.

function  robot = planar2g

  persistent memory;

  if length(memory) == 0
    memory = planar(2);
    memory.gamma_q = @gamma_q;
  end

  robot = memory;
end

function  [q,qd,G,gs] = gamma_q( model, qo, qdo )

  y = qo(1);
  q = [ y; 2*sin(y) ];

  G = [ 1; 2*cos(y) ];

  yd = qdo(1);
  qd = [ yd; 2*cos(y)*yd ];

  g = [ 0; -2*sin(y)*yd^2 ];

  Tstab = 0.1;
  gstab = 2/Tstab * (qd - qdo) + 1/Tstab^2 * (q - qo);

  gs = g + gstab;
end

Notes

1. When using constrained models in simulations, it is important to choose initial values of q and qd that satisfy the constraints.  One way to achieve this is to store suitable initial values directly in the model data structure (at the time it is created) so that they can be extracted by the m-code that initializes the Simulink model's workspace.  Another way is to use the model's gamma_q directly; i.e., the m-code calls model.gamma_q directly (with initial guesses for q and qd) and uses the returned values of q and qd as the initial values for the simulation.