Week 17 - ngetachew/Portfolio GitHub Wiki

On Monday, the forward_kinematics function stopped working. I found that if you plug in 0 for any of the coordinates, it returned the correct angle,, but otherwise, it didn't work. I resorted to going back on the internet to find another alternative. Thankfully, a grad student from Ohio State did a research paper on Delta Arms and published it on the internet. However, his work was not well commented, so I found it difficult to implement it in my code. Here is a screenshot of it:

He also didn't bother to discuss the required measurements for his variables outside of a few diagrams, which left me and a mentor guessing what he meant.

The solution I found left me banging my head against the desk. I found another repository on GitHub that had a slightly different version of DeltaArm.py, so I copied in its forward_kinematics function, and it worked. Here is the final code: ` def forward_kinematics(theta1, theta2, theta3): rf = DeltaArm.upper_len re = DeltaArm.lower_len f = DeltaArm.fixed_edge e = DeltaArm.effector_edge #Finding J' points (centers of intersecting spheres) x1 = 0 y1 = (f - e) / (2 * math.sqrt(3)) + rf * math.cos(math.radians(theta1)) z1 = -rf * math.sin(math.radians(theta1)) (x1, y1, z1) = DeltaArm.rotate_point_to_yz_plane(x1, y1, z1, DeltaArm.phi_vals[0])

    x2 = 0
    y2 = (f - e) / (2 * math.sqrt(3)) + rf * math.cos(math.radians(theta2))
    z2 = -rf * math.sin(math.radians(theta2))
    (x2, y2, z2) = DeltaArm.rotate_point_to_yz_plane(x2, y2, z2, DeltaArm.phi_vals[1])

    x3 = 0
    y3 = (f - e) / (2 * math.sqrt(3)) + rf * math.cos(math.radians(theta3))
    z3 = -rf * math.sin(math.radians(theta3))
    (x3, y3, z3) = DeltaArm.rotate_point_to_yz_plane(x3, y3, z3, DeltaArm.phi_vals[2])

    # Find intersection of 3 spheres
    w1 = x1 ** 2 + y1 ** 2 + z1 ** 2
    w2 = x2 ** 2 + y2 ** 2 + z2 ** 2
    w3 = x3 ** 2 + y3 ** 2 + z3 ** 2

    # Coefficients in EQN x = a1*z + b1
    dnm = (x3 - x1) * (y2 - y1) - (x2 - x1) * (y3 - y1)

    a1 = ((z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1))
    b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2

    a2 = -((z2 - z1) * (x3 - x1) - (z3 - z1) * (x2 - x1))
    b2 = ((w1 - w2) * (x1 - x3) - (w1 - w3) * (x1 - x2)) / 2

    # Coefficients in Quadratic
    A = dnm ** 2 + a1 ** 2 + a2 ** 2
    B = 2 * (a1 * (b1 - x1 * dnm) + a2 * (b2 - y1 * dnm) - z1 * dnm ** 2)
    C = (b1 - x1 * dnm) ** 2 + (b2 - y1 * dnm) ** 2 + (z1 ** 2 - re ** 2) * dnm ** 2

    # Quadratic EQN
    disc = B ** 2 - 4 * A * C
    # discriminant < 0 -> no solution
    if disc < 0:
        return (-99, -99, -99)
    z = (-B - math.sqrt(disc)) / (2 * A)

    # Solve for x and y from z
    x = (a1 * z + b1) / dnm
    y = -(a2 * z + b2) / dnm

    # Fudge z for end effector height
    z = z - DeltaArm.end_effector_z_offset

    # GG EZ
    return (x, y, z)

`

I would like to go over this in detail at some later point in time, because I still have no idea what this does. With this, I was able to get the move_to_point_in_straight_line()` function to work. It is actually very clever, it uses the similar triangles theorem to calculate how much it needs to change its x,y, and z coordinates. It moves in very small increments to create the effect of moving in a straight line.