diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index 9e3ba2082..c34a02460 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -3,8 +3,6 @@ RobotDescriptionManager, CameraDescription from ..datastructures.enums import Arms, Grasp, GripperState, GripperType, TorsoState from ..ros.ros_tools import get_ros_package_path -from tf.transformations import quaternion_from_euler -import numpy as np from ..helper import get_robot_mjcf_path @@ -88,12 +86,12 @@ pr2_description.add_kinematic_chain("neck", "head_pan_link", "head_tilt_link") ################################# Grasps ################################## -pr2_description.add_grasp_orientations({Grasp.FRONT: list(quaternion_from_euler(0, 0, 0)), - Grasp.BACK: list(quaternion_from_euler(0, 0, np.pi)), - Grasp.LEFT: list(quaternion_from_euler(0, 0, -np.pi/2)), - Grasp.RIGHT: list(quaternion_from_euler(0, 0, np.pi/2)), - Grasp.TOP: list(quaternion_from_euler(0, np.pi/2, 0)), - Grasp.BOTTOM: list(quaternion_from_euler(0, -np.pi/2, 0))}) +pr2_description.add_grasp_orientations({Grasp.FRONT: [0.0, 0.0, 0.0, 1.0], + Grasp.BACK: [0.0, 0.0, 1.0, 0.0], + Grasp.LEFT: [0.0, 0.0, -0.7071067811865475, 0.7071067811865476], + Grasp.RIGHT: [0.0, 0.0, 0.7071067811865475, 0.7071067811865476], + Grasp.TOP: [0.0, 0.7071067811865475, 0.0, 0.7071067811865476], + Grasp.BOTTOM: [0.0, -0.7071067811865475, 0.0, 0.7071067811865476]}) # Add to RobotDescriptionManager rdm = RobotDescriptionManager()