I'm trying to make at Python script that calculates the inverse kinematics for at 3DoF planar robot. I've got the forwards kinematic working, but i am stuck at the inverse kinematics.
The problem is that q2 and q3 is actuated together by one hydraulic piston. with maximum angles being: q2 = 49 degrees q3 = 57 degrees
q2 and q3 only "bends" downwards. The fact that q3 is directly constrained by q2 makes it quite hard.
I've tried a few thing but the "best" working one, is only working in a narrow XYZ coordinate, and is off.
I've attached the currently "working" code. Ignore Throw and *0.675 when calculating q4
def invKinNew(xyz, throw): d1 = 3.3 # Rotation height a1 = 4.706 # Arm length a2 = 0.386 # Flap length a3 = 0.352 q3max=49 q4max=57 # Calculate oc xc = xyz yc = xyz zc = xyz if throw > 0.386 and throw < 25: a3 = throw elif throw == 0: a3 = 0.386 else: raise Exception("Throw outside border") # Calculate q1 which is the rotation and defined by X and Y q1 = math.atan2(yc, xc) # z can be negative sinde point can be under first joint height. s = (zc - d1) #Calculate q2 and q3. The 2 joints responsible for Z height distance away from origo r2 = xc**2 + yc**2 D = ( r2 + s**2 - a1**2 - a2**2 - a3**2)/(2*a1*a2*a3) q4 = math.atan2(-math.sqrt(1-D**2), D)*0.675 q3 = (q3max * q4)/q4max q2 = math.atan2(s, math.sqrt(r2)) - math.atan2(a2*math.sin(q3) + a3*math.sin(q4), a1 + a2*math.cos(q3)+ a3*math.cos(q4)) return math.degrees(q1), math.degrees(q2), math.degrees(q3), math.degrees(q4)
I've attached som test figure showing the behaviour