[motion-exe] first wip on the motion executioner we will get rid of the process manager

Author: sunava

src/pycram/robot_execution/__init__.py

@@ -0,0 +1,131 @@
+from pycram.external_interfaces import giskard
+from pycram.robot_description import RobotDescription
+from pycram.world import World
+from pycram.local_transformer import LocalTransformer
+from pycram.datastructures.enums import MovementType, WaypointsMovementType
+from pycram.helpers import euler_from_quaternion
+from scipy.spatial.transform import Rotation as R
+import numpy as np
+
+# Falls du eigene Exceptions hast:
+from pycram.failures import NavigationGoalNotReachedError, ToolPoseNotReachedError
+
+def map_MoveMotion(motion):
+    giskard.avoid_all_collisions()
+    giskard.achieve_cartesian_goal(
+        motion.target,
+        RobotDescription.current_robot_description.base_link,
+        "map"
+    )
+    if not World.current_world.robot.pose.almost_equal(motion.target, 0.05, 3):
+        raise NavigationGoalNotReachedError(World.current_world.robot.pose, motion.target)
+
+def map_LookingMotion(motion):
+    target = motion.target
+    robot = World.robot
+
+    local_transformer = LocalTransformer()
+    neck = RobotDescription.current_robot_description.get_neck()
+    pan_link = neck["yaw"][0]
+    tilt_link = neck["pitch"][0]
+    pan_joint = neck["yaw"][1]
+    tilt_joint = neck["pitch"][1]
+
+    pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame(pan_link)).position.to_list()
+    pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame(tilt_link)).position.to_list()
+    new_pan = np.arctan2(pose_in_pan[1], pose_in_pan[0])
+
+    tilt_offset = RobotDescription.current_robot_description.get_offset(tilt_joint)
+    quaternion_list = [0, 0, 0, 1]
+    if tilt_offset:
+        q = tilt_offset.pose.orientation
+        quaternion_list = [q.x, q.y, q.z, q.w]
+
+    tilt_offset_rotation = euler_from_quaternion(quaternion_list, axes='sxyz')
+    adjusted_pose_in_tilt = R.from_euler('xyz', tilt_offset_rotation).apply(pose_in_tilt)
+    new_tilt = -np.arctan2(adjusted_pose_in_tilt[2], np.sqrt(adjusted_pose_in_tilt[0] ** 2 + adjusted_pose_in_tilt[1] ** 2))
+
+    if RobotDescription.current_robot_description.name in {"iCub", "tiago_dual"}:
+        new_tilt = -new_tilt
+
+    current_pan = robot.get_joint_position(pan_joint)
+    current_tilt = robot.get_joint_position(tilt_joint)
+
+    giskard.avoid_all_collisions()
+    giskard.achieve_joint_goal({
+        pan_joint: new_pan + current_pan,
+        tilt_joint: new_tilt + current_tilt
+    })
+
+def map_MoveTCPMotion(motion):
+    lt = LocalTransformer()
+    pose_in_map = lt.transform_pose(motion.target, "map")
+    tip_link = RobotDescription.current_robot_description.get_arm_chain(motion.arm).get_tool_frame()
+    root_link = "map"
+
+    gripper_that_can_collide = motion.arm if motion.allow_gripper_collision else None
+    if motion.allow_gripper_collision:
+        giskard.allow_gripper_collision(motion.arm)
+
+    if motion.movement_type == MovementType.STRAIGHT_TRANSLATION:
+        giskard.achieve_straight_translation_goal(pose_in_map.position.to_list(), tip_link, root_link)
+    elif motion.movement_type == MovementType.STRAIGHT_CARTESIAN:
+        giskard.achieve_straight_cartesian_goal(pose_in_map, tip_link, root_link)
+    elif motion.movement_type == MovementType.TRANSLATION:
+        giskard.achieve_translation_goal(pose_in_map.position.to_list(), tip_link, root_link)
+    elif motion.movement_type == MovementType.CARTESIAN:
+        giskard.achieve_cartesian_goal(pose_in_map, tip_link, root_link,
+                                       grippers_that_can_collide=gripper_that_can_collide)
+
+    if not World.current_world.robot.get_link_pose(tip_link).almost_equal(motion.target, 0.3, 3):
+        raise ToolPoseNotReachedError(World.current_world.robot.get_link_pose(tip_link), motion.target)
+
+def map_MoveArmJointsMotion(motion):
+    joint_goals = {}
+    if motion.left_arm_poses:
+        joint_goals.update(motion.left_arm_poses)
+    if motion.right_arm_poses:
+        joint_goals.update(motion.right_arm_poses)
+    giskard.avoid_all_collisions()
+    giskard.achieve_joint_goal(joint_goals)
+
+def map_MoveJointsMotion(motion):
+    name_to_position = dict(zip(motion.names, motion.positions))
+    giskard.avoid_all_collisions()
+    giskard.achieve_joint_goal(
+        name_to_position,
+        align=motion.align,
+        tip_link=motion.tip_link,
+        tip_normal=motion.tip_normal,
+        root_link=motion.root_link,
+        root_normal=motion.root_normal
+    )
+
+def map_OpeningMotion(motion):
+    giskard.achieve_open_container_goal(
+        RobotDescription.current_robot_description.get_arm_chain(motion.arm).get_tool_frame(),
+        motion.object_part.name
+    )
+
+def map_ClosingMotion(motion):
+    giskard.achieve_close_container_goal(
+        RobotDescription.current_robot_description.get_arm_chain(motion.arm).get_tool_frame(),
+        motion.object_part.name
+    )
+
+def map_MoveTCPWaypointsMotion(motion):
+    lt = LocalTransformer()
+    waypoints = [lt.transform_pose(x, "map") for x in motion.waypoints]
+    tip_link = RobotDescription.current_robot_description.get_arm_chain(motion.arm).get_tool_frame()
+    root_link = "map"
+
+    giskard.avoid_all_collisions()
+    if motion.allow_gripper_collision:
+        giskard.allow_gripper_collision(motion.arm)
+
+    giskard.achieve_cartesian_waypoints_goal(
+        waypoints=waypoints,
+        tip_link=tip_link,
+        root_link=root_link,
+        enforce_final_orientation=True if motion.movement_type == WaypointsMovementType.ENFORCE_ORIENTATION_FINAL_POINT else False
+    )

