Merge pull request #103 from sunava/pickup

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

src/pycram/designator.py

@@ -6,6 +6,7 @@
 from copy import copy
 from inspect import isgenerator, isgeneratorfunction
 
+import rospy
 import sqlalchemy
 
 from .bullet_world import (Object as BulletWorldObject, BulletWorld)
@@ -14,6 +15,7 @@
 from time import time
 from typing import List, Dict, Any, Type, Optional, Union, get_type_hints, Callable, Tuple, Iterable
 
+from .local_transformer import LocalTransformer
 from .pose import Pose
 from .robot_descriptions import robot_description
 
@@ -573,6 +575,18 @@ 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 +693,50 @@ 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: str, pose: Pose) -> Pose:
+            """
+            Returns the adjusted target pose based on special knowledge for "grasp front".
+
+            :param grasp: From which side the object should be grasped
+            :param pose: Pose at which the object should be grasped, before adjustment
+            :return: The adjusted grasp pose
+            """
+            lt = LocalTransformer()
+            pose_in_object = lt.transform_to_object_frame(pose, self.bullet_world_object)
+
+            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_in_object.pose.position.x += value[0]
+                    pose_in_object.pose.position.y += value[1]
+                    pose_in_object.pose.position.z += value[2]
+                    rospy.loginfo("Adjusted target pose based on special knowledge for grasp: ", grasp)
+                    return pose_in_object
+            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,6 +20,7 @@
 from ..designator import ActionDesignatorDescription
 from ..bullet_world import BulletWorld
 from ..pose import Pose
+from ..helper import multiply_quaternions
 
 
 class MoveTorsoAction(ActionDesignatorDescription):
@@ -251,7 +252,7 @@ def __init__(self, arms: List[Arms], resolver=None):
         :param arms: A list of possible arms, that could be used
         :param resolver: An optional resolver that returns a performable designator from the designator description
         """
-        super(ParkArmsAction, self).__init__(resolver)
+        super().__init__(resolver)
         self.arms: List[Arms] = arms
 
     def ground(self) -> Action:
@@ -294,8 +295,69 @@ 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 object frame, basically the origin of the object 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 = 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"
+            gripper_frame = robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm))
+            # First rotate the gripper, so the further calculations makes sense
+            tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame)
+            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,10 +376,8 @@ def insert(self, session: sqlalchemy.orm.session.Session, **kwargs):
 
             return action
 
-    def __init__(self,
-                 object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object],
-                 arms: List[str],
-                 grasps: List[str], resolver=None):
+    def __init__(self, object_designator_description:  Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object],
+                 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
         picked up, an arm that should be used as well as the grasp from which side the object should be picked up.
@@ -327,9 +387,8 @@ def __init__(self,
         :param grasps: List of possible grasps for the object
         :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
+        super().__init__(resolver)
+        self.object_designator_description: ObjectDesignatorDescription = object_designator_description
         self.arms: List[str] = arms
         self.grasps: List[str] = grasps
 
@@ -424,8 +483,7 @@ def ground(self) -> Action:
         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.target_locations[0])
+        return self.Action(obj_desig, self.arms[0], self.target_locations[0])
 
 
 class NavigateAction(ActionDesignatorDescription):

src/pycram/helper.py

@@ -18,10 +18,10 @@
 from macropy.core.quotes import ast_literal, q
 from .bullet_world import Object as BulletWorldObject
 from .local_transformer import LocalTransformer
-from .pose import Transform
+from .pose import Transform, Pose
 from .robot_descriptions import robot_description
 import os
-
+import math
 
 class bcolors:
     """
@@ -169,3 +169,74 @@ def has(self, index: int) -> bool:
             return True
         except StopIteration:
             return False
+
+
+def axis_angle_to_quaternion(axis: List, angle: float) -> Tuple:
+    """
+    Convert axis-angle to quaternion.
+
+    :param axis: (x, y, z) tuple representing rotation axis.
+    :param angle: rotation angle in degree
+    :return: The quaternion representing the axis angle
+    """
+    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) -> List:
+    """
+    Multiply two quaternions using the robotics convention (x, y, z, w).
+
+    :param q1: The first quaternion
+    :param q2: The second quaternion
+    :return: The quaternion resulting from the multiplication
+    """
+    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: List, v: List) -> List:
+    """
+    Rotate a vector v using quaternion q.
+
+    :param q: A quaternion of how v should be rotated
+    :param v: A vector that should be rotated by q
+    :return: V rotated by Q as a quaternion
+    """
+    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: Pose, pose2: Pose) -> Tuple:
+    """
+    Multiply two poses.
+
+    :param pose1: first Pose that should be multiplied
+    :param pose2: Second Pose that should be multiplied
+    :return: A Tuple of position and quaternion as result of the multiplication
+    """
+    pos1, quat1 = pose1.pose.position, pose1.pose.orientation
+    pos2, quat2 = pose2.pose.position, pose2.pose.orientation
+    # 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