FUNCTION SMC_SolveEulerWrist : BOOL
Solves the inverse kinematics of an euler wrist, i.e. a robot hand built from three rotary joints whose axes intersect in a single point. The three joints 4, 5, and 6 are assumed to be arranged according to the following Denavit-Hartenberg parameters:
|
i |
Joint angle (offset) [deg] |
Link offset [u] |
Link length [u] |
Link twist [deg] |
|
4 |
0 |
d4 |
0 |
90 |
|
5 |
0 |
0 |
0 |
-90 |
|
6 |
0 |
d6 |
0 |
0 |
InOut:
|
Scope |
Name |
Type |
Comment |
|
Return |
SMC_SolveEulerWrist |
BOOL |
|
|
Inout Const |
mOTool |
The orientation of the tool as a matrix. The columns of the matrix are the unit vectors of the tool coordinate system, expressed in world coordinates. Used read-only. |
|
|
Input |
bHandDown |
BOOL |
To disambiguate the two possible configurations of joint 5 |
|
Output |
q3 |
LREAL |
The angle of joint 3 in rad. |
|
q4 |
LREAL |
The angle of joint 4 in rad. |
|
|
q5 |
LREAL |
The angle of joint 5 in rad. |
|
|
bSingularity |
BOOL |
True if q4 is (almost) zero. In this case q5 is set to zero (as there is no unique solution for q3 and q5). |
