diff --git a/src/pycram/designator.py b/src/pycram/designator.py index d772c6be2..83ba2d1ac 100644 --- a/src/pycram/designator.py +++ b/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): """ diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 397fce5ad..a565841e5 100644 --- a/src/pycram/designators/action_designator.py +++ b/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): diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 22f6a9c5d..64acc9cad 100644 --- a/src/pycram/helper.py +++ b/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 \ No newline at end of file