Your browser was unable to load all of the resources. They may have been blocked by your firewall, proxy or browser configuration.
Press Ctrl+F5 or Ctrl+Shift+R to have your browser try again.

Is it possible to get TCP position if manipulator joint values are known? (Repost from Git Issues) #38

#1

qwasko1212 asks:

I need to convert robot joint rotations to Cartesian coordinates of the TCP.

Example:
joints: [val, val, val, val, val, val] → Cartesian coordinates: [x, y, z, roll, pitch, yaw]

The idea of this task is to compare real robot position with expected output. Real joints positions are read directly from the robot.
I have tried different methods, but none of them lead me to the proper result. Is it possible to perform this conversion using Python klampt?

  • replies 3
  • views 221
  • likes 0
krishauser · Team Author
#2

Yes, it just takes a few lines of code.

from klampt import *
from klampt.math import so3

#setup
world = WorldModel()
world.loadFile("myrobot.urdf")
robot = world.robot(0)

#query
endEffectorLink = robot.link(robot.numLinks()-1)
robot.setConfig(joints)
x,y,z = endEffectorLink.getWorldPosition([toolx,tooly,toolz])
rotation = endEffectorLink.getTransform()[0]
roll,pitch,yaw = so3.rpy(rotation)
#3

Thank you for your reply. I have a question. What is [toolx,tooly,toolz]?

krishauser · Team Author
#4

These would be the tool coordinates in the local frame of the end effector. (Default would be 0,0,0)