src/pycram/robot_execution/giskard_mapping.py

@@ -0,0 +1,18 @@
+from pycram.external_interfaces import giskard
+from pycram.plan import Plan, MotionNode
+from pycram.robot_description import RobotDescription
+from pycram.robot_plans import BaseMotion
+
+# --- generischer executor ---
+def execute_with_giskard(motion: BaseMotion):
+    func_name = f"map_{type(motion).__name__}"
+    mapper = globals().get(func_name)
+    if not callable(mapper):
+        raise ValueError(f"No Giskard mapper function found for {type(motion).__name__}")
+    mapper(motion)
+
+def execute_leaf_motions_with_giskard(plan: Plan):
+    for node in plan.nodes:
+        if isinstance(node, MotionNode) and not node.children:
+            print("Executing:", node.designator_ref)
+            execute_with_giskard(node.designator_ref)

src/pycram/robot_execution/motion_executioner.py

@@ -0,0 +1,4 @@
+from pycram.robot_plans.motions import MoveMotion
+from pycram.external_interfaces import giskard
+from pycram.robot_description import RobotDescription
+

src/pycram/robot_execution/motion_registry.py

@@ -2,9 +2,16 @@
 
 from .base import BaseMotion
 from ...datastructures.enums import Arms
+from ...datastructures.pose import PoseStamped
+from ...datastructures.world import World
 from ...description import ObjectDescription
