Precalculate grasp angles

cram2 · Jan 15, 2025 · 52af09e · 52af09e
1 parent 8454970
commit 52af09e
Showing 1 changed file with 6 additions and 8 deletions.
diff --git 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()