Plot Frames using Switch with an URDF Model

[rbousigues]

Hello,

Is there a way to plot directly the XYZ vector describing the frame for each body in the specified .urdf file using Switch ?

To start, I tried to use spatialmath.base trplot function to superpose the end-effector frame on my model but it opens a new window...

A simple example to work on could be:

import roboticstoolbox as rtb
import spatialmath as sm
from spatialmath.base import trplot
import numpy as np
import matplotlib.pyplot as plt

panda = rtb.models.URDF.Panda() # Load Panda model using its URDF description
q_test = panda.qz # use neutral pose q=[0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]
panda.plot(q = q_test, block=False) # Show the robot with switch

T_test =  panda.fkine(q_test) # Obtain the current transformation of end-effector
print("T = ", T_test)
trplot(T_test.A)
plt.show()

Thank you for your help !
Best regards