+from ...external_interfaces.ik import request_ik
 from ...plan import with_plan
 from ...process_module import ProcessModuleManager
+from ...robot_description import RobotDescription
+from ...utils import _apply_ik
+from ...world_concepts.world_object import Object
+from ...world_reasoning import link_pose_for_joint_config
 
 
 @with_plan
@@ -24,8 +31,33 @@ class OpeningMotion(BaseMotion):
     """
 
     def perform(self):
-        pm_manager = ProcessModuleManager().get_manager()
-        return pm_manager.open().execute(self)
+        part_of_object = self.object_part.parent_entity
+
+        container_joint_name = part_of_object.find_joint_above_link(self.object_part.name)
+        lower_limit, upper_limit = part_of_object.get_joint_limits(container_joint_name)
+
+        goal_pose = link_pose_for_joint_config(part_of_object, {
+            container_joint_name: max(lower_limit, upper_limit - 0.05)}, self.object_part.name)
+
+        self._move_arm_tcp(goal_pose, World.robot, self.arm)
+
+        part_of_object.set_joint_position(container_joint_name, upper_limit)
+
+    def _move_arm_tcp(target: PoseStamped, robot: Object, arm: Arms, tip_link: str = None) -> None:
+        """
+        Calls the ik solver to calculate the inverse kinematics of the arm and then sets the joint states accordingly.
+
+        :param target: Target pose to which the end-effector should move.
+        :param robot: Robot object representing the robot.
+        :param arm: Which arm to move
+        """
+        if tip_link is None:
+            tip_link = RobotDescription.current_robot_description.get_arm_chain(arm).get_tool_frame()
+
+        joints = RobotDescription.current_robot_description.get_arm_chain(arm).joints
+
+        inv = request_ik(target, robot, joints, tip_link)
+        _apply_ik(robot, inv)
 
 
 @with_plan
@@ -45,5 +77,30 @@ class ClosingMotion(BaseMotion):
     """
 
     def perform(self):
-        pm_manager = ProcessModuleManager().get_manager()
-        return pm_manager.close().execute(self)
+        part_of_object = self.object_part.parent_entity
+
+        container_joint_name = part_of_object.find_joint_above_link(self.object_part.name)
+        lower_joint_limit = part_of_object.get_joint_limits(container_joint_name)[0]
+
+        goal_pose = link_pose_for_joint_config(part_of_object, {
+            container_joint_name: lower_joint_limit}, self.object_part.name)
+
+        self._move_arm_tcp(goal_pose, World.robot, self.arm)
+
+        part_of_object.set_joint_position(container_joint_name, lower_joint_limit)
+
+    def _move_arm_tcp(target: PoseStamped, robot: Object, arm: Arms, tip_link: str = None) -> None:
+        """
+        Calls the ik solver to calculate the inverse kinematics of the arm and then sets the joint states accordingly.
+
+        :param target: Target pose to which the end-effector should move.
+        :param robot: Robot object representing the robot.
+        :param arm: Which arm to move
+        """
+        if tip_link is None:
+            tip_link = RobotDescription.current_robot_description.get_arm_chain(arm).get_tool_frame()
+
+        joints = RobotDescription.current_robot_description.get_arm_chain(arm).joints
+
+        inv = request_ik(target, robot, joints, tip_link)
+        _apply_ik(robot, inv)

src/pycram/robot_plans/motions/container.py

@@ -5,14 +5,17 @@
 from ...datastructures.enums import Arms, GripperState, MovementType, WaypointsMovementType, Frame
 from ...datastructures.grasp import GraspDescription
 from ...datastructures.pose import PoseStamped
+from ...datastructures.world import World
+from ...external_interfaces.ik import request_ik
 from ...failure_handling import try_motion
 from ...failures import ToolPoseNotReachedError
 from ...local_transformer import LocalTransformer
 from ...plan import with_plan
 from ...process_module import ProcessModuleManager
