Inverse Kinematic for 3DoF chain with 2 links constrained together

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.

L1: 3.3m, L2: 4.7m, L3: 0.386m, L4: 0.352m 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

I don't have the time to think this all the way through, but my gut reaction here is that you don't really have two degrees of freedom with those two joints because they're actuated together.

It's not clear to me what exactly you mean when you say, "q2 and q3 is actuated together by one hydraulic piston" - is there literally just one piston? If so then you should drop q2 and q3 and just use one joint for the hydraulic piston, where your q would be the linear extension of that piston. You'll wind up with some equations where your output link L4 is a function of the piston extension and the lengths of L2 and L3, but that's expected.

The math gets a bit more complex in a way, but that's what you need to do to satisfy the constraint.

In a DH parameter setup, instead of having one joint that has strictly a rotation component or strictly a translation component, I think you'll wind up with a "compound" joint where you have rotations and a translation that are some function of your joint position.

Hopefully this "hint" is enough to provoke some better answers :P

In general, when teaching my students how to do IK my first recommendation is always to find a variable (length/angle/value/etc) that is only a function of one joint so that they have one equation with one unknown. They can then look for another variable with is a function of one joint, or that is a function of two joints where one is the newly-solved-for joint variable.

In this case, the distance from joint 1 to the end of the arm is only a function of the hydraulic piston driving joints 2 and 3, so you should be able to formulate an equation that only has the length of the piston as an unknown if given the desired end-effector position (since the location of joint 1 is fixed).

Once you have solved for the hydraulic piston length, you also get the joint 2 and 3 positions (based on your initial description), which will allow you to then solve for the angle of joint 1 based on treating everything after that joint as a rigid body with a known length.