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 3dof planar robot

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

    # Calculate oc
    xc = xyz[0]
    yc = xyz[1]
    zc = xyz[2]
    if throw > 0.386 and throw < 25: 
        a3 = throw
    elif throw == 0:
        a3 = 0.386
        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

Test figure1 Test figure2 Test figure3


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.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.