Updated transform function

def transform2(
    ee_pose: tuple,
    seg_longitudes: tuple,
    offset_xyz=(0, 0, 0),
) -> tuple:
    cant_joints = 4
    assert len(ee_pose) == 4
    assert len(seg_longitudes) == cant_joints
    assert len(offset_xyz) == 3

    x, y, z, w = ee_pose
    l1, l2, l3, l4 = seg_longitudes
    offset_x, offset_y, offset_z = offset_xyz
    phi = arctan2(JOINT_3_ORIGIN[2], JOINT_3_ORIGIN[0])

    x = x - offset_x
    y = y - offset_y
    z = z - offset_z

    q1 = arctan2(y, x)

    r = norm([x, y]) - l4 * cos(w)
    h = l1 - z - l4 * sin(w)

    q2 = phi + arctan2(h,r) - arccos((r**2 + h**2 + l2**2 - l3**2) / (2*l2*norm([r, h])))
    q3 = arctan2(h - l2*sin(q2-phi), r - l2*cos(q2-phi)) - q2
    q4 = w - q2 - q3

    return (q1, q2, q3, q4)

Originally posted by @JuanM04 in #24 (comment)

The GIF with the most votes wins. Vote by reacting with 👍🏻 for gif 1️⃣ or 👎🏻 for gif 2️⃣.

The poll is made within github so that we can see how it will actually look like once the repo is public.

Gazebo simulation screen:

RViz visualizer screen:

@santiago1311 @JuanM04 @OakNimroW

There is tons of tildes missing in messages in verbose mode, like in every occurence of posición, or ángulo.

• (#32) Create a service that using the general Denavit–Hartenberg matrix converts robot parameters to global coordinates.
• #34 Implement a frontend tool to display the end-effector position from the current robot pose (either by subscribing to a pose topic or making a request at the time of the calculation)

Implement a frontend tool to display the end-effector position from the current robot pose (either by subscribing to a pose topic or making a request at the time of the calculation)