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.
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
gamma_q can now be defined as follows: it is a function to compute γ(y), G and gs=g+gstab from q and q', where gstab is a stabilization term described below. The calling convention for gamma_q is
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 gs 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, gstab, in the return value gs. This term is a function of the difference between qn and qo, and between qdn and qdo. If both differences are zero then gstab=0. Otherwise, gstab introduces a bias into q" that gradually reduces the differences as the integration proceeds. Constraint stabilization is discussed in §8.3 of RBDA.
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.
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
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