FUNCTION SMC_SolveTwoRotJoints : BOOL
Solves the inverse kinematics for two parallel rotatory joints in sequence. The following situation is assumed:
v _
\ _
b \ _ J2
/ x
q2 / \ .
/ \ q1 .
a \ .
\ .
\ .
\. J1
x <-----+ 0
|
|
v y
Note
q1 and q2 are measured around the z-axis, i.e. counter-clockwise in the above diagram. If the links a and b are perpendicular, q2 is zero.
The function returns true if (and only if) a solution exists. Otherwise, the point v is not in the working-space.
q1 is assumed to be in the range ]-pi, pi], q2 in ]-pi, pi/2[.
InOut:
|
Scope |
Name |
Type |
Comment |
|
Return |
SMC_SolveTwoRotJoints |
BOOL |
|
|
Input |
dVX |
LREAL |
The x-position of the target |
|
dVY |
LREAL |
The y-position of the target |
|
|
dA |
LREAL |
The length of the first link, must be positive |
|
|
dB |
LREAL |
The length of the second link, must be positive |
|
|
bElbowLow |
BOOL |
To disambiguate the two possible configurations |
|
|
Output |
q1 |
LREAL |
The angle of joint 1 in radian, in the range ]-p2, pi] |
|
q2 |
LREAL |
The angle of joint 2 in radian, in the range ]-pi, pi/2[ |
