import roboticstoolbox as rtb
from spatialmath import SE3 #Operaciones
from roboticstoolbox.backends.swift import Swift
import time
robot = rtb.models.DH.Panda()
robot
robot.qz
robot.qr
T = robot.fkine(robot.qz)
T
T = SE3(0.7, 0.2, 0.1)*SE3.OA([0,1,0],[0,0,-1])
T
solution = robot.ikine_LM(T)
solution
q_pickup = solution.q
robot.fkine(q_pickup)
qt = rtb.jtraj(robot.qz,q_pickup,50)
qt
backend = Swift()
backend.launch()
panda = rtb.models.URDF.Panda()
backend.add(panda)
for qk in qt.q:
panda.q = qk
backend.step()