search

petercorke / robotics-toolbox-python

Robotics Toolbox for Python
MIT License
2.07k stars 432 forks source link

Inverse kinematics with a custom tool frame #452

Open guri-dominic opened 1 month ago

guri-dominic commented 1 month ago

Thank you for your work on this great software package!

I am working with the xarm6 urdf with no end-effector, and I would like to use a tool frame transform. For forward kinematics, I am using

Tool = sm.SE3.Tz(0.1)
ee = xarm.fkine(xarm.q, tool=Tool)

How do I include Tool in the ik solvers? Setting xarm.tool = Tool does not work, and there is no tool argument in the ik functions.

guri-dominic commented 1 month ago

I found a solution:

import roboticstoolbox as rbt
import spatialmath as sm

ets_tool_transform = rbt.ET.tz(0.1)

x6 = # (type = rtb.Robot), from URDF file
ets = x6.ets()
# ets = x6.ets().copy()   # could be better?
ets.insert(ets_tool_transform)

# Usage:
ets.fkine(q)
ets.ik_GN(T)  # T \in sm.SE3

I don't know if this is a bug, but using x6.fkine(..), x6.ik_GN(..), x6.ik_GN(..), etc. does not produce the same results as calling ets functions, even though updating ets also updates x6.ets().