Skip to content

Instantly share code, notes, and snippets.

@kamikat
Created September 15, 2024 16:29
Show Gist options
  • Save kamikat/1a433c235702ca75dcb6d5c0d963900c to your computer and use it in GitHub Desktop.
Save kamikat/1a433c235702ca75dcb6d5c0d963900c to your computer and use it in GitHub Desktop.
Forward kinematics with `/compute_fk` interface.
import typing
import rospy
import moveit_commander
import moveit_msgs.srv
def moveit_compute_fk(
move_group: moveit_commander.MoveGroupCommander,
joint_position: typing.List[float],
ee_link = "tool0"
):
compute_fk = rospy.ServiceProxy("compute_fk", moveit_msgs.srv.GetPositionFK)
req = moveit_msgs.srv.GetPositionFKRequest()
req.header.frame_id = "base_link"
req.fk_link_names = [ee_link]
req.robot_state = move_group.get_current_state()
req.robot_state.joint_state.position = joint_position
return compute_fk(req).pose_stamped[0].pose
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment