Squirrels have fuzzy tails.
0 Members and 1 Guest are viewing this topic.
'mx = X 'my = up (Z) 'mz = Y 'p = Wrist angle (against ground-plane) Private Function InverseKinematics(mx As Single, my As Single, mz As Single, p As Single) As Byte Dim r, pRad, rb, yb, q, p1, p2 As Single Dim temp_check As Single On Error GoTo Calculations_Failed r = Sqr(mx * mx + mz * mz) ' horizontal distance to target baseAngle = atan2(-mz, -mx) ' angle to the target pRad = p * 0.0174532925 ' Degrees to Radians rb = ((r - L3 * Cos(pRad)) / (2 * L1)) yb = ((my - H - L3 * Sin(pRad)) / (2 * L1)) q = (Sqr(1 / (rb * rb + yb * yb) - 1)) p1 = (atan2(yb + q * rb, rb - q * yb)) p2 = (atan2(yb - q * rb, rb + q * yb)) shoulder = p1 - (90 * 0.0174532925) ' angle of the shoulder joint elbow = p2 - shoulder ' angle of the elbow joint wrist = pRad - p2 ' angle of the wrist joint ' Convert all values to degrees baseAngle = baseAngle * 57.2957795 shoulder = shoulder * 57.2957795 elbow = elbow * 57.2957795 elbow = (elbow - 90) * -1 wrist = wrist * 57.2957795 wrist = wrist + 17 ' Correct the over-shooting (this is done to match the AREXX Robot) ' Invert every joint except the base (this is done to match the AREXX Robot) shoulder = shoulder * -1 elbow = elbow * -1 wrist = wrist * -1 ' Check to make sure that the solution is valid If (rb * rb + yb * yb) > 1 Then ' Impossible due to limited arm length! InverseKinematics = 1 Exit Function End If If Abs(baseAngle) > 90 Then 'Impossible due to limited servo range! InverseKinematics = 2 Exit Function End If If Abs(shoulder) > 90 Then 'Impossible due to limited servo range! InverseKinematics = 2 Exit Function End If If Abs(elbow) > 90 Then 'Impossible due to limited servo range! InverseKinematics = 2 Exit Function End If If Abs(wrist) > 90 Then 'Impossible due to limited servo range! InverseKinematics = 2 Exit Function End If InverseKinematics = 0 Exit FunctionCalculations_Failed: InverseKinematics = 3 End Function
Isn't it possible to make something which inverts the robot when it comes over the base servo limit.You know, if I want to have the robot go to a point where the base servo have to be 100 degrees, isn't it the possible to make the base servo go to -80 degree and then invert the other servo's?
Sorry to correct you, but it's a 4DOF (base, shoulder, elbow, wrist) with a 2DOF gripper (rotate wrist and open/close)
A simple if statement? Could you please explain a little bit more, or make a short example?