Merge pull request #5 from Tigul/vanessa-pickup-fix

[action-desig] Changes for the PR and pickup plan fix

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
 
@@ -572,6 +574,7 @@ def ground(self) -> Location:
         """
         raise NotImplementedError(f"{type(self)}.ground() is not implemented.")
 
+
 #this knowledge should be somewhere else i guess
 SPECIAL_KNOWLEDGE = {
     'bigknife':
@@ -582,6 +585,8 @@ def ground(self) -> Location:
         [("front", [1.0, 2.0, 3.0]),
          ("key2", [4.0, 5.0, 6.0])]
 }
+
+
 class ObjectDesignatorDescription(DesignatorDescription):
     """
     Class for object designator descriptions.
@@ -688,29 +693,16 @@ 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):
+        def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> 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".
+            :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:
@@ -719,13 +711,32 @@ def special_knowledge(self, grasp, pose):
             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
+                    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,7 +20,8 @@
 from ..designator import ActionDesignatorDescription
 from ..bullet_world import BulletWorld
 from ..pose import Pose
-import pycram.helper as helper
+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:
@@ -301,17 +302,16 @@ def perform(self) -> None:
             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
+            # 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 = helper.multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y,
+            ori = multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y,
                                         adjusted_oTm.orientation.z, adjusted_oTm.orientation.w],
                                        grasp)
 
@@ -322,19 +322,18 @@ def perform(self) -> None:
             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
+            # 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)
-            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()
+            # 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
@@ -360,7 +359,6 @@ def perform(self) -> None:
             # 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)
 
@@ -378,8 +376,8 @@ def insert(self, session: sqlalchemy.orm.session.Session, **kwargs):
 
             return action
 
-    def __init__(self, object_designator_description: ObjectDesignatorDescription, 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.
@@ -389,7 +387,7 @@ def __init__(self, object_designator_description: ObjectDesignatorDescription, a
         :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)
+        super().__init__(resolver)
         self.object_designator_description: ObjectDesignatorDescription = object_designator_description
         self.arms: List[str] = arms
         self.grasps: List[str] = grasps
@@ -400,7 +398,11 @@ def ground(self) -> Action:
 
         :return: A performable designator
         """
-        return self.Action(self.object_designator_description.ground(), self.arms[0], self.grasps[0])
+        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])
+
 
 class PlaceAction(ActionDesignatorDescription):
     """
@@ -481,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,7 +18,7 @@
 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
@@ -169,11 +169,15 @@ def has(self, index: int) -> bool:
             return True
         except StopIteration:
             return False
-def axis_angle_to_quaternion(axis, angle):
+
+
+def axis_angle_to_quaternion(axis: List, angle: float) -> Tuple:
     """
     Convert axis-angle to quaternion.
-    axis: (x, y, z) tuple representing rotation axis.
-    angle: rotation angle in degree
+
+    :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]))
@@ -186,10 +190,14 @@ def axis_angle_to_quaternion(axis, angle):
 
     return (x, y, z, w)
 
-def multiply_quaternions(q1, q2):
+
+def multiply_quaternions(q1, q2) -> List:
     """
     Multiply two quaternions using the robotics convention (x, y, z, w).
-    q1, q2: tuple representing quaternion.
+
+    :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
@@ -201,23 +209,30 @@ def multiply_quaternions(q1, q2):
 
     return (x, y, z, w)
 
-def quaternion_rotate(q, v):
+
+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, pose2):
+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
     """
-    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)