[action-desig-pickup]added everything for the pickup similar to cram

src/pycram/designator.py

@@ -572,7 +572,16 @@ def ground(self) -> Location:
         """
         raise NotImplementedError(f"{type(self)}.ground() is not implemented.")
 
-
+#this knowledge should be somewhere else i guess
+SPECIAL_KNOWLEDGE = {
+    'bigknife':
+        [("top", [-0.08, 0, 0])],
+    'whisk':
+        [("top", [-0.08, 0, 0])],
+    'bowl':
+        [("front", [1.0, 2.0, 3.0]),
+         ("key2", [4.0, 5.0, 6.0])]
+}
 class ObjectDesignatorDescription(DesignatorDescription):
     """
     Class for object designator descriptions.
@@ -679,6 +688,44 @@ def __repr__(self):
                 [f"{f.name}={self.__getattribute__(f.name)}" for f in dataclasses.fields(self)] + [
                     f"pose={self.pose}"]) + ')'
 
+        def special_knowledge_adjustment_pose(self, grasp, pose):
+            """
+            Returns the adjusted target pose based on special knowledge for "grasp front".
+            """
+
+            special_knowledge = []  # Initialize as an empty list
+            if self.type in SPECIAL_KNOWLEDGE:
+                special_knowledge = SPECIAL_KNOWLEDGE[self.type]
+
+            for key, value in special_knowledge:
+                if key == grasp:
+                    # Adjust target pose based on special knowledge
+                    pose.pose.position.x += value[0]
+                    pose.pose.position.y += value[1]
+                    pose.pose.position.z += value[2]
+                    print("Adjusted target pose based on special knowledge for grasp: ", grasp)
+                    return pose
+            return pose
+
+        def special_knowledge(self, grasp, pose):
+            """
+            Returns t special knowledge for "grasp front".
+            """
+
+            special_knowledge = []  # Initialize as an empty list
+            if self.type in SPECIAL_KNOWLEDGE:
+                special_knowledge = SPECIAL_KNOWLEDGE[self.type]
+
+            for key, value in special_knowledge:
+                if key == grasp:
+                    # Adjust target pose based on special knowledge
+                    pose.pose.position.x += value[0]
+                    pose.pose.position.y += value[1]
+                    pose.pose.position.z += value[2]
+                    print("Adjusted target pose based on special knowledge for grasp: ", grasp)
+                    return pose
+            return pose
+
     def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] = None,
                  resolver: Optional[Callable] = None):
         """

src/pycram/designators/action_designator.py

@@ -20,7 +20,7 @@
 from ..designator import ActionDesignatorDescription
 from ..bullet_world import BulletWorld
 from ..pose import Pose
-
+import pycram.helper as helper
 
 class MoveTorsoAction(ActionDesignatorDescription):
     """
@@ -294,8 +294,72 @@ class Action(ActionDesignatorDescription.Action):
 
         @with_tree
         def perform(self) -> None:
+            # Store the object's data copy at execution
             self.object_at_execution = self.object_designator.data_copy()
-            PickUpMotion(object_desig=self.object_designator, arm=self.arm, grasp=self.grasp).resolve().perform()
+            robot = BulletWorld.robot
+            # Retrieve object and robot from designators
+            object = self.object_designator.bullet_world_object
+            # Get grasp orientation and target pose
+            grasp = robot_description.grasps.get_orientation_for_grasp(self.grasp)
+
+            # oTm = Object Pose in Frame map
+            oTm = object.get_pose()
+            # Transform the object pose to the map frame
+            mTo = object.local_transformer.transform_to_object_frame(oTm, object)
+            # Adjust the pose according to the special knowledge of the object designator
+            adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo)
+            # Transform the adjusted pose to the map frame
+            adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map")
+            # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper
+            ori = helper.multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y,
+                                        adjusted_oTm.orientation.z, adjusted_oTm.orientation.w],
+                                       grasp)
+
+            # Set the orientation of the object pose by grasp in MAP
+            adjusted_oTm.orientation.x = ori[0]
+            adjusted_oTm.orientation.y = ori[1]
+            adjusted_oTm.orientation.z = ori[2]
+            adjusted_oTm.orientation.w = ori[3]
+
+            # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh
+            gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame"
+            print(adjusted_oTm)
+            # First rotate the gripper, so the further calculations maakes sense
+            tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame)
+            print(tmp_for_rotate_pose)
+            tmp_for_rotate_pose.pose.position.x = 0
+            tmp_for_rotate_pose.pose.position.y = 0
+            tmp_for_rotate_pose.pose.position.z = -0.1
+            gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map")
+
+            #Perform Gripper Rotate
+            BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose)
+            MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform()
+
+            oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame)
+            oTg.pose.position.x -= 0.1 # in x since this is how the gripper is oriented
+            prepose = object.local_transformer.transform_pose(oTg, "map")
+
+            # Perform the motion with the prepose and open gripper
+            BulletWorld.current_bullet_world.add_vis_axis(prepose)
+            MoveTCPMotion(prepose, self.arm).resolve().perform()
+            MoveGripperMotion(motion="open", gripper=self.arm).resolve().perform()
+
+            # Perform the motion with the adjusted pose -> actual grasp and close gripper
+            BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm)
+            MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform()
+            adjusted_oTm.pose.position.z += 0.03
+            MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform()
+            tool_frame = robot_description.get_tool_frame(self.arm)
+            robot.attach(object, tool_frame)
+
+            # Lift object
+            BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm)
+            MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform()
+
+            # Remove the vis axis from the world
+            BulletWorld.current_bullet_world.remove_vis_axis()
+
 
         def to_sql(self) -> ORMPickUpAction:
             return ORMPickUpAction(self.arm, self.grasp)
@@ -314,9 +378,7 @@ def insert(self, session: sqlalchemy.orm.session.Session, **kwargs):
 
             return action
 
-    def __init__(self,
-                 object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object],
-                 arms: List[str],
+    def __init__(self, object_designator_description: ObjectDesignatorDescription, arms: List[str],
                  grasps: List[str], resolver=None):
         """
         Lets the robot pick up an object. The description needs an object designator describing the object that should be
@@ -328,8 +390,7 @@ def __init__(self,
         :param resolver: An optional resolver that returns a performable designator with elements from the lists of possible paramter
         """
         super(PickUpAction, self).__init__(resolver)
-        self.object_designator_description: Union[
-            ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description
+        self.object_designator_description: ObjectDesignatorDescription = object_designator_description
         self.arms: List[str] = arms
         self.grasps: List[str] = grasps
 
@@ -339,11 +400,7 @@ def ground(self) -> Action:
 
         :return: A performable designator
         """
-        obj_desig = self.object_designator_description if isinstance(self.object_designator_description,
-                                                                     ObjectDesignatorDescription.Object) else self.object_designator_description.resolve()
-
-        return self.Action(obj_desig, self.arms[0], self.grasps[0])
-
+        return self.Action(self.object_designator_description.ground(), self.arms[0], self.grasps[0])
 
 class PlaceAction(ActionDesignatorDescription):
     """

src/pycram/helper.py

@@ -21,7 +21,7 @@
 from .pose import Transform
 from .robot_descriptions import robot_description
 import os
-
+import math
 
 class bcolors:
     """
@@ -169,3 +169,59 @@ def has(self, index: int) -> bool:
             return True
         except StopIteration:
             return False
+def axis_angle_to_quaternion(axis, angle):
+    """
+    Convert axis-angle to quaternion.
+    axis: (x, y, z) tuple representing rotation axis.
+    angle: rotation angle in degree
+    """
+    angle = math.radians(angle)
+    axis_length = math.sqrt(sum([i ** 2 for i in axis]))
+    normalized_axis = tuple(i / axis_length for i in axis)
+
+    x = normalized_axis[0] * math.sin(angle / 2)
+    y = normalized_axis[1] * math.sin(angle / 2)
+    z = normalized_axis[2] * math.sin(angle / 2)
+    w = math.cos(angle / 2)
+
+    return (x, y, z, w)
+
+def multiply_quaternions(q1, q2):
+    """
+    Multiply two quaternions using the robotics convention (x, y, z, w).
+    q1, q2: tuple representing quaternion.
+    """
+    x1, y1, z1, w1 = q1
+    x2, y2, z2, w2 = q2
+
+    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
+    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
+    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
+    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
+
+    return (x, y, z, w)
+
+def quaternion_rotate(q, v):
+    """
+    Rotate a vector v using quaternion q.
+    """
+    q_conj = (-q[0], -q[1], -q[2], q[3])  # Conjugate of the quaternion
+    v_quat = (*v, 0)  # Represent the vector as a quaternion with w=0
+    return multiply_quaternions(multiply_quaternions(q, v_quat), q_conj)[:3]
+
+
+def multiply_poses(pose1, pose2):
+    """
+    Multiply two poses.
+    """
+    print(pose1, pose2)
+    pos1, quat1 = pose1.pose.position, pose1.pose.orientation
+    pos2, quat2 = pose2.pose.position, pose2.pose.orientation
+    print(pos1, quat1, pos2, quat2)
+    # Multiply the orientations
+    new_quat = multiply_quaternions(quat1, quat2)
+
+    # Transform the position
+    new_pos = np.add(pos1, quaternion_rotate(quat1, pos2))
+
+    return new_pos, new_quat