SMC_Trafo_HandleWristSingularity (FUN)
FUNCTION SMC_Trafo_HandleWristSingularity : BOOL
Computes angles for q3 and q5 of an 6DOF articulated robot in a singular wrist configuration (q4 ~ 0). The angles are computed so that (q3 - q3_)^2 + (q5 - q5_)^2 is minimized under the following constraints:
q3 + q5 = alpha (the orientation is correct)
q3_min <= q3 <= q3_max, q5_min <= q5 <= q5_max (the angles are in the valid range)
It is assumed that q3_ and q5_ are in the valid range. Returns FALSE if (and only if) there is no solution in the valid range.
InOut:
|
Scope |
Name |
Type |
Comment |
|
Return |
SMC_Trafo_HandleWristSingularity |
BOOL |
|
|
Input |
q3_ |
LREAL |
The old orientation of axis 3 |
|
q5_ |
LREAL |
The old orientation of axis 5 |
|
|
alpha |
LREAL |
The desired sum of q3 and q5 |
|
|
q3_min |
LREAL |
The minimum value for axis 3 |
|
|
q3_max |
LREAL |
The maximum value for axis 3 |
|
|
q5_min |
LREAL |
The minimum value for axis 5 |
|
|
q5_max |
LREAL |
The maximum value for axis 5 |
|
|
Output |
q3 |
LREAL |
The new value for axis 3 |
|
q5 |
LREAL |
The new value for axis 5 |