+from ...robot_description import RobotDescription
+from ...utils import _apply_ik
 from ...world_concepts.world_object import Object
 
-
 # class ReachMotion(BaseMotion):
 #     """
 #     """
@@ -88,8 +91,15 @@ class MoveGripperMotion(BaseMotion):
     """
 
     def perform(self):
-        pm_manager = ProcessModuleManager().get_manager()
-        return pm_manager.move_gripper().execute(self)
+        robot_description = RobotDescription.current_robot_description
+        gripper = self.gripper
+        arm_chain = robot_description.get_arm_chain(gripper)
+        if arm_chain.end_effector.gripper_object_name is not None:
+            robot = World.current_world.get_object_by_name(arm_chain.end_effector.gripper_object_name)
+        else:
+            robot = World.robot
+        motion = self.motion
+        robot.set_multiple_joint_positions(arm_chain.get_static_gripper_state(motion))
 
 
 @with_plan
@@ -117,9 +127,26 @@ class MoveTCPMotion(BaseMotion):
     """
 
     def perform(self):
-        pm_manager = ProcessModuleManager().get_manager()
-        try_motion(pm_manager.move_tcp(), self, ToolPoseNotReachedError)
+        target = self.target
+        robot = World.robot
+        arm = self.arm
+        self.move_arm_tcp(target=target, robot=robot, arm=arm)
+
+    def move_arm_tcp(self, target: PoseStamped, robot: Object, arm: Arms, tip_link: str = None) -> None:
+        """
+        Calls the ik solver to calculate the inverse kinematics of the arm and then sets the joint states accordingly.
 
+        :param target: Target pose to which the end-effector should move.
+        :param robot: Robot object representing the robot.
+        :param arm: Which arm to move
+        """
+        if tip_link is None:
+            tip_link = RobotDescription.current_robot_description.get_arm_chain(arm).get_tool_frame()
+
+        joints = RobotDescription.current_robot_description.get_arm_chain(arm).joints
+
+        inv = request_ik(target, robot, joints, tip_link)
+        _apply_ik(robot, inv)
 
 @with_plan
 @dataclass
@@ -146,5 +173,23 @@ class MoveTCPWaypointsMotion(BaseMotion):
     """
 
     def perform(self):
-        pm_manager = ProcessModuleManager().get_manager()
-        pm_manager.move_tcp_waypoints().execute(self)
+        waypoints = self.waypoints
+        robot = World.robot
+        arm = self.arm
+        for waypoint in waypoints:
+            self.move_arm_tcp(target=waypoint, robot=robot, arm=arm)
+    def move_arm_tcp(self, target: PoseStamped, robot: Object, arm: Arms, tip_link: str = None) -> None:
+        """
+        Calls the ik solver to calculate the inverse kinematics of the arm and then sets the joint states accordingly.
+
+        :param target: Target pose to which the end-effector should move.
+        :param robot: Robot object representing the robot.
+        :param arm: Which arm to move
+        """
+        if tip_link is None:
+            tip_link = RobotDescription.current_robot_description.get_arm_chain(arm).get_tool_frame()
+
+        joints = RobotDescription.current_robot_description.get_arm_chain(arm).joints
+
+        inv = request_ik(target, robot, joints, tip_link)
+        _apply_ik(robot, inv)

src/pycram/robot_plans/motions/gripper.py

@@ -2,6 +2,7 @@
 
 from .base import BaseMotion
 from ...datastructures.pose import PoseStamped
+from ...datastructures.world import World
 from ...plan import with_plan
 from ...process_module import ProcessModuleManager
 
@@ -24,8 +25,8 @@ class MoveMotion(BaseMotion):
     """
 
     def perform(self):
-        pm_manager = ProcessModuleManager().get_manager()
-        return pm_manager.navigate().execute(self)
+        robot = World.robot
+        robot.set_pose(self.target)
 
 
 @with_plan