Coordinate systems in the swift backend

[XiaoP76]

Hi !

I'd like to show coordinate systems at each joint.
I tried this approach:

robot = rtb.models.LBR()
env.add(robot, robot_alpha=0.5)

for link in robot.links:
    if link.isjoint:
        l_frame = sg.Axes(0.1)
        l_frame.attach_to(link)
        env.add(l_frame)

but I get the coordinate systems at the base of the link, not at its end.
It would be great to work with the attach_to() mechanism so that the coordinate systems are updated automatically when the robot moves.

How would you approach this with the swift backend ?

Best regards

Peter

[petercorke]

Hi @XiaoP76, thank you for your interest in RTB, and apologies for the very long delay in replying. Various life events got in the way, and since May this year I've been doing a lot of work across RTB, Swift and SpatialGeometry.

Your approach was right: attach_to() does what you want, and the frames follow the robot automatically. I checked that after moving the arm, each attached Axes sits exactly on robot.fkine(q, end=link).

What you saw is a matter of convention. For URDF robots a link's frame is on the link side of the joint that connects it to the preceding link. So the frame attached to the link for joint j is at joint j, with its z axis along the joint axis, and it moves with joint j. That is where the link's mesh starts, so it looks like "the base of the link", but it is the correct place to show the coordinate frame of joint j.

If you want a frame at the distal end of the link instead, where joint j+1 sits, attach it to the same link with a constant offset. Do not attach it to the next link: that link's frame moves with joint j+1, so the frame would swing with a joint that is not part of this link.

import roboticstoolbox as rtb
import spatialgeometry as sg
from spatialmath import SE3

robot = rtb.models.LBR()
env.add(robot, robot_alpha=0.5)

def distal_offset(link):
    """Pose of the end of `link` (where its child's joint sits), in link's frame"""
    T = SE3()
    for et in link.children[0].ets:   # constants before the child's joint variable
        if et.isjoint:
            break
        T = T * SE3(et.A())
    return T

for link in robot.links:
    if link.isjoint:
        j = link.jindex

        frame_j = sg.Axes(0.1)                          # frame of joint j
        frame_j.attach_to(link)

        frame_end = sg.Axes(0.1, pose=distal_offset(link))   # distal end of link j
        frame_end.attach_to(link)

        env.add(frame_j)
        env.add(frame_end)

Note that on the LBR some adjacent joints are coincident (the shoulder and wrist are spherical), so some pairs of frames will sit on top of each other.

Axes also has options such as arrows=True, with radius= and linewidth=, which render the axes as colored arrows.

Writing out the offset loop by hand is clumsy for what is a reasonable thing to want, so I've opened an enhancement Issue for a helper: #726. If you hit a problem with any of this, please post an Issue rather than a discussion thread. Thanks for the question.