From c23e5022a4ccbbc4523e2a8df39665cb907f5788 Mon Sep 17 00:00:00 2001 From: mohammadkhoshnazarr Date: Thu, 10 Oct 2024 15:41:45 +0200 Subject: [PATCH 01/49] [SaveWorldState] Corrected the save state functionality and added the ability for the used to specift the state id. --- src/pycram/datastructures/world.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index ed5978cd0..48ed13068 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1157,6 +1157,7 @@ def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_ :param state_id: The used specified unique id representing the state. :param use_same_id: If the same id should be used for the state. + :param state_id: The used specified unique id representing the state. :return: The unique id representing the state. """ pass From b712e8abd94f68b6e0689da27067f2aba7367eda Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 11 Oct 2024 12:13:22 +0200 Subject: [PATCH 02/49] [SaveState] Corrected arguments order in save simulator state. --- src/pycram/datastructures/world.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 48ed13068..ed5978cd0 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1157,7 +1157,6 @@ def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_ :param state_id: The used specified unique id representing the state. :param use_same_id: If the same id should be used for the state. - :param state_id: The used specified unique id representing the state. :return: The unique id representing the state. """ pass From 75723b3f76d19bbf56fce4739662d31ce1b55cea Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 15 Oct 2024 18:03:54 +0200 Subject: [PATCH 03/49] [MultiverseContact] (WIP) Adding support for multiple contact points. --- src/pycram/datastructures/dataclasses.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index e083f42bf..d7528de45 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -892,6 +892,25 @@ def intersected(self) -> bool: return self.distance >= 0 and self.body_name != "" +@dataclass +class MultiverseObjectContactData: + """ + A dataclass to store all the contact data returned from Multiverse for a single object. + """ + body_names: List[str] + data: MultiverseContactData + + +@dataclass +class MultiverseContactData: + """ + A dataclass to store the contact data returned from Multiverse between two objects. + """ + points: List[MultiverseContactPoint] + forces: Dict[str, List[float]] + torques: Dict[str, List[float]] + + @dataclass class MultiverseContactPoint: """ From 1cb23fb8f1bd47e38d46812eb9fe901ffac1b518 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 16 Oct 2024 18:48:17 +0200 Subject: [PATCH 04/49] [MultiverseContact] Multiple contact points with normals now working. --- src/pycram/datastructures/dataclasses.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index d7528de45..b32e713b6 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -898,17 +898,7 @@ class MultiverseObjectContactData: A dataclass to store all the contact data returned from Multiverse for a single object. """ body_names: List[str] - data: MultiverseContactData - - -@dataclass -class MultiverseContactData: - """ - A dataclass to store the contact data returned from Multiverse between two objects. - """ - points: List[MultiverseContactPoint] - forces: Dict[str, List[float]] - torques: Dict[str, List[float]] + data: List[MultiverseContactPoint] @dataclass From 8a67ac7b8e506c27474ec411e7ea1fda7809a310 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 17 Oct 2024 13:06:50 +0200 Subject: [PATCH 05/49] [euROBIN] Added ur5e description and a demo for using it in eurobin. --- .../demo_euROBIN_industrial_robotics.py | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index 67dbf38da..ffac48ce8 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -32,3 +32,39 @@ world.exit() +import logging +import os + +from rospkg import RosPack + +from pycram.datastructures.enums import ObjectType +from pycram.world_concepts.world_object import Object +from pycram.datastructures.pose import Pose +from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor +from pycram.ros_utils.joint_state_publisher import JointStatePublisher +from pycram.worlds.multiverse import Multiverse + +SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__)) +PYCRAM_DIR = os.path.join(SCRIPT_DIR, os.pardir, os.pardir) +RESOURCE_DIR = os.path.join(PYCRAM_DIR, "resources") + +SPAWNING_POSES = { + "robot": Pose([0, 0, 0], [0.0, 0.0, 0.0, 1.0]), # x,y,z,qx,qy,qz,qw + "cereal": Pose([0.5, 0.5, 2.0], [0.0, 0.0, 0.0, 1.0]) +} + + +if __name__ == '__main__': + root = logging.getLogger() + root.setLevel(logging.INFO) + + world = Multiverse(simulation_name="ur5e_with_task_board") + + # Load environment, robot and objects + rospack = RosPack() + + robot = Object("ur5e", ObjectType.ROBOT, "ur5e_with_gripper/urdf/ur5e.urdf") + jsp = JointStatePublisher() + # fts = ForceTorqueSensor("ee_fixed_joint") + + world.simulate(1) From e25d055115744137ea15ef5d41e8783c565e9495 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 18 Oct 2024 13:03:54 +0200 Subject: [PATCH 06/49] [euROBIN] new process_module for gripper, accept grippers as separate pycram objects, closing and opening gripper works. --- .../demo_euROBIN_industrial_robotics.py | 19 +++++++++++++++++-- src/pycram/robot_description.py | 10 +++++++++- .../ur5e_controlled_description.py | 2 +- 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index ffac48ce8..44272a030 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -37,12 +37,14 @@ from rospkg import RosPack -from pycram.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType, GripperState, Arms +from pycram.process_module import simulated_robot, real_robot from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor from pycram.ros_utils.joint_state_publisher import JointStatePublisher from pycram.worlds.multiverse import Multiverse +from pycram.designators.action_designator import SetGripperAction SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__)) PYCRAM_DIR = os.path.join(SCRIPT_DIR, os.pardir, os.pardir) @@ -54,6 +56,15 @@ } +def spawn_ur5e_with_gripper(): + robot = Object("ur5e", ObjectType.ROBOT, "universal_robot/ur5e/urdf/ur5e.urdf") + gripper = Object("gripper-2F-85", ObjectType.GRIPPER, "robotiq/gripper-2F-85/gripper-2F-85.urdf") + wrist_3_tf_frame = robot.get_link_tf_frame("wrist_3_link") + gripper.set_pose(Pose([0, 0.1, 0], [1.0, 0.0, 0.0, -1.0], frame=wrist_3_tf_frame)) + robot.attach(gripper, parent_link="wrist_3_link") + return robot, gripper + + if __name__ == '__main__': root = logging.getLogger() root.setLevel(logging.INFO) @@ -63,8 +74,12 @@ # Load environment, robot and objects rospack = RosPack() - robot = Object("ur5e", ObjectType.ROBOT, "ur5e_with_gripper/urdf/ur5e.urdf") + robot, gripper = spawn_ur5e_with_gripper() + jsp = JointStatePublisher() # fts = ForceTorqueSensor("ee_fixed_joint") + robot_arms = [chain.arm_type for chain in robot.robot_description.get_manipulator_chains()] + with real_robot: + SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform() world.simulate(1) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index e04f1a38d..654098883 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -119,10 +119,16 @@ class RobotDescription: Virtual mobile base joint names for mobile robots, these joints are not part of the URDF, however they are used to move the robot in the simulation (e.g. set_pose for the robot would actually move these joints) """ + gripper_name: Optional[str] = None + """ + Name of the gripper of the robot if it has one, this is used when the gripper is a different Object with its own + description file outside the robot description file. + """ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None, - ignore_joints: Optional[List[str]] = None): + ignore_joints: Optional[List[str]] = None, + gripper_name: Optional[str] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -135,6 +141,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, :param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots :param mjcf_path: Path to the MJCF file of the robot :param ignore_joints: List of joint names that are not used. + :param gripper_name: Name of the gripper of the robot if it has one and is a separate Object. """ self.name = name self.base_link = base_link @@ -152,6 +159,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, self.links: List[str] = [l.name for l in self.urdf_object.links] self.joints: List[str] = [j.name for j in self.urdf_object.joints] self.virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = virtual_mobile_base_joints + self.gripper_name = gripper_name @property def has_actuators(self): diff --git a/src/pycram/robot_descriptions/ur5e_controlled_description.py b/src/pycram/robot_descriptions/ur5e_controlled_description.py index 403c5669a..125ef1495 100644 --- a/src/pycram/robot_descriptions/ur5e_controlled_description.py +++ b/src/pycram/robot_descriptions/ur5e_controlled_description.py @@ -20,7 +20,7 @@ filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, ROBOT_NAME, resources_dir=multiverse_resources) mjcf_filename = get_robot_mjcf_path(robot_relative_dir, ROBOT_NAME, multiverse_resources=multiverse_resources) ur5_description = RobotDescription(ROBOT_NAME, "base_link", "", "", - filename, mjcf_path=mjcf_filename) + filename, mjcf_path=mjcf_filename, gripper_name=GRIPPER_NAME) ################################## Arm ################################## arm = KinematicChainDescription("manipulator", "base_link", "wrist_3_link", From 86b714c0f1346276fa24d03702758dbb45f3ff50 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 5 Nov 2024 17:39:53 +0100 Subject: [PATCH 07/49] [GiskardandRobokudo] Ignore virtual joints when setting joint goal in giskard. Updated robot_description usage from old version to new version in Giskard. Added options for disabling monitors and allowing gripper collision in achieve_cartesian_goal in Giskard and added these options to the configurations of the world. Added a globale variable that indicates if robokudo was found or not to avoid using real detection processs module if Robokudo was not found. --- src/pycram/external_interfaces/giskard.py | 1 + src/pycram/process_modules/pr2_process_modules.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index c594660d6..de6f704dd 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -22,6 +22,7 @@ try: from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry + from giskard_msgs.srv import UpdateWorld except ModuleNotFoundError as e: logwarn("Failed to import Giskard messages, the real robot will not be available") diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 9351b8890..f5df10351 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -58,7 +58,7 @@ def feedback_callback(msg): class Pr2MoveGripperReal(ProcessModule): """ - Opens or closes the gripper of the real robot, gripper uses an action server for this instead of giskard + Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard """ def _execute(self, designator: MoveGripperMotion): From 8de923dcfeff6c3336db8bd991c346157197dc30 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 7 Nov 2024 17:22:31 +0100 Subject: [PATCH 08/49] [MultiverseFallschoolDemo] (WIP) Need to figure out why giskard state is not synced correctly. Fixed Goal Validator. robot_state_updater.py also updates states of objects. no need for original pose anymore to move mobile robot in multiverse. --- demos/pycram_multiverse_demo/fallschool_demo.py | 8 +++++++- src/pycram/external_interfaces/ik.py | 2 +- src/pycram/process_modules/pr2_process_modules.py | 6 ++++++ 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index d60276976..e114043e7 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -13,6 +13,7 @@ from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ LookAtAction, DetectAction from pycram.designators.object_designator import BelieveObject +from pycram.failures import PerceptionObjectNotFound from pycram.process_module import simulated_robot, with_simulated_robot, real_robot from pycram.ros_utils.robot_state_updater import WorldStateUpdater from pycram.world_concepts.world_object import Object @@ -59,6 +60,8 @@ def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): fridge_base_pose.position.x += 0.16 fridge_base_pose.position.y += -0.1 milk.set_pose(fridge_base_pose, base=True) +print(milk.get_position_as_list()) +print(milk.get_orientation_as_list()) robot_desig = BelieveObject(names=[robot.name]) @@ -76,7 +79,10 @@ def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + try: + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + except PerceptionObjectNotFound: + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform() diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 5a89a679f..9510b2cb3 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -171,7 +171,7 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str :param gripper: Name of the tool frame which should grasp, this should be at the end of the given joint chain :return: A Pose at which the robt should stand as well as a dictionary of joint values """ - if "/giskard" not in rosnode.get_node_names(): + if "/giskard" not in rosnode.get_node_names() or True: return robot.pose, request_kdl_ik(target_pose, robot, joints, gripper) return request_giskard_ik(target_pose, robot, gripper) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index f5df10351..85cd00e0b 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -22,8 +22,14 @@ if Multiverse is not None: logwarn("Import for control_msgs for gripper in Multiverse failed") +try: + from ..worlds import Multiverse +except ImportError: + Multiverse = NoneType + try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 + from control_msgs.msg import GripperCommandGoal, GripperCommandAction except ImportError: logdebug("Pr2GripperCommandGoal not found") From 2adf2baae46c664578a3ee727bedae5fd1ef18c7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 8 Nov 2024 20:06:50 +0100 Subject: [PATCH 09/49] [MultiverseFallschoolDemo] (WIP) Need to fix multiverse contact, and make a feedback loop on gripper closing. --- .../behavior_tree_config.py | 310 ++++++++++++++++ demos/pycram_multiverse_demo/pr2.py | 301 ++++++++++++++++ demos/pycram_multiverse_demo/world_config.py | 333 ++++++++++++++++++ src/pycram/designators/action_designator.py | 10 +- src/pycram/external_interfaces/ik.py | 2 +- 5 files changed, 954 insertions(+), 2 deletions(-) create mode 100644 demos/pycram_multiverse_demo/behavior_tree_config.py create mode 100644 demos/pycram_multiverse_demo/pr2.py create mode 100644 demos/pycram_multiverse_demo/world_config.py diff --git a/demos/pycram_multiverse_demo/behavior_tree_config.py b/demos/pycram_multiverse_demo/behavior_tree_config.py new file mode 100644 index 000000000..952764df8 --- /dev/null +++ b/demos/pycram_multiverse_demo/behavior_tree_config.py @@ -0,0 +1,310 @@ +from abc import ABC, abstractmethod +from typing import Optional + +from giskardpy.exceptions import SetupException +from giskardpy.god_map import god_map +from giskardpy.model.ros_msg_visualization import VisualizationMode +from giskardpy.tree.behaviors.tf_publisher import TfPublishingModes +from giskardpy.tree.branches.giskard_bt import GiskardBT +from giskardpy.tree.control_modes import ControlModes +from giskardpy.utils.utils import is_running_in_pytest + + +class BehaviorTreeConfig(ABC): + + def __init__(self, mode: ControlModes, control_loop_max_hz: float = 50, simulation_max_hz: Optional[float] = None): + """ + + :param mode: Defines the default setup of the behavior tree. + :param control_loop_max_hz: if mode == ControlModes.standalone: limits the simulation speed + if mode == ControlModes.open_loop: limits the control loop of the base tracker + if mode == ControlModes.close_loop: limits the control loop + """ + self._control_mode = mode + self.control_loop_max_hz = control_loop_max_hz + self.simulation_max_hz = simulation_max_hz + + @abstractmethod + def setup(self): + """ + Implement this method to configure the behavior tree using it's self. methods. + """ + + @property + def tree(self) -> GiskardBT: + return god_map.tree + + def _create_behavior_tree(self): + god_map.tree = GiskardBT(control_mode=self._control_mode) + + def set_defaults(self): + pass + + def set_tree_tick_rate(self, rate: float = 0.05): + """ + How often the tree ticks per second. + :param rate: in /s + """ + self.tree_tick_rate = rate + + def add_visualization_marker_publisher(self, + mode: VisualizationMode, + add_to_sync: Optional[bool] = None, + add_to_control_loop: Optional[bool] = None): + """ + + :param add_to_sync: Markers are published while waiting for a goal. + :param add_to_control_loop: Markers are published during the closed loop control sequence, this is slow. + :param use_decomposed_meshes: True: publish decomposed meshes used for collision avoidance, these likely only + available on the machine where Giskard is running. + False: use meshes defined in urdf. + """ + if add_to_sync: + self.tree.wait_for_goal.publish_state.add_visualization_marker_behavior(mode) + if add_to_control_loop: + self.tree.control_loop_branch.publish_state.add_visualization_marker_behavior(mode) + + def add_qp_data_publisher(self, publish_lb: bool = False, publish_ub: bool = False, + publish_lbA: bool = False, publish_ubA: bool = False, + publish_bE: bool = False, publish_Ax: bool = False, + publish_Ex: bool = False, publish_xdot: bool = False, + publish_weights: bool = False, publish_g: bool = False, + publish_debug: bool = False, add_to_base: bool = False): + """ + QP data is streamed and can be visualized in e.g. plotjuggler. Useful for debugging. + """ + self.add_evaluate_debug_expressions() + if god_map.is_open_loop(): + self.tree.execute_traj.base_closed_loop.publish_state.add_qp_data_publisher( + publish_lb=publish_lb, + publish_ub=publish_ub, + publish_lbA=publish_lbA, + publish_ubA=publish_ubA, + publish_bE=publish_bE, + publish_Ax=publish_Ax, + publish_Ex=publish_Ex, + publish_xdot=publish_xdot, + publish_weights=publish_weights, + publish_g=publish_g, + publish_debug=publish_debug) + else: + self.tree.control_loop_branch.publish_state.add_qp_data_publisher( + publish_lb=publish_lb, + publish_ub=publish_ub, + publish_lbA=publish_lbA, + publish_ubA=publish_ubA, + publish_bE=publish_bE, + publish_Ax=publish_Ax, + publish_Ex=publish_Ex, + publish_xdot=publish_xdot, + publish_weights=publish_weights, + publish_g=publish_g, + publish_debug=publish_debug) + + def add_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False): + """ + Plots the generated trajectories. + :param normalize_position: Positions are centered around zero. + :param wait: True: Behavior tree waits for this plotter to finish. + False: Plot is generated in a separate thread to not slow down Giskard. + """ + self.tree.cleanup_control_loop.add_plot_trajectory(normalize_position, wait) + + def add_trajectory_visualizer(self): + self.tree.cleanup_control_loop.add_visualize_trajectory() + + def add_debug_trajectory_visualizer(self): + self.tree.cleanup_control_loop.add_debug_visualize_trajectory() + + def add_debug_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False): + """ + Plots debug expressions defined in goals. + """ + self.add_evaluate_debug_expressions() + self.tree.cleanup_control_loop.add_plot_debug_trajectory(normalize_position=normalize_position, + wait=wait) + + def add_gantt_chart_plotter(self): + self.add_evaluate_debug_expressions() + self.tree.cleanup_control_loop.add_plot_gantt_chart() + + def add_goal_graph_plotter(self): + self.add_evaluate_debug_expressions() + self.tree.prepare_control_loop.add_plot_goal_graph() + + def add_debug_marker_publisher(self): + """ + Publishes debug expressions defined in goals. + """ + self.add_evaluate_debug_expressions() + self.tree.control_loop_branch.publish_state.add_debug_marker_publisher() + + def add_tf_publisher(self, include_prefix: bool = True, tf_topic: str = 'tf', + mode: TfPublishingModes = TfPublishingModes.attached_and_world_objects): + """ + Publishes tf for Giskard's internal state. + """ + self.tree.wait_for_goal.publish_state.add_tf_publisher(include_prefix=include_prefix, + tf_topic=tf_topic, + mode=mode) + if god_map.is_standalone(): + self.tree.control_loop_branch.publish_state.add_tf_publisher(include_prefix=include_prefix, + tf_topic=tf_topic, + mode=mode) + + def add_evaluate_debug_expressions(self): + self.tree.prepare_control_loop.add_compile_debug_expressions() + if god_map.is_closed_loop(): + self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=False) + else: + self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=True) + if god_map.is_open_loop(): + god_map.tree.execute_traj.prepare_base_control.add_compile_debug_expressions() + god_map.tree.execute_traj.base_closed_loop.add_evaluate_debug_expressions(log_traj=False) + + def add_js_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False): + """ + Publishes joint states for Giskard's internal state. + """ + god_map.tree.control_loop_branch.publish_state.add_joint_state_publisher(include_prefix=include_prefix, + topic_name=topic_name, + only_prismatic_and_revolute=True) + god_map.tree.wait_for_goal.publish_state.add_joint_state_publisher(include_prefix=include_prefix, + topic_name=topic_name, + only_prismatic_and_revolute=True) + + def add_free_variable_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False): + """ + Publishes joint states for Giskard's internal state. + """ + god_map.tree.control_loop_branch.publish_state.add_joint_state_publisher(include_prefix=include_prefix, + topic_name=topic_name, + only_prismatic_and_revolute=False) + god_map.tree.wait_for_goal.publish_state.add_joint_state_publisher(include_prefix=include_prefix, + topic_name=topic_name, + only_prismatic_and_revolute=False) + + +class StandAloneBTConfig(BehaviorTreeConfig): + def __init__(self, + debug_mode: bool = False, + publish_js: bool = False, + visualization_mode: VisualizationMode = VisualizationMode.VisualsFrameLocked, + publish_free_variables: bool = False, + publish_tf: bool = True, + include_prefix: bool = False, + simulation_max_hz: Optional[float] = None): + """ + The default behavior tree for Giskard in standalone mode. Make sure to set up the robot interface accordingly. + :param debug_mode: enable various debugging tools. + :param publish_js: publish current world state. + :param publish_tf: publish all link poses in tf. + :param simulation_max_hz: if not None, will limit the frequency of the simulation. + :param include_prefix: whether to include the robot name prefix when publishing joint states or tf + """ + self.include_prefix = include_prefix + self.visualization_mode = visualization_mode + if is_running_in_pytest(): + if god_map.is_in_github_workflow(): + publish_js = False + publish_tf = False + debug_mode = False + simulation_max_hz = None + super().__init__(ControlModes.standalone, simulation_max_hz=simulation_max_hz) + self.debug_mode = debug_mode + self.publish_js = publish_js + self.publish_free_variables = publish_free_variables + self.publish_tf = publish_tf + if publish_js and publish_free_variables: + raise SetupException('publish_js and publish_free_variables cannot be True at the same time.') + + def setup(self): + self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=True, + mode=self.visualization_mode) + if self.publish_tf: + self.add_tf_publisher(include_prefix=self.include_prefix, mode=TfPublishingModes.all) + self.add_gantt_chart_plotter() + self.add_goal_graph_plotter() + if self.debug_mode: + self.add_trajectory_plotter(wait=True) + # self.add_trajectory_visualizer() + # self.add_debug_trajectory_visualizer() + self.add_debug_trajectory_plotter(wait=True) + self.add_debug_marker_publisher() + # self.add_debug_marker_publisher() + if self.publish_js: + self.add_js_publisher(include_prefix=self.include_prefix) + if self.publish_free_variables: + self.add_free_variable_publisher() + + +class OpenLoopBTConfig(BehaviorTreeConfig): + def __init__(self, + debug_mode: bool = False, + control_loop_max_hz: float = 50, + visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed, + simulation_max_hz: Optional[float] = None): + """ + The default behavior tree for Giskard in open-loop mode. It will first plan the trajectory in simulation mode + and then publish it to connected joint trajectory followers. The base trajectory is tracked with a closed-loop + controller. + :param debug_mode: enable various debugging tools. + :param control_loop_max_hz: if not None, limits the frequency of the base trajectory controller. + """ + super().__init__(ControlModes.open_loop, control_loop_max_hz=control_loop_max_hz, + simulation_max_hz=simulation_max_hz) + if god_map.is_in_github_workflow(): + debug_mode = False + self.debug_mode = debug_mode + self.visualization_mode = visualization_mode + + def setup(self): + self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=True, + mode=self.visualization_mode) + self.add_gantt_chart_plotter() + self.add_goal_graph_plotter() + if self.debug_mode or True: + self.add_trajectory_plotter(wait=True) + self.add_debug_trajectory_plotter(wait=True) + self.add_debug_marker_publisher() + self.add_qp_data_publisher( + publish_debug=True, + publish_xdot=True, + # publish_lbA=True, + # publish_ubA=True + ) + + +class ClosedLoopBTConfig(BehaviorTreeConfig): + def __init__(self, debug_mode: bool = False, + control_loop_max_hz: float = 50, + visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed, + simulation_max_hz: Optional[float] = None): + """ + The default configuration for Giskard in closed loop mode. Make use to set up the robot interface accordingly. + :param debug_mode: If True, will publish debug data on topics. This will significantly slow down the control loop. + :param control_loop_max_hz: Limits the control loop frequency. If None, it will go as fast as possible. + """ + super().__init__(ControlModes.close_loop, control_loop_max_hz=control_loop_max_hz, + simulation_max_hz=simulation_max_hz) + if god_map.is_in_github_workflow(): + debug_mode = False + self.debug_mode = debug_mode + self.visualization_mode = visualization_mode + + def setup(self): + self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=False, + mode=self.visualization_mode) + # self.add_qp_data_publisher(publish_xdot=True, publish_lb=True, publish_ub=True) + self.add_gantt_chart_plotter() + self.add_goal_graph_plotter() + if self.debug_mode: + self.add_trajectory_plotter(wait=True) + self.add_debug_trajectory_plotter(wait=True) + self.add_debug_marker_publisher() + # self.add_qp_data_publisher( + # publish_debug=True, + # publish_xdot=True, + # # publish_lbA=True, + # # publish_ubA=True + # ) diff --git a/demos/pycram_multiverse_demo/pr2.py b/demos/pycram_multiverse_demo/pr2.py new file mode 100644 index 000000000..ae4cc1872 --- /dev/null +++ b/demos/pycram_multiverse_demo/pr2.py @@ -0,0 +1,301 @@ +from giskardpy.configs.giskard import CollisionAvoidanceConfig, RobotInterfaceConfig +from giskardpy.configs.world_config import WorldWithOmniDriveRobot +from giskardpy.data_types import Derivatives + + +class WorldWithPR2Config(WorldWithOmniDriveRobot): + def __init__(self, map_name: str = 'map', localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', drive_joint_name: str = 'brumbrum'): + super().__init__(map_name, localization_joint_name, odom_link_name, drive_joint_name) + + def setup(self): + super().setup() + self.set_joint_limits(limit_map={Derivatives.velocity: 2, + Derivatives.jerk: 60}, + joint_name='head_pan_joint') + self.set_joint_limits(limit_map={Derivatives.velocity: 4, + Derivatives.jerk: 120}, + joint_name='head_tilt_joint') + + + +class PR2StandaloneInterface(RobotInterfaceConfig): + drive_joint_name: str + + def __init__(self, drive_joint_name: str): + self.drive_joint_name = drive_joint_name + + def setup(self): + self.register_controlled_joints([ + 'torso_lift_joint', + 'head_pan_joint', + 'head_tilt_joint', + 'r_shoulder_pan_joint', + 'r_shoulder_lift_joint', + 'r_upper_arm_roll_joint', + 'r_forearm_roll_joint', + 'r_elbow_flex_joint', + 'r_wrist_flex_joint', + 'r_wrist_roll_joint', + 'l_shoulder_pan_joint', + 'l_shoulder_lift_joint', + 'l_upper_arm_roll_joint', + 'l_forearm_roll_joint', + 'l_elbow_flex_joint', + 'l_wrist_flex_joint', + 'l_wrist_roll_joint', + self.drive_joint_name, + ]) + + +class PR2JointTrajServerMujocoInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('/joint_states') + self.sync_odometry_topic('/pr2/base_footprint', self.drive_joint_name) + self.add_follow_joint_trajectory_server( + namespace='/pr2/whole_body_controller') + self.add_follow_joint_trajectory_server( + namespace='/pr2/l_gripper_l_finger_controller') + self.add_follow_joint_trajectory_server( + namespace='/pr2/r_gripper_l_finger_controller') + self.add_base_cmd_velocity(cmd_vel_topic='/pr2/cmd_vel', + track_only_velocity=True, + joint_name=self.drive_joint_name) + +class PR2JointTrajServerMultiverseInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'pr2', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + # self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + # tf_parent_frame=self.map_name, + # tf_child_frame=self.odom_link_name) + # self. + self.sync_joint_state_topic('/real/pr2/joint_states') + self.sync_odometry_topic('/odom', self.drive_joint_name) + self.add_follow_joint_trajectory_server( + namespace='/real/pr2/head_controller') + self.add_follow_joint_trajectory_server( + namespace='/real/pr2/torso_controller') + self.add_follow_joint_trajectory_server( + namespace='/real/pr2/left_arm_controller') + self.add_follow_joint_trajectory_server( + namespace='/real/pr2/right_arm_controller') + self.add_base_cmd_velocity(cmd_vel_topic='/cmd_vel', + track_only_velocity=True, + joint_name=self.drive_joint_name) + +class PR2VelocityMujocoInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('pr2/joint_states') + self.sync_odometry_topic('/pr2/base_footprint', self.drive_joint_name) + self.add_joint_velocity_controller(namespaces=[ + 'pr2/torso_lift_velocity_controller', + 'pr2/r_upper_arm_roll_velocity_controller', + 'pr2/r_shoulder_pan_velocity_controller', + 'pr2/r_shoulder_lift_velocity_controller', + 'pr2/r_forearm_roll_velocity_controller', + 'pr2/r_elbow_flex_velocity_controller', + 'pr2/r_wrist_flex_velocity_controller', + 'pr2/r_wrist_roll_velocity_controller', + 'pr2/l_upper_arm_roll_velocity_controller', + 'pr2/l_shoulder_pan_velocity_controller', + 'pr2/l_shoulder_lift_velocity_controller', + 'pr2/l_forearm_roll_velocity_controller', + 'pr2/l_elbow_flex_velocity_controller', + 'pr2/l_wrist_flex_velocity_controller', + 'pr2/l_wrist_roll_velocity_controller', + 'pr2/head_pan_velocity_controller', + 'pr2/head_tilt_velocity_controller', + ]) + + self.add_base_cmd_velocity(cmd_vel_topic='/pr2/cmd_vel', + joint_name=self.drive_joint_name) + + +class PR2VelocityIAIInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('/joint_states') + self.sync_odometry_topic('/robot_pose_ekf/odom_combined', self.drive_joint_name) + self.add_joint_velocity_group_controller(namespace='l_arm_joint_group_velocity_controller') + self.add_joint_velocity_group_controller(namespace='r_arm_joint_group_velocity_controller') + self.add_joint_position_controller(namespaces=[ + 'head_pan_position_controller', + 'head_tilt_position_controller', + ]) + + self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', + joint_name=self.drive_joint_name) + + +class PR2CollisionAvoidance(CollisionAvoidanceConfig): + def __init__(self, drive_joint_name: str = 'brumbrum'): + super().__init__() + self.drive_joint_name = drive_joint_name + + def setup(self): + self.load_self_collision_matrix('package://giskardpy/self_collision_matrices/iai/pr2.srdf') + self.set_default_external_collision_avoidance(soft_threshold=0.1, + hard_threshold=0.0) + for joint_name in ['r_wrist_roll_joint', 'l_wrist_roll_joint']: + self.overwrite_external_collision_avoidance(joint_name, + number_of_repeller=4, + soft_threshold=0.05, + hard_threshold=0.0, + max_velocity=0.2) + for joint_name in ['r_wrist_flex_joint', 'l_wrist_flex_joint']: + self.overwrite_external_collision_avoidance(joint_name, + number_of_repeller=2, + soft_threshold=0.05, + hard_threshold=0.0, + max_velocity=0.2) + for joint_name in ['r_elbow_flex_joint', 'l_elbow_flex_joint']: + self.overwrite_external_collision_avoidance(joint_name, + soft_threshold=0.05, + hard_threshold=0.0) + for joint_name in ['r_forearm_roll_joint', 'l_forearm_roll_joint']: + self.overwrite_external_collision_avoidance(joint_name, + soft_threshold=0.025, + hard_threshold=0.0) + self.fix_joints_for_collision_avoidance([ + 'r_gripper_l_finger_joint', + 'l_gripper_l_finger_joint' + ]) + self.overwrite_external_collision_avoidance(self.drive_joint_name, + number_of_repeller=2, + soft_threshold=0.2, + hard_threshold=0.1) + + +class PR2JointTrajServerIAIInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('/joint_states') + self.sync_odometry_topic('/robot_pose_ekf/odom_combined', self.drive_joint_name) + fill_velocity_values = False + self.add_follow_joint_trajectory_server(namespace='/l_arm_controller', + fill_velocity_values=fill_velocity_values) + self.add_follow_joint_trajectory_server(namespace='/r_arm_controller', + fill_velocity_values=fill_velocity_values) + self.add_follow_joint_trajectory_server(namespace='/torso_controller', + fill_velocity_values=fill_velocity_values) + self.add_follow_joint_trajectory_server(namespace='/head_traj_controller', + fill_velocity_values=fill_velocity_values) + self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', + track_only_velocity=True, + joint_name=self.drive_joint_name) + + +class PR2JointTrajServerUnrealInterface(RobotInterfaceConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom_combined', + drive_joint_name: str = 'brumbrum'): + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, + tf_parent_frame=self.map_name, + tf_child_frame=self.odom_link_name) + self.sync_joint_state_topic('/joint_states') + self.sync_odometry_topic('/base_odometry/odom', self.drive_joint_name) + fill_velocity_values = False + self.add_follow_joint_trajectory_server(namespace='/whole_body_controller', + fill_velocity_values=fill_velocity_values) + self.add_follow_joint_trajectory_server(namespace='/head_traj_controller', + fill_velocity_values=fill_velocity_values) + self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', + track_only_velocity=True, + joint_name=self.drive_joint_name) diff --git a/demos/pycram_multiverse_demo/world_config.py b/demos/pycram_multiverse_demo/world_config.py new file mode 100644 index 000000000..6a177ac5a --- /dev/null +++ b/demos/pycram_multiverse_demo/world_config.py @@ -0,0 +1,333 @@ +from __future__ import annotations + +import abc +from abc import ABC +from typing import Dict, Optional, Union + +import numpy as np +import rospy +from numpy.typing import NDArray +from std_msgs.msg import ColorRGBA + +from giskardpy.god_map import god_map +from giskardpy.model.joints import FixedJoint, OmniDrive, DiffDrive, Joint6DOF, OneDofJoint +from giskardpy.model.links import Link +from giskardpy.model.utils import robot_name_from_urdf_string +from giskardpy.model.world import WorldTree +from giskardpy.data_types import my_string, PrefixName, Derivatives, derivative_map + + +class WorldConfig(ABC): + + def __init__(self): + god_map.world = WorldTree() + self.set_default_weights() + + @property + def world(self) -> WorldTree: + return god_map.world + + def set_defaults(self): + pass + + @abc.abstractmethod + def setup(self): + """ + Implement this method to configure the initial world using it's self. methods. + """ + + @property + def robot_group_name(self) -> str: + return god_map.world.robot_name + + def set_default_weights(self, + velocity_weight: float = 0.01, + acceleration_weight: float = 0, + jerk_weight: float = 0.01): + """ + The default values are set automatically, even if this function is not called. + A typical goal has a weight of 1, so the values in here should be sufficiently below that. + """ + god_map.world.update_default_weights({Derivatives.velocity: velocity_weight, + Derivatives.acceleration: acceleration_weight, + Derivatives.jerk: jerk_weight}) + + def set_weight(self, weight_map: derivative_map, joint_name: str, group_name: Optional[str] = None): + """ + Set weights for joints that are used by the qp controller. Don't change this unless you know what you are doing. + """ + joint_name = god_map.world.search_for_joint_name(joint_name, group_name) + joint = god_map.world.joints[joint_name] + if not isinstance(joint, OneDofJoint): + raise ValueError(f'Can\'t change weight because {joint_name} is not of type {str(OneDofJoint)}.') + free_variable = god_map.world.free_variables[joint.free_variable.name] + for derivative, weight in weight_map.items(): + free_variable.quadratic_weights[derivative] = weight + + def get_root_link_of_group(self, group_name: str) -> PrefixName: + return god_map.world.groups[group_name].root_link_name + + def set_joint_limits(self, limit_map: derivative_map, joint_name: my_string, group_name: Optional[str] = None): + """ + Set the joint limits for individual joints + :param limit_map: maps Derivatives to values, e.g. {Derivatives.velocity: 1, + Derivatives.acceleration: np.inf, + Derivatives.jerk: 30} + """ + joint_name = god_map.world.search_for_joint_name(joint_name, group_name) + joint = god_map.world.joints[joint_name] + if not isinstance(joint, OneDofJoint): + raise ValueError(f'Can\'t change limits because {joint_name} is not of type {str(OneDofJoint)}.') + free_variable = god_map.world.free_variables[joint.free_variable.name] + for derivative, limit in limit_map.items(): + free_variable.set_lower_limit(derivative, -limit) + free_variable.set_upper_limit(derivative, limit) + + def set_default_color(self, r: float, g: float, b: float, a: float): + """ + :param r: 0-1 + :param g: 0-1 + :param b: 0-1 + :param a: 0-1 + """ + god_map.world.default_link_color = ColorRGBA(r, g, b, a) + + def set_default_limits(self, new_limits: derivative_map): + """ + The default values will be set automatically, even if this function is not called. + :param new_limits: e.g. {Derivatives.velocity: 1, + Derivatives.acceleration: np.inf, + Derivatives.jerk: 30} + """ + god_map.world.update_default_limits(new_limits) + + def add_robot_urdf(self, + urdf: str, + group_name: str) -> str: + """ + Add a robot urdf to the world. + :param urdf: robot urdf as string, not the path + :param group_name: + """ + if group_name is None: + group_name = robot_name_from_urdf_string(urdf) + god_map.world.add_urdf(urdf=urdf, group_name=group_name, actuated=True) + return group_name + + def add_robot_from_parameter_server(self, + parameter_name: str = 'robot_description', + group_name: Optional[str] = None) -> str: + """ + Add a robot urdf from parameter server to Giskard. + :param parameter_name: + :param group_name: How to call the robot. If nothing is specified it will get the name it has in the urdf + """ + urdf = rospy.get_param(parameter_name) + return self.add_robot_urdf(urdf=urdf, group_name=group_name) + + def add_fixed_joint(self, parent_link: my_string, child_link: my_string, + homogenous_transform: Optional[NDArray] = None): + """ + Add a fixed joint to Giskard's world. Can be used to e.g. connect a non-mobile robot to the world frame. + :param parent_link: + :param child_link: + :param homogenous_transform: a 4x4 transformation matrix. + """ + if homogenous_transform is None: + homogenous_transform = np.eye(4) + parent_link = god_map.world.search_for_link_name(parent_link) + + child_link = PrefixName.from_string(child_link, set_none_if_no_slash=True) + joint_name = PrefixName(f'{parent_link}_{child_link}_fixed_joint', None) + joint = FixedJoint(name=joint_name, parent_link_name=parent_link, child_link_name=child_link, + parent_T_child=homogenous_transform) + god_map.world._add_joint(joint) + + def add_diff_drive_joint(self, + name: str, + parent_link_name: my_string, + child_link_name: my_string, + robot_group_name: Optional[str] = None, + translation_limits: Optional[derivative_map] = None, + rotation_limits: Optional[derivative_map] = None): + """ + Same as add_omni_drive_joint, but for a differential drive. + """ + joint_name = PrefixName(name, robot_group_name) + parent_link_name = PrefixName.from_string(parent_link_name, set_none_if_no_slash=True) + child_link_name = PrefixName.from_string(child_link_name, set_none_if_no_slash=True) + brumbrum_joint = DiffDrive(parent_link_name=parent_link_name, + child_link_name=child_link_name, + name=joint_name, + translation_limits=translation_limits, + rotation_limits=rotation_limits) + god_map.world._add_joint(brumbrum_joint) + god_map.world.deregister_group(robot_group_name) + god_map.world.register_group(robot_group_name, root_link_name=parent_link_name, actuated=True) + + def add_6dof_joint(self, parent_link: my_string, child_link: my_string, joint_name: my_string): + """ + Add a 6dof joint to Giskard's world. Generally used if you want Giskard to keep track of a tf transform, + e.g. for localization. + :param parent_link: + :param child_link: + """ + parent_link = god_map.world.search_for_link_name(parent_link) + child_link = PrefixName.from_string(child_link, set_none_if_no_slash=True) + joint_name = PrefixName.from_string(joint_name, set_none_if_no_slash=True) + joint = Joint6DOF(name=joint_name, parent_link_name=parent_link, child_link_name=child_link) + god_map.world._add_joint(joint) + + def add_empty_link(self, link_name: my_string): + """ + If you need a virtual link during your world building. + """ + link = Link(link_name) + god_map.world._add_link(link) + + def add_omni_drive_joint(self, + name: str, + parent_link_name: Union[str, PrefixName], + child_link_name: Union[str, PrefixName], + robot_group_name: Optional[str] = None, + translation_limits: Optional[derivative_map] = None, + rotation_limits: Optional[derivative_map] = None, + x_name: Optional[PrefixName] = None, + y_name: Optional[PrefixName] = None, + yaw_vel_name: Optional[PrefixName] = None): + """ + Use this to connect a robot urdf of a mobile robot to the world if it has an omni-directional drive. + :param parent_link_name: + :param child_link_name: + :param robot_group_name: set if there are multiple robots + :param name: Name of the new link. Has to be unique and may be required in other functions. + :param translation_limits: in m/s**3 + :param rotation_limits: in rad/s**3 + """ + joint_name = PrefixName(name, robot_group_name) + parent_link_name = PrefixName.from_string(parent_link_name, set_none_if_no_slash=True) + child_link_name = PrefixName.from_string(child_link_name, set_none_if_no_slash=True) + brumbrum_joint = OmniDrive(parent_link_name=parent_link_name, + child_link_name=child_link_name, + name=joint_name, + translation_limits=translation_limits, + rotation_limits=rotation_limits, + x_name=x_name, + y_name=y_name, + yaw_name=yaw_vel_name) + god_map.world._add_joint(brumbrum_joint) + god_map.world.deregister_group(robot_group_name) + god_map.world.register_group(robot_group_name, root_link_name=parent_link_name, actuated=True) + + +class EmptyWorld(WorldConfig): + def __init__(self): + super().__init__() + + def setup(self): + self._default_limits = { + Derivatives.velocity: 1, + } + self.set_default_weights(velocity_weight=1, acceleration_weight=1, jerk_weight=1) + self.set_default_limits(self._default_limits) + self.add_empty_link('map') + + +class WorldWithFixedRobot(WorldConfig): + def __init__(self, joint_limits: Dict[Derivatives, float] = None): + super().__init__() + self._joint_limits = joint_limits + + def setup(self): + self.set_default_limits(self._joint_limits) + self.add_robot_from_parameter_server() + + +class WorldWithOmniDriveRobot(WorldConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom', + drive_joint_name: str = 'brumbrum'): + super().__init__() + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.set_default_limits({Derivatives.velocity: 1, + Derivatives.acceleration: np.inf, + Derivatives.jerk: 30}) + self.add_empty_link(self.map_name) + self.add_empty_link(self.odom_link_name) + self.add_fixed_joint(parent_link=self.map_name, child_link=self.odom_link_name) + # self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, + # joint_name=self.localization_joint_name) + self.add_robot_from_parameter_server() + root_link_name = self.get_root_link_of_group(self.robot_group_name) + self.add_omni_drive_joint(name=self.drive_joint_name, + parent_link_name=self.odom_link_name, + child_link_name=root_link_name, + translation_limits={ + Derivatives.velocity: 0.2, + Derivatives.acceleration: 1, + Derivatives.jerk: 5, + }, + rotation_limits={ + Derivatives.velocity: 0.2, + Derivatives.acceleration: 1, + Derivatives.jerk: 5 + }, + robot_group_name=self.robot_group_name) + self.set_joint_limits(limit_map={Derivatives.velocity: 3, + Derivatives.jerk: 60}, + joint_name='head_pan_joint') + + +class WorldWithDiffDriveRobot(WorldConfig): + map_name: str + localization_joint_name: str + odom_link_name: str + drive_joint_name: str + + def __init__(self, + map_name: str = 'map', + localization_joint_name: str = 'localization', + odom_link_name: str = 'odom', + drive_joint_name: str = 'brumbrum'): + super().__init__() + self.map_name = map_name + self.localization_joint_name = localization_joint_name + self.odom_link_name = odom_link_name + self.drive_joint_name = drive_joint_name + + def setup(self): + self.set_default_limits({Derivatives.velocity: 1, + Derivatives.acceleration: np.inf, + Derivatives.jerk: 30}) + self.add_empty_link(self.map_name) + self.add_empty_link(self.odom_link_name) + self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, + joint_name=self.localization_joint_name) + self.add_robot_from_parameter_server() + root_link_name = self.get_root_link_of_group(self.robot_group_name) + self.add_diff_drive_joint(name=self.drive_joint_name, + parent_link_name=self.odom_link_name, + child_link_name=root_link_name, + translation_limits={ + Derivatives.velocity: 0.2, + Derivatives.acceleration: 1, + Derivatives.jerk: 5, + }, + rotation_limits={ + Derivatives.velocity: 0.2, + Derivatives.acceleration: 1, + Derivatives.jerk: 5 + }, + robot_group_name=self.robot_group_name) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 1ca3464b1..e8701f9d0 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -24,6 +24,7 @@ from ..failures import ObjectUnfetchable, ReachabilityFailure, NavigationGoalNotReachedError, PerceptionObjectNotFound, \ ObjectNotGraspedError from ..robot_description import RobotDescription +from ..ros.ros_tools import sleep from ..tasktree import with_tree from ..world_reasoning import contact @@ -279,6 +280,7 @@ def plan(self) -> None: robot = World.robot # Retrieve object and robot from designators object = self.object_designator.world_object + # Get grasp orientation and target pose grasp = RobotDescription.current_robot_description.grasps[self.grasp] # oTm = Object Pose in Frame map @@ -446,7 +448,13 @@ def plan(self) -> None: for pose in pickup_loc: if self.arm in pose.reachable_arms: pickup_pose = pose - break + NavigateActionPerformable(pickup_pose.pose).perform() + robot = robot_desig.resolve().world_object + if robot.pose.almost_equal(pickup_pose.pose, 0.1, 3): + break + else: + pickup_pose = None + continue if not pickup_pose: raise ObjectUnfetchable( f"Found no pose for the robot to grasp the object: {self.object_designator} with arm: {self.arm}") diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 9510b2cb3..5a89a679f 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -171,7 +171,7 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str :param gripper: Name of the tool frame which should grasp, this should be at the end of the given joint chain :return: A Pose at which the robt should stand as well as a dictionary of joint values """ - if "/giskard" not in rosnode.get_node_names() or True: + if "/giskard" not in rosnode.get_node_names(): return robot.pose, request_kdl_ik(target_pose, robot, joints, gripper) return request_giskard_ik(target_pose, robot, gripper) From 217e174d4e27cb5b85a16a7ed875d137111b7115 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Nov 2024 13:03:26 +0100 Subject: [PATCH 10/49] [NavigateAction] Add an option that asks if the joint states should be kept while navigating. --- .../behavior_tree_config.py | 310 ---------------- .../pycram_multiverse_demo/fallschool_demo.py | 4 + demos/pycram_multiverse_demo/pr2.py | 301 ---------------- demos/pycram_multiverse_demo/world_config.py | 333 ------------------ 4 files changed, 4 insertions(+), 944 deletions(-) delete mode 100644 demos/pycram_multiverse_demo/behavior_tree_config.py delete mode 100644 demos/pycram_multiverse_demo/pr2.py delete mode 100644 demos/pycram_multiverse_demo/world_config.py diff --git a/demos/pycram_multiverse_demo/behavior_tree_config.py b/demos/pycram_multiverse_demo/behavior_tree_config.py deleted file mode 100644 index 952764df8..000000000 --- a/demos/pycram_multiverse_demo/behavior_tree_config.py +++ /dev/null @@ -1,310 +0,0 @@ -from abc import ABC, abstractmethod -from typing import Optional - -from giskardpy.exceptions import SetupException -from giskardpy.god_map import god_map -from giskardpy.model.ros_msg_visualization import VisualizationMode -from giskardpy.tree.behaviors.tf_publisher import TfPublishingModes -from giskardpy.tree.branches.giskard_bt import GiskardBT -from giskardpy.tree.control_modes import ControlModes -from giskardpy.utils.utils import is_running_in_pytest - - -class BehaviorTreeConfig(ABC): - - def __init__(self, mode: ControlModes, control_loop_max_hz: float = 50, simulation_max_hz: Optional[float] = None): - """ - - :param mode: Defines the default setup of the behavior tree. - :param control_loop_max_hz: if mode == ControlModes.standalone: limits the simulation speed - if mode == ControlModes.open_loop: limits the control loop of the base tracker - if mode == ControlModes.close_loop: limits the control loop - """ - self._control_mode = mode - self.control_loop_max_hz = control_loop_max_hz - self.simulation_max_hz = simulation_max_hz - - @abstractmethod - def setup(self): - """ - Implement this method to configure the behavior tree using it's self. methods. - """ - - @property - def tree(self) -> GiskardBT: - return god_map.tree - - def _create_behavior_tree(self): - god_map.tree = GiskardBT(control_mode=self._control_mode) - - def set_defaults(self): - pass - - def set_tree_tick_rate(self, rate: float = 0.05): - """ - How often the tree ticks per second. - :param rate: in /s - """ - self.tree_tick_rate = rate - - def add_visualization_marker_publisher(self, - mode: VisualizationMode, - add_to_sync: Optional[bool] = None, - add_to_control_loop: Optional[bool] = None): - """ - - :param add_to_sync: Markers are published while waiting for a goal. - :param add_to_control_loop: Markers are published during the closed loop control sequence, this is slow. - :param use_decomposed_meshes: True: publish decomposed meshes used for collision avoidance, these likely only - available on the machine where Giskard is running. - False: use meshes defined in urdf. - """ - if add_to_sync: - self.tree.wait_for_goal.publish_state.add_visualization_marker_behavior(mode) - if add_to_control_loop: - self.tree.control_loop_branch.publish_state.add_visualization_marker_behavior(mode) - - def add_qp_data_publisher(self, publish_lb: bool = False, publish_ub: bool = False, - publish_lbA: bool = False, publish_ubA: bool = False, - publish_bE: bool = False, publish_Ax: bool = False, - publish_Ex: bool = False, publish_xdot: bool = False, - publish_weights: bool = False, publish_g: bool = False, - publish_debug: bool = False, add_to_base: bool = False): - """ - QP data is streamed and can be visualized in e.g. plotjuggler. Useful for debugging. - """ - self.add_evaluate_debug_expressions() - if god_map.is_open_loop(): - self.tree.execute_traj.base_closed_loop.publish_state.add_qp_data_publisher( - publish_lb=publish_lb, - publish_ub=publish_ub, - publish_lbA=publish_lbA, - publish_ubA=publish_ubA, - publish_bE=publish_bE, - publish_Ax=publish_Ax, - publish_Ex=publish_Ex, - publish_xdot=publish_xdot, - publish_weights=publish_weights, - publish_g=publish_g, - publish_debug=publish_debug) - else: - self.tree.control_loop_branch.publish_state.add_qp_data_publisher( - publish_lb=publish_lb, - publish_ub=publish_ub, - publish_lbA=publish_lbA, - publish_ubA=publish_ubA, - publish_bE=publish_bE, - publish_Ax=publish_Ax, - publish_Ex=publish_Ex, - publish_xdot=publish_xdot, - publish_weights=publish_weights, - publish_g=publish_g, - publish_debug=publish_debug) - - def add_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False): - """ - Plots the generated trajectories. - :param normalize_position: Positions are centered around zero. - :param wait: True: Behavior tree waits for this plotter to finish. - False: Plot is generated in a separate thread to not slow down Giskard. - """ - self.tree.cleanup_control_loop.add_plot_trajectory(normalize_position, wait) - - def add_trajectory_visualizer(self): - self.tree.cleanup_control_loop.add_visualize_trajectory() - - def add_debug_trajectory_visualizer(self): - self.tree.cleanup_control_loop.add_debug_visualize_trajectory() - - def add_debug_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False): - """ - Plots debug expressions defined in goals. - """ - self.add_evaluate_debug_expressions() - self.tree.cleanup_control_loop.add_plot_debug_trajectory(normalize_position=normalize_position, - wait=wait) - - def add_gantt_chart_plotter(self): - self.add_evaluate_debug_expressions() - self.tree.cleanup_control_loop.add_plot_gantt_chart() - - def add_goal_graph_plotter(self): - self.add_evaluate_debug_expressions() - self.tree.prepare_control_loop.add_plot_goal_graph() - - def add_debug_marker_publisher(self): - """ - Publishes debug expressions defined in goals. - """ - self.add_evaluate_debug_expressions() - self.tree.control_loop_branch.publish_state.add_debug_marker_publisher() - - def add_tf_publisher(self, include_prefix: bool = True, tf_topic: str = 'tf', - mode: TfPublishingModes = TfPublishingModes.attached_and_world_objects): - """ - Publishes tf for Giskard's internal state. - """ - self.tree.wait_for_goal.publish_state.add_tf_publisher(include_prefix=include_prefix, - tf_topic=tf_topic, - mode=mode) - if god_map.is_standalone(): - self.tree.control_loop_branch.publish_state.add_tf_publisher(include_prefix=include_prefix, - tf_topic=tf_topic, - mode=mode) - - def add_evaluate_debug_expressions(self): - self.tree.prepare_control_loop.add_compile_debug_expressions() - if god_map.is_closed_loop(): - self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=False) - else: - self.tree.control_loop_branch.add_evaluate_debug_expressions(log_traj=True) - if god_map.is_open_loop(): - god_map.tree.execute_traj.prepare_base_control.add_compile_debug_expressions() - god_map.tree.execute_traj.base_closed_loop.add_evaluate_debug_expressions(log_traj=False) - - def add_js_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False): - """ - Publishes joint states for Giskard's internal state. - """ - god_map.tree.control_loop_branch.publish_state.add_joint_state_publisher(include_prefix=include_prefix, - topic_name=topic_name, - only_prismatic_and_revolute=True) - god_map.tree.wait_for_goal.publish_state.add_joint_state_publisher(include_prefix=include_prefix, - topic_name=topic_name, - only_prismatic_and_revolute=True) - - def add_free_variable_publisher(self, topic_name: Optional[str] = None, include_prefix: bool = False): - """ - Publishes joint states for Giskard's internal state. - """ - god_map.tree.control_loop_branch.publish_state.add_joint_state_publisher(include_prefix=include_prefix, - topic_name=topic_name, - only_prismatic_and_revolute=False) - god_map.tree.wait_for_goal.publish_state.add_joint_state_publisher(include_prefix=include_prefix, - topic_name=topic_name, - only_prismatic_and_revolute=False) - - -class StandAloneBTConfig(BehaviorTreeConfig): - def __init__(self, - debug_mode: bool = False, - publish_js: bool = False, - visualization_mode: VisualizationMode = VisualizationMode.VisualsFrameLocked, - publish_free_variables: bool = False, - publish_tf: bool = True, - include_prefix: bool = False, - simulation_max_hz: Optional[float] = None): - """ - The default behavior tree for Giskard in standalone mode. Make sure to set up the robot interface accordingly. - :param debug_mode: enable various debugging tools. - :param publish_js: publish current world state. - :param publish_tf: publish all link poses in tf. - :param simulation_max_hz: if not None, will limit the frequency of the simulation. - :param include_prefix: whether to include the robot name prefix when publishing joint states or tf - """ - self.include_prefix = include_prefix - self.visualization_mode = visualization_mode - if is_running_in_pytest(): - if god_map.is_in_github_workflow(): - publish_js = False - publish_tf = False - debug_mode = False - simulation_max_hz = None - super().__init__(ControlModes.standalone, simulation_max_hz=simulation_max_hz) - self.debug_mode = debug_mode - self.publish_js = publish_js - self.publish_free_variables = publish_free_variables - self.publish_tf = publish_tf - if publish_js and publish_free_variables: - raise SetupException('publish_js and publish_free_variables cannot be True at the same time.') - - def setup(self): - self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=True, - mode=self.visualization_mode) - if self.publish_tf: - self.add_tf_publisher(include_prefix=self.include_prefix, mode=TfPublishingModes.all) - self.add_gantt_chart_plotter() - self.add_goal_graph_plotter() - if self.debug_mode: - self.add_trajectory_plotter(wait=True) - # self.add_trajectory_visualizer() - # self.add_debug_trajectory_visualizer() - self.add_debug_trajectory_plotter(wait=True) - self.add_debug_marker_publisher() - # self.add_debug_marker_publisher() - if self.publish_js: - self.add_js_publisher(include_prefix=self.include_prefix) - if self.publish_free_variables: - self.add_free_variable_publisher() - - -class OpenLoopBTConfig(BehaviorTreeConfig): - def __init__(self, - debug_mode: bool = False, - control_loop_max_hz: float = 50, - visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed, - simulation_max_hz: Optional[float] = None): - """ - The default behavior tree for Giskard in open-loop mode. It will first plan the trajectory in simulation mode - and then publish it to connected joint trajectory followers. The base trajectory is tracked with a closed-loop - controller. - :param debug_mode: enable various debugging tools. - :param control_loop_max_hz: if not None, limits the frequency of the base trajectory controller. - """ - super().__init__(ControlModes.open_loop, control_loop_max_hz=control_loop_max_hz, - simulation_max_hz=simulation_max_hz) - if god_map.is_in_github_workflow(): - debug_mode = False - self.debug_mode = debug_mode - self.visualization_mode = visualization_mode - - def setup(self): - self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=True, - mode=self.visualization_mode) - self.add_gantt_chart_plotter() - self.add_goal_graph_plotter() - if self.debug_mode or True: - self.add_trajectory_plotter(wait=True) - self.add_debug_trajectory_plotter(wait=True) - self.add_debug_marker_publisher() - self.add_qp_data_publisher( - publish_debug=True, - publish_xdot=True, - # publish_lbA=True, - # publish_ubA=True - ) - - -class ClosedLoopBTConfig(BehaviorTreeConfig): - def __init__(self, debug_mode: bool = False, - control_loop_max_hz: float = 50, - visualization_mode: VisualizationMode = VisualizationMode.CollisionsDecomposed, - simulation_max_hz: Optional[float] = None): - """ - The default configuration for Giskard in closed loop mode. Make use to set up the robot interface accordingly. - :param debug_mode: If True, will publish debug data on topics. This will significantly slow down the control loop. - :param control_loop_max_hz: Limits the control loop frequency. If None, it will go as fast as possible. - """ - super().__init__(ControlModes.close_loop, control_loop_max_hz=control_loop_max_hz, - simulation_max_hz=simulation_max_hz) - if god_map.is_in_github_workflow(): - debug_mode = False - self.debug_mode = debug_mode - self.visualization_mode = visualization_mode - - def setup(self): - self.add_visualization_marker_publisher(add_to_sync=True, add_to_control_loop=False, - mode=self.visualization_mode) - # self.add_qp_data_publisher(publish_xdot=True, publish_lb=True, publish_ub=True) - self.add_gantt_chart_plotter() - self.add_goal_graph_plotter() - if self.debug_mode: - self.add_trajectory_plotter(wait=True) - self.add_debug_trajectory_plotter(wait=True) - self.add_debug_marker_publisher() - # self.add_qp_data_publisher( - # publish_debug=True, - # publish_xdot=True, - # # publish_lbA=True, - # # publish_ubA=True - # ) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index e114043e7..22ebacdff 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -23,6 +23,10 @@ from pycrap import PhysicalObject +rospy_logger = logging.getLogger('rosout') +rospy_logger.setLevel(logging.DEBUG) + + @with_simulated_robot def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() diff --git a/demos/pycram_multiverse_demo/pr2.py b/demos/pycram_multiverse_demo/pr2.py deleted file mode 100644 index ae4cc1872..000000000 --- a/demos/pycram_multiverse_demo/pr2.py +++ /dev/null @@ -1,301 +0,0 @@ -from giskardpy.configs.giskard import CollisionAvoidanceConfig, RobotInterfaceConfig -from giskardpy.configs.world_config import WorldWithOmniDriveRobot -from giskardpy.data_types import Derivatives - - -class WorldWithPR2Config(WorldWithOmniDriveRobot): - def __init__(self, map_name: str = 'map', localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', drive_joint_name: str = 'brumbrum'): - super().__init__(map_name, localization_joint_name, odom_link_name, drive_joint_name) - - def setup(self): - super().setup() - self.set_joint_limits(limit_map={Derivatives.velocity: 2, - Derivatives.jerk: 60}, - joint_name='head_pan_joint') - self.set_joint_limits(limit_map={Derivatives.velocity: 4, - Derivatives.jerk: 120}, - joint_name='head_tilt_joint') - - - -class PR2StandaloneInterface(RobotInterfaceConfig): - drive_joint_name: str - - def __init__(self, drive_joint_name: str): - self.drive_joint_name = drive_joint_name - - def setup(self): - self.register_controlled_joints([ - 'torso_lift_joint', - 'head_pan_joint', - 'head_tilt_joint', - 'r_shoulder_pan_joint', - 'r_shoulder_lift_joint', - 'r_upper_arm_roll_joint', - 'r_forearm_roll_joint', - 'r_elbow_flex_joint', - 'r_wrist_flex_joint', - 'r_wrist_roll_joint', - 'l_shoulder_pan_joint', - 'l_shoulder_lift_joint', - 'l_upper_arm_roll_joint', - 'l_forearm_roll_joint', - 'l_elbow_flex_joint', - 'l_wrist_flex_joint', - 'l_wrist_roll_joint', - self.drive_joint_name, - ]) - - -class PR2JointTrajServerMujocoInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('/joint_states') - self.sync_odometry_topic('/pr2/base_footprint', self.drive_joint_name) - self.add_follow_joint_trajectory_server( - namespace='/pr2/whole_body_controller') - self.add_follow_joint_trajectory_server( - namespace='/pr2/l_gripper_l_finger_controller') - self.add_follow_joint_trajectory_server( - namespace='/pr2/r_gripper_l_finger_controller') - self.add_base_cmd_velocity(cmd_vel_topic='/pr2/cmd_vel', - track_only_velocity=True, - joint_name=self.drive_joint_name) - -class PR2JointTrajServerMultiverseInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'pr2', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - # self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - # tf_parent_frame=self.map_name, - # tf_child_frame=self.odom_link_name) - # self. - self.sync_joint_state_topic('/real/pr2/joint_states') - self.sync_odometry_topic('/odom', self.drive_joint_name) - self.add_follow_joint_trajectory_server( - namespace='/real/pr2/head_controller') - self.add_follow_joint_trajectory_server( - namespace='/real/pr2/torso_controller') - self.add_follow_joint_trajectory_server( - namespace='/real/pr2/left_arm_controller') - self.add_follow_joint_trajectory_server( - namespace='/real/pr2/right_arm_controller') - self.add_base_cmd_velocity(cmd_vel_topic='/cmd_vel', - track_only_velocity=True, - joint_name=self.drive_joint_name) - -class PR2VelocityMujocoInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('pr2/joint_states') - self.sync_odometry_topic('/pr2/base_footprint', self.drive_joint_name) - self.add_joint_velocity_controller(namespaces=[ - 'pr2/torso_lift_velocity_controller', - 'pr2/r_upper_arm_roll_velocity_controller', - 'pr2/r_shoulder_pan_velocity_controller', - 'pr2/r_shoulder_lift_velocity_controller', - 'pr2/r_forearm_roll_velocity_controller', - 'pr2/r_elbow_flex_velocity_controller', - 'pr2/r_wrist_flex_velocity_controller', - 'pr2/r_wrist_roll_velocity_controller', - 'pr2/l_upper_arm_roll_velocity_controller', - 'pr2/l_shoulder_pan_velocity_controller', - 'pr2/l_shoulder_lift_velocity_controller', - 'pr2/l_forearm_roll_velocity_controller', - 'pr2/l_elbow_flex_velocity_controller', - 'pr2/l_wrist_flex_velocity_controller', - 'pr2/l_wrist_roll_velocity_controller', - 'pr2/head_pan_velocity_controller', - 'pr2/head_tilt_velocity_controller', - ]) - - self.add_base_cmd_velocity(cmd_vel_topic='/pr2/cmd_vel', - joint_name=self.drive_joint_name) - - -class PR2VelocityIAIInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('/joint_states') - self.sync_odometry_topic('/robot_pose_ekf/odom_combined', self.drive_joint_name) - self.add_joint_velocity_group_controller(namespace='l_arm_joint_group_velocity_controller') - self.add_joint_velocity_group_controller(namespace='r_arm_joint_group_velocity_controller') - self.add_joint_position_controller(namespaces=[ - 'head_pan_position_controller', - 'head_tilt_position_controller', - ]) - - self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', - joint_name=self.drive_joint_name) - - -class PR2CollisionAvoidance(CollisionAvoidanceConfig): - def __init__(self, drive_joint_name: str = 'brumbrum'): - super().__init__() - self.drive_joint_name = drive_joint_name - - def setup(self): - self.load_self_collision_matrix('package://giskardpy/self_collision_matrices/iai/pr2.srdf') - self.set_default_external_collision_avoidance(soft_threshold=0.1, - hard_threshold=0.0) - for joint_name in ['r_wrist_roll_joint', 'l_wrist_roll_joint']: - self.overwrite_external_collision_avoidance(joint_name, - number_of_repeller=4, - soft_threshold=0.05, - hard_threshold=0.0, - max_velocity=0.2) - for joint_name in ['r_wrist_flex_joint', 'l_wrist_flex_joint']: - self.overwrite_external_collision_avoidance(joint_name, - number_of_repeller=2, - soft_threshold=0.05, - hard_threshold=0.0, - max_velocity=0.2) - for joint_name in ['r_elbow_flex_joint', 'l_elbow_flex_joint']: - self.overwrite_external_collision_avoidance(joint_name, - soft_threshold=0.05, - hard_threshold=0.0) - for joint_name in ['r_forearm_roll_joint', 'l_forearm_roll_joint']: - self.overwrite_external_collision_avoidance(joint_name, - soft_threshold=0.025, - hard_threshold=0.0) - self.fix_joints_for_collision_avoidance([ - 'r_gripper_l_finger_joint', - 'l_gripper_l_finger_joint' - ]) - self.overwrite_external_collision_avoidance(self.drive_joint_name, - number_of_repeller=2, - soft_threshold=0.2, - hard_threshold=0.1) - - -class PR2JointTrajServerIAIInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('/joint_states') - self.sync_odometry_topic('/robot_pose_ekf/odom_combined', self.drive_joint_name) - fill_velocity_values = False - self.add_follow_joint_trajectory_server(namespace='/l_arm_controller', - fill_velocity_values=fill_velocity_values) - self.add_follow_joint_trajectory_server(namespace='/r_arm_controller', - fill_velocity_values=fill_velocity_values) - self.add_follow_joint_trajectory_server(namespace='/torso_controller', - fill_velocity_values=fill_velocity_values) - self.add_follow_joint_trajectory_server(namespace='/head_traj_controller', - fill_velocity_values=fill_velocity_values) - self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', - track_only_velocity=True, - joint_name=self.drive_joint_name) - - -class PR2JointTrajServerUnrealInterface(RobotInterfaceConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom_combined', - drive_joint_name: str = 'brumbrum'): - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.sync_6dof_joint_with_tf_frame(joint_name=self.localization_joint_name, - tf_parent_frame=self.map_name, - tf_child_frame=self.odom_link_name) - self.sync_joint_state_topic('/joint_states') - self.sync_odometry_topic('/base_odometry/odom', self.drive_joint_name) - fill_velocity_values = False - self.add_follow_joint_trajectory_server(namespace='/whole_body_controller', - fill_velocity_values=fill_velocity_values) - self.add_follow_joint_trajectory_server(namespace='/head_traj_controller', - fill_velocity_values=fill_velocity_values) - self.add_base_cmd_velocity(cmd_vel_topic='/base_controller/command', - track_only_velocity=True, - joint_name=self.drive_joint_name) diff --git a/demos/pycram_multiverse_demo/world_config.py b/demos/pycram_multiverse_demo/world_config.py deleted file mode 100644 index 6a177ac5a..000000000 --- a/demos/pycram_multiverse_demo/world_config.py +++ /dev/null @@ -1,333 +0,0 @@ -from __future__ import annotations - -import abc -from abc import ABC -from typing import Dict, Optional, Union - -import numpy as np -import rospy -from numpy.typing import NDArray -from std_msgs.msg import ColorRGBA - -from giskardpy.god_map import god_map -from giskardpy.model.joints import FixedJoint, OmniDrive, DiffDrive, Joint6DOF, OneDofJoint -from giskardpy.model.links import Link -from giskardpy.model.utils import robot_name_from_urdf_string -from giskardpy.model.world import WorldTree -from giskardpy.data_types import my_string, PrefixName, Derivatives, derivative_map - - -class WorldConfig(ABC): - - def __init__(self): - god_map.world = WorldTree() - self.set_default_weights() - - @property - def world(self) -> WorldTree: - return god_map.world - - def set_defaults(self): - pass - - @abc.abstractmethod - def setup(self): - """ - Implement this method to configure the initial world using it's self. methods. - """ - - @property - def robot_group_name(self) -> str: - return god_map.world.robot_name - - def set_default_weights(self, - velocity_weight: float = 0.01, - acceleration_weight: float = 0, - jerk_weight: float = 0.01): - """ - The default values are set automatically, even if this function is not called. - A typical goal has a weight of 1, so the values in here should be sufficiently below that. - """ - god_map.world.update_default_weights({Derivatives.velocity: velocity_weight, - Derivatives.acceleration: acceleration_weight, - Derivatives.jerk: jerk_weight}) - - def set_weight(self, weight_map: derivative_map, joint_name: str, group_name: Optional[str] = None): - """ - Set weights for joints that are used by the qp controller. Don't change this unless you know what you are doing. - """ - joint_name = god_map.world.search_for_joint_name(joint_name, group_name) - joint = god_map.world.joints[joint_name] - if not isinstance(joint, OneDofJoint): - raise ValueError(f'Can\'t change weight because {joint_name} is not of type {str(OneDofJoint)}.') - free_variable = god_map.world.free_variables[joint.free_variable.name] - for derivative, weight in weight_map.items(): - free_variable.quadratic_weights[derivative] = weight - - def get_root_link_of_group(self, group_name: str) -> PrefixName: - return god_map.world.groups[group_name].root_link_name - - def set_joint_limits(self, limit_map: derivative_map, joint_name: my_string, group_name: Optional[str] = None): - """ - Set the joint limits for individual joints - :param limit_map: maps Derivatives to values, e.g. {Derivatives.velocity: 1, - Derivatives.acceleration: np.inf, - Derivatives.jerk: 30} - """ - joint_name = god_map.world.search_for_joint_name(joint_name, group_name) - joint = god_map.world.joints[joint_name] - if not isinstance(joint, OneDofJoint): - raise ValueError(f'Can\'t change limits because {joint_name} is not of type {str(OneDofJoint)}.') - free_variable = god_map.world.free_variables[joint.free_variable.name] - for derivative, limit in limit_map.items(): - free_variable.set_lower_limit(derivative, -limit) - free_variable.set_upper_limit(derivative, limit) - - def set_default_color(self, r: float, g: float, b: float, a: float): - """ - :param r: 0-1 - :param g: 0-1 - :param b: 0-1 - :param a: 0-1 - """ - god_map.world.default_link_color = ColorRGBA(r, g, b, a) - - def set_default_limits(self, new_limits: derivative_map): - """ - The default values will be set automatically, even if this function is not called. - :param new_limits: e.g. {Derivatives.velocity: 1, - Derivatives.acceleration: np.inf, - Derivatives.jerk: 30} - """ - god_map.world.update_default_limits(new_limits) - - def add_robot_urdf(self, - urdf: str, - group_name: str) -> str: - """ - Add a robot urdf to the world. - :param urdf: robot urdf as string, not the path - :param group_name: - """ - if group_name is None: - group_name = robot_name_from_urdf_string(urdf) - god_map.world.add_urdf(urdf=urdf, group_name=group_name, actuated=True) - return group_name - - def add_robot_from_parameter_server(self, - parameter_name: str = 'robot_description', - group_name: Optional[str] = None) -> str: - """ - Add a robot urdf from parameter server to Giskard. - :param parameter_name: - :param group_name: How to call the robot. If nothing is specified it will get the name it has in the urdf - """ - urdf = rospy.get_param(parameter_name) - return self.add_robot_urdf(urdf=urdf, group_name=group_name) - - def add_fixed_joint(self, parent_link: my_string, child_link: my_string, - homogenous_transform: Optional[NDArray] = None): - """ - Add a fixed joint to Giskard's world. Can be used to e.g. connect a non-mobile robot to the world frame. - :param parent_link: - :param child_link: - :param homogenous_transform: a 4x4 transformation matrix. - """ - if homogenous_transform is None: - homogenous_transform = np.eye(4) - parent_link = god_map.world.search_for_link_name(parent_link) - - child_link = PrefixName.from_string(child_link, set_none_if_no_slash=True) - joint_name = PrefixName(f'{parent_link}_{child_link}_fixed_joint', None) - joint = FixedJoint(name=joint_name, parent_link_name=parent_link, child_link_name=child_link, - parent_T_child=homogenous_transform) - god_map.world._add_joint(joint) - - def add_diff_drive_joint(self, - name: str, - parent_link_name: my_string, - child_link_name: my_string, - robot_group_name: Optional[str] = None, - translation_limits: Optional[derivative_map] = None, - rotation_limits: Optional[derivative_map] = None): - """ - Same as add_omni_drive_joint, but for a differential drive. - """ - joint_name = PrefixName(name, robot_group_name) - parent_link_name = PrefixName.from_string(parent_link_name, set_none_if_no_slash=True) - child_link_name = PrefixName.from_string(child_link_name, set_none_if_no_slash=True) - brumbrum_joint = DiffDrive(parent_link_name=parent_link_name, - child_link_name=child_link_name, - name=joint_name, - translation_limits=translation_limits, - rotation_limits=rotation_limits) - god_map.world._add_joint(brumbrum_joint) - god_map.world.deregister_group(robot_group_name) - god_map.world.register_group(robot_group_name, root_link_name=parent_link_name, actuated=True) - - def add_6dof_joint(self, parent_link: my_string, child_link: my_string, joint_name: my_string): - """ - Add a 6dof joint to Giskard's world. Generally used if you want Giskard to keep track of a tf transform, - e.g. for localization. - :param parent_link: - :param child_link: - """ - parent_link = god_map.world.search_for_link_name(parent_link) - child_link = PrefixName.from_string(child_link, set_none_if_no_slash=True) - joint_name = PrefixName.from_string(joint_name, set_none_if_no_slash=True) - joint = Joint6DOF(name=joint_name, parent_link_name=parent_link, child_link_name=child_link) - god_map.world._add_joint(joint) - - def add_empty_link(self, link_name: my_string): - """ - If you need a virtual link during your world building. - """ - link = Link(link_name) - god_map.world._add_link(link) - - def add_omni_drive_joint(self, - name: str, - parent_link_name: Union[str, PrefixName], - child_link_name: Union[str, PrefixName], - robot_group_name: Optional[str] = None, - translation_limits: Optional[derivative_map] = None, - rotation_limits: Optional[derivative_map] = None, - x_name: Optional[PrefixName] = None, - y_name: Optional[PrefixName] = None, - yaw_vel_name: Optional[PrefixName] = None): - """ - Use this to connect a robot urdf of a mobile robot to the world if it has an omni-directional drive. - :param parent_link_name: - :param child_link_name: - :param robot_group_name: set if there are multiple robots - :param name: Name of the new link. Has to be unique and may be required in other functions. - :param translation_limits: in m/s**3 - :param rotation_limits: in rad/s**3 - """ - joint_name = PrefixName(name, robot_group_name) - parent_link_name = PrefixName.from_string(parent_link_name, set_none_if_no_slash=True) - child_link_name = PrefixName.from_string(child_link_name, set_none_if_no_slash=True) - brumbrum_joint = OmniDrive(parent_link_name=parent_link_name, - child_link_name=child_link_name, - name=joint_name, - translation_limits=translation_limits, - rotation_limits=rotation_limits, - x_name=x_name, - y_name=y_name, - yaw_name=yaw_vel_name) - god_map.world._add_joint(brumbrum_joint) - god_map.world.deregister_group(robot_group_name) - god_map.world.register_group(robot_group_name, root_link_name=parent_link_name, actuated=True) - - -class EmptyWorld(WorldConfig): - def __init__(self): - super().__init__() - - def setup(self): - self._default_limits = { - Derivatives.velocity: 1, - } - self.set_default_weights(velocity_weight=1, acceleration_weight=1, jerk_weight=1) - self.set_default_limits(self._default_limits) - self.add_empty_link('map') - - -class WorldWithFixedRobot(WorldConfig): - def __init__(self, joint_limits: Dict[Derivatives, float] = None): - super().__init__() - self._joint_limits = joint_limits - - def setup(self): - self.set_default_limits(self._joint_limits) - self.add_robot_from_parameter_server() - - -class WorldWithOmniDriveRobot(WorldConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom', - drive_joint_name: str = 'brumbrum'): - super().__init__() - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.set_default_limits({Derivatives.velocity: 1, - Derivatives.acceleration: np.inf, - Derivatives.jerk: 30}) - self.add_empty_link(self.map_name) - self.add_empty_link(self.odom_link_name) - self.add_fixed_joint(parent_link=self.map_name, child_link=self.odom_link_name) - # self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, - # joint_name=self.localization_joint_name) - self.add_robot_from_parameter_server() - root_link_name = self.get_root_link_of_group(self.robot_group_name) - self.add_omni_drive_joint(name=self.drive_joint_name, - parent_link_name=self.odom_link_name, - child_link_name=root_link_name, - translation_limits={ - Derivatives.velocity: 0.2, - Derivatives.acceleration: 1, - Derivatives.jerk: 5, - }, - rotation_limits={ - Derivatives.velocity: 0.2, - Derivatives.acceleration: 1, - Derivatives.jerk: 5 - }, - robot_group_name=self.robot_group_name) - self.set_joint_limits(limit_map={Derivatives.velocity: 3, - Derivatives.jerk: 60}, - joint_name='head_pan_joint') - - -class WorldWithDiffDriveRobot(WorldConfig): - map_name: str - localization_joint_name: str - odom_link_name: str - drive_joint_name: str - - def __init__(self, - map_name: str = 'map', - localization_joint_name: str = 'localization', - odom_link_name: str = 'odom', - drive_joint_name: str = 'brumbrum'): - super().__init__() - self.map_name = map_name - self.localization_joint_name = localization_joint_name - self.odom_link_name = odom_link_name - self.drive_joint_name = drive_joint_name - - def setup(self): - self.set_default_limits({Derivatives.velocity: 1, - Derivatives.acceleration: np.inf, - Derivatives.jerk: 30}) - self.add_empty_link(self.map_name) - self.add_empty_link(self.odom_link_name) - self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, - joint_name=self.localization_joint_name) - self.add_robot_from_parameter_server() - root_link_name = self.get_root_link_of_group(self.robot_group_name) - self.add_diff_drive_joint(name=self.drive_joint_name, - parent_link_name=self.odom_link_name, - child_link_name=root_link_name, - translation_limits={ - Derivatives.velocity: 0.2, - Derivatives.acceleration: 1, - Derivatives.jerk: 5, - }, - rotation_limits={ - Derivatives.velocity: 0.2, - Derivatives.acceleration: 1, - Derivatives.jerk: 5 - }, - robot_group_name=self.robot_group_name) From aea959323c213ed54998292c319babdfc1685e45 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Nov 2024 15:44:16 +0100 Subject: [PATCH 11/49] [ActionDesignator] better prepose and approach technique for picking up milk. --- src/pycram/process_modules/pr2_process_modules.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 85cd00e0b..0ae24093e 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -49,7 +49,7 @@ def feedback_callback(msg): loginfo(f"Gripper Action Feedback: {msg}") goal = GripperCommandGoal() - goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.4 + goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.7 goal.command.max_effort = 50.0 if designator.gripper == "right": controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd" From aafcf49adf0fa8e004cef800287a95d5bf45f5b3 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Nov 2024 19:15:52 +0100 Subject: [PATCH 12/49] [MultiverseFallschoolDemo] The demo works. --- src/pycram/process_modules/pr2_process_modules.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 0ae24093e..85cd00e0b 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -49,7 +49,7 @@ def feedback_callback(msg): loginfo(f"Gripper Action Feedback: {msg}") goal = GripperCommandGoal() - goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.7 + goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.4 goal.command.max_effort = 50.0 if designator.gripper == "right": controller_topic = "/real/pr2/right_gripper_controller/gripper_cmd" From f9dd053e75c1825a23b46c97066f599fefa7d328 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 13 Nov 2024 12:32:56 +0100 Subject: [PATCH 13/49] [MultiverseFallschoolDemo] try and redo actions Added mutex for multiverse writer client --- src/pycram/worlds/multiverse_communication/clients.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index e02ee92aa..7bca176f6 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -485,6 +485,7 @@ def send_data_to_server(self, data: List, if receive_meta_data: self.request_meta_data["receive"] = receive_meta_data self.send_and_receive_meta_data() + self.lock.release() self.send_data = data self.send_and_receive_data() response_meta_data = self.response_meta_data From 5bf6eced009b79040f208657a54e607a69b35ac7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 20 Nov 2024 09:39:03 +0100 Subject: [PATCH 14/49] [FixORM] added new paramters to orm classes to match actions/motions. --- test/test_task_tree.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/test_task_tree.py b/test/test_task_tree.py index 8d7fcaca3..26fd3002d 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -35,8 +35,8 @@ def test_tree_creation(self): # self.tearDownBulletWorld() tt = pycram.tasktree.task_tree - self.assertEqual(15, len(tt.root)) - self.assertEqual(10, len(tt.root.leaves)) + self.assertEqual(16, len(tt.root)) + self.assertEqual(11, len(tt.root.leaves)) # check that all nodes succeeded for node in anytree.PreOrderIter(tt.root): @@ -74,8 +74,8 @@ def test_simulated_tree(self): self.plan() tt = pycram.tasktree.task_tree - self.assertEqual(15, len(tt.root)) - self.assertEqual(10, len(tt.root.leaves)) + self.assertEqual(16, len(tt.root)) + self.assertEqual(11, len(tt.root.leaves)) self.assertEqual(len(pycram.tasktree.task_tree), 1) From e04a49b2db1104dc1bd32f6d8971cc8085d6e95b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 20 Nov 2024 17:59:45 +0100 Subject: [PATCH 15/49] [PhysicalObject] Added a physical object class. --- src/pycram/datastructures/dataclasses.py | 53 +++++++++ src/pycram/datastructures/world_entity.py | 127 +++++++++++++++++++++- src/pycram/object_descriptors/urdf.py | 6 +- src/pycram/world_concepts/world_object.py | 1 + 4 files changed, 181 insertions(+), 6 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index b32e713b6..a7e7c433d 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -478,6 +478,58 @@ class State(ABC): pass +@dataclass +class PhysicalBodyState(State): + """ + Dataclass for storing the state of a physical body. + """ + pose: Pose + is_translating: Optional[bool] + is_rotating: Optional[bool] + velocity: Optional[List[float]] + contact_points: Optional[ContactPointsList] + acceptable_pose_error: Tuple[float, float] = (0.001, 0.001) + acceptable_velocity_error: Tuple[float, float] = (0.001, 0.001) + + def __eq__(self, other: 'PhysicalBodyState'): + return (self.pose_is_almost_equal(other) and self.is_translating == other.is_translating + and self.is_rotating == other.is_rotating and self.velocity_is_almost_equal(other), + self.contact_points == other.contact_points) + + def pose_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: + """ + Check if the pose of the object is almost equal to the pose of another object. + + :param other: The state of the other object. + :return: True if the poses are almost equal, False otherwise. + """ + return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1]) + + def velocity_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: + """ + Check if the velocity of the object is almost equal to the velocity of another object. + + :param other: The state of the other object. + :return: True if the velocities are almost equal, False otherwise. + """ + return ((self.velocity is None and other.velocity is None) or + (self.vector_is_almost_equal(self.velocity, other.velocity, self.acceptable_velocity_error[0]) and + self.is_translating == other.is_translating and self.is_rotating == other.is_rotating) + ) + + @staticmethod + def vector_is_almost_equal(vector1: List[float], vector2: List[float], acceptable_error: float) -> bool: + """ + Check if the vector is almost equal to another vector. + + :param vector1: The first vector. + :param vector2: The second vector. + :param acceptable_error: The acceptable error. + :return: True if the vectors are almost equal, False otherwise. + """ + return np.all(np.array(vector1) - np.array(vector2) <= acceptable_error) + + @dataclass class LinkState(State): """ @@ -815,6 +867,7 @@ class VirtualJoint: name: str type_: JointType axes: Optional[Point] = None + object: Optional[Object] = None @property def type(self): diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 41c317fad..76878d2cb 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -1,13 +1,16 @@ +from __future__ import annotations + import os import pickle from abc import ABC, abstractmethod -from typing_extensions import TYPE_CHECKING, Dict, Optional +from typing_extensions import TYPE_CHECKING, Dict, Optional, List -from .dataclasses import State +from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, VisualShape, PhysicalBodyState if TYPE_CHECKING: from ..datastructures.world import World + from .pose import Pose, Point, GeoQuaternion as Quaternion class StateEntity: @@ -110,10 +113,124 @@ def remove_saved_states(self) -> None: class WorldEntity(StateEntity, ABC): """ - A data class that represents an entity of the world, such as an object or a link. + A class that represents an entity of the world, such as an object or a link. """ - def __init__(self, _id: int, world: 'World'): + def __init__(self, _id: int, world: World): StateEntity.__init__(self) self.id = _id - self.world: 'World' = world + self.world: World = world + + +class PhysicalBody(WorldEntity, ABC): + """ + A class that represents a physical body in the world that has some related physical properties. + """ + + def __init__(self, body_id: int, world: World): + WorldEntity.__init__(self, body_id, world) + self._is_translating: Optional[bool] = None + self._is_rotating: Optional[bool] = None + self._velocity: Optional[List[float]] = None + + @property + def current_state(self) -> PhysicalBodyState: + return PhysicalBodyState(self.pose, self.is_translating, self.is_rotating, self.velocity, self.contact_points) + + @property + def velocity(self) -> Optional[List[float]]: + return self._velocity + + @velocity.setter + def velocity(self, velocity: List[float]) -> None: + self._velocity = velocity + + @property + def is_translating(self) -> Optional[bool]: + return self._is_translating + + @is_translating.setter + def is_translating(self, is_translating: bool) -> None: + self._is_translating = is_translating + + @property + def is_rotating(self) -> Optional[bool]: + return self._is_rotating + + @is_rotating.setter + def is_rotating(self, is_rotating: bool) -> None: + self._is_rotating = is_rotating + + @abstractmethod + @property + def color(self) -> Color: + """ + :return: The color of this body. + """ + ... + + @abstractmethod + @property + def shape(self) -> VisualShape: + """ + :return: The shape of this body. + """ + ... + + @abstractmethod + @property + def pose(self) -> Pose: + """ + :return: The pose of this entity. + """ + ... + + @abstractmethod + @property + def contact_points(self) -> ContactPointsList: + """ + :return: The contact points of this body with other physical bodies. + """ + ... + + @abstractmethod + def get_contact_points_with_body(self, body: 'PhysicalBody') -> ContactPointsList: + """ + :param body: The body to get the contact points with. + :return: The contact points of this body with the given body. + """ + ... + + @abstractmethod + @property + def distances(self) -> Dict['PhysicalBody', float]: + """ + :return: The closest distances of this body to other physical bodies. + """ + ... + + @abstractmethod + def get_distance_with_body(self, body: 'PhysicalBody') -> float: + """ + :param body: The body to get the distance with. + :return: The closest distance of this body to the given body. + """ + ... + + @property + def is_moving(self) -> Optional[bool]: + """ + :return: True if this body is moving, False if not, and None if not known. + """ + if self.is_translating is not None or self.is_rotating is not None: + return self.is_translating or self.is_rotating + else: + return None + + @property + def is_stationary(self) -> Optional[bool]: + """ + :return: True if this body is stationary, False otherwise. + """ + return None if self.is_moving is None else not self.is_moving + diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index bdb48b8f7..bf7c196a8 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -4,7 +4,7 @@ import numpy as np -from ..ros.logging import logerr +from ..ros.logging import logerr, logwarn from ..ros.ros_tools import create_ros_pack, ResourceNotFound, get_parameter from geometry_msgs.msg import Point from tf.transformations import quaternion_from_euler, euler_from_quaternion @@ -82,6 +82,10 @@ def collision(self) -> Union[Collision, List[Collision], None]: else: return self.parsed_description.collisions + @property + def all_collisions(self) -> List[Collision]: + return self.parsed_description.collisions + class JointDescription(AbstractJointDescription): urdf_type_map = {'unknown': JointType.UNKNOWN, diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 034385126..80cee2c0a 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -25,6 +25,7 @@ from ..local_transformer import LocalTransformer from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..object_descriptors.urdf import ObjectDescription as URDF +from ..ros.data_types import Time from ..ros.logging import logwarn try: From e734b00b22007230e6d277f0c0cd07cb5e5525cf Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 20 Nov 2024 20:42:04 +0100 Subject: [PATCH 16/49] [PhysicalObject] remove contacts, object state has a physical body state. --- src/pycram/datastructures/dataclasses.py | 29 ++++++++--------------- src/pycram/datastructures/world_entity.py | 2 +- 2 files changed, 11 insertions(+), 20 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index a7e7c433d..2656cdad3 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -484,17 +484,18 @@ class PhysicalBodyState(State): Dataclass for storing the state of a physical body. """ pose: Pose - is_translating: Optional[bool] - is_rotating: Optional[bool] - velocity: Optional[List[float]] - contact_points: Optional[ContactPointsList] + is_translating: bool + is_rotating: bool + velocity: List[float] acceptable_pose_error: Tuple[float, float] = (0.001, 0.001) acceptable_velocity_error: Tuple[float, float] = (0.001, 0.001) def __eq__(self, other: 'PhysicalBodyState'): - return (self.pose_is_almost_equal(other) and self.is_translating == other.is_translating - and self.is_rotating == other.is_rotating and self.velocity_is_almost_equal(other), - self.contact_points == other.contact_points) + return (self.pose_is_almost_equal(other) + and self.is_translating == other.is_translating + and self.is_rotating == other.is_rotating + and self.velocity_is_almost_equal(other) + ) def pose_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: """ @@ -585,27 +586,17 @@ class ObjectState(State): """ Dataclass for storing the state of an object. """ - pose: Pose + body_state: PhysicalBodyState attachments: Dict[Object, Attachment] link_states: Dict[int, LinkState] joint_states: Dict[int, JointState] - acceptable_pose_error: Tuple[float, float] def __eq__(self, other: 'ObjectState'): - return (self.pose_is_almost_equal(other) + return (self.body_state == other.body_state and self.all_attachments_exist(other) and self.all_attachments_are_equal(other) and self.link_states == other.link_states and self.joint_states == other.joint_states) - def pose_is_almost_equal(self, other: 'ObjectState') -> bool: - """ - Check if the pose of the object is almost equal to the pose of another object. - - :param other: The state of the other object. - :return: True if the poses are almost equal, False otherwise. - """ - return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1]) - def all_attachments_exist(self, other: 'ObjectState') -> bool: """ Check if all attachments exist in the other object state. diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 76878d2cb..09be487b4 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -135,7 +135,7 @@ def __init__(self, body_id: int, world: World): @property def current_state(self) -> PhysicalBodyState: - return PhysicalBodyState(self.pose, self.is_translating, self.is_rotating, self.velocity, self.contact_points) + return PhysicalBodyState(self.pose, self.is_translating, self.is_rotating, self.velocity) @property def velocity(self) -> Optional[List[float]]: From 09189655a1d42cf7d389194e8d84c1e28e0391cf Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 21 Nov 2024 16:03:42 +0100 Subject: [PATCH 17/49] [PhysicalBody] (WIP) refactoring bullet world to conform with new/changed world methods. --- src/pycram/datastructures/world.py | 122 ++++++++++++++--- src/pycram/datastructures/world_entity.py | 141 +++++++++++++++++--- src/pycram/description.py | 155 +++++----------------- src/pycram/world_concepts/constraints.py | 2 +- src/pycram/world_concepts/world_object.py | 123 ++++++++--------- src/pycram/world_reasoning.py | 4 +- src/pycram/worlds/bullet_world.py | 25 +++- src/pycram/worlds/multiverse.py | 2 +- test/test_multiverse.py | 5 +- 9 files changed, 342 insertions(+), 237 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index ed5978cd0..eecf8235f 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -10,7 +10,7 @@ import numpy as np from geometry_msgs.msg import Point from trimesh.parent import Geometry3D -from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type +from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type, deprecated import pycrap from pycrap import PhysicalObject, Floor, Apartment, Robot @@ -24,7 +24,7 @@ ContactPointsList, VirtualMobileBaseJoints, RotatedBoundingBox) from ..datastructures.enums import JointType, WorldMode, Arms from ..datastructures.pose import Pose, Transform -from ..datastructures.world_entity import StateEntity +from ..datastructures.world_entity import StateEntity, PhysicalBody from ..failures import ProspectionObjectNotFound, WorldObjectNotFound from ..local_transformer import LocalTransformer from ..robot_description import RobotDescription @@ -137,6 +137,65 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self.on_add_object_callbacks: List[Callable[[Object], None]] = [] + def get_link_contact_points(self, link: Link) -> ContactPointsList: + """ + Return the contact points of a link with all other objects in the world. + + :param link: The link. + :return: A list of contact points. + """ + raise NotImplementedError + + def get_link_contact_points_with_body(self, link: Link, body: PhysicalBody) -> ContactPointsList: + """ + Return the contact points of a link with a specific body in the world. + + :param link: The link. + :param body: The body. + :return: A list of contact points. + """ + raise NotImplementedError + + def get_link_distance_with_body(self, link: Link, body: PhysicalBody) -> float: + """ + Return the distance of a link with a specific body in the world. + + :param link: The link. + :param body: The body. + :return: The distance between the link and the body. + """ + raise NotImplementedError + + def get_link_closest_points_with_body(self, link: Link, body: PhysicalBody, max_distance: float) -> ClosestPointsList: + """ + Return the closest points of a link with a specific body in the world. + + :param link: The link. + :param body: The body. + :param max_distance: The maximum distance between the points. + :return: A list of closest points. + """ + raise NotImplementedError + + def get_link_distances(self, link: Link) -> Dict[PhysicalBody, float]: + """ + Return the distances of a link with all other objects in the world. + + :param link: The link. + :return: A dictionary of object names and distances. + """ + raise NotImplementedError + + def get_link_closest_points(self, link: Link, max_distance: float) -> ClosestPointsList: + """ + Return the closest points of a link with all other objects in the world. + + :param link: The link. + :param max_distance: The maximum distance between the points. + :return: A list of closest points. + """ + raise NotImplementedError + def get_object_convex_hull(self, obj: Object) -> Geometry3D: """ Get the convex hull of an object. @@ -636,7 +695,7 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False, curr_time = Time().now() self.step(func) for objects, callbacks in self.coll_callbacks.items(): - contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1]) + contact_points = self.get_contact_points_between_two_bodies(objects[0], objects[1]) if len(contact_points) > 0: callbacks.on_collision_cb() elif callbacks.no_collision_cb is not None: @@ -645,14 +704,6 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False, loop_time = Time().now() - curr_time time_diff = self.simulation_time_step - loop_time.to_sec() time.sleep(max(0, time_diff)) - self.update_all_objects_poses() - - def update_all_objects_poses(self) -> None: - """ - Update the positions of all objects in the world. - """ - for obj in self.objects: - obj.update_pose() @abstractmethod def get_object_pose(self, obj: Object) -> Pose: @@ -737,7 +788,7 @@ def perform_collision_detection(self) -> None: """ pass - @abstractmethod + @deprecated("Use get_body_contact_points instead") def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Return a list of contact points of this Object with all other Objects. @@ -745,16 +796,53 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: :param obj: The object. :return: A list of all contact points with other objects """ - pass + return self.get_body_contact_points(obj) @abstractmethod + def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: + """ + Return the contact points of a body with all other bodies in the world. + + :param body: The body. + """ + pass + + @deprecated("Use get_contact_points_between_two_bodies instead") def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: """ - Return a list of contact points between obj_a and obj_b. + Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of bodies. + """ + return self.get_contact_points_between_two_bodies(obj1, obj2) + + @abstractmethod + def get_contact_points_between_two_bodies(self, body_1: PhysicalBody, body_2: PhysicalBody) -> ContactPointsList: + """ + Return a list of contact points between two bodies. + + :param body_1: The first body. + :param body_2: The second body. + :return: A list of all contact points between the two bodies. + """ + pass + + @abstractmethod + def get_body_distances(self, body: PhysicalBody) -> Dict[PhysicalBody, float]: + """ + Return the distances of this body with all other bodies in the world. + + :param body: The body. + :return: A dictionary of body names and distances. + """ + pass + + @abstractmethod + def get_distance_between_two_bodies(self, body_1: PhysicalBody, body_2: PhysicalBody) -> float: + """ + Return the distance between two bodies. - :param obj1: The first object. - :param obj2: The second object. - :return: A list of all contact points between the two objects. + :param body_1: The first body. + :param body_2: The second body. + :return: The distance between the two bodies. """ pass diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 09be487b4..a46eb0ebf 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -4,13 +4,17 @@ import pickle from abc import ABC, abstractmethod -from typing_extensions import TYPE_CHECKING, Dict, Optional, List +from trimesh.parent import Geometry3D +from typing_extensions import TYPE_CHECKING, Dict, Optional, List, Any, deprecated, Union -from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, VisualShape, PhysicalBodyState +from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, VisualShape, PhysicalBodyState, \ + AxisAlignedBoundingBox, RotatedBoundingBox +from ..local_transformer import LocalTransformer +from ..ros.data_types import Time if TYPE_CHECKING: from ..datastructures.world import World - from .pose import Pose, Point, GeoQuaternion as Quaternion + from .pose import Pose, Point, GeoQuaternion as Quaternion, Transform class StateEntity: @@ -129,10 +133,38 @@ class PhysicalBody(WorldEntity, ABC): def __init__(self, body_id: int, world: World): WorldEntity.__init__(self, body_id, world) + self.local_transformer = LocalTransformer() self._is_translating: Optional[bool] = None self._is_rotating: Optional[bool] = None self._velocity: Optional[List[float]] = None + @abstractmethod + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis-aligned bounding box of this body. + """ + ... + + @abstractmethod + def get_rotated_bounding_box(self) -> RotatedBoundingBox: + """ + :return: The rotated bounding box of this body. + """ + ... + + def _plot_convex_hull(self): + """ + Plot the convex hull of the geometry. + """ + self.get_convex_hull().show() + + @abstractmethod + def get_convex_hull(self) -> Geometry3D: + """ + :return: The convex hull of this body. + """ + ... + @property def current_state(self) -> PhysicalBodyState: return PhysicalBodyState(self.pose, self.is_translating, self.is_rotating, self.velocity) @@ -161,61 +193,103 @@ def is_rotating(self) -> Optional[bool]: def is_rotating(self, is_rotating: bool) -> None: self._is_rotating = is_rotating - @abstractmethod @property - def color(self) -> Color: + def position(self) -> Point: """ - :return: The color of this body. + :return: A Point object containing the position of the link relative to the world frame. """ - ... + return self.pose.position - @abstractmethod @property - def shape(self) -> VisualShape: + def position_as_list(self) -> List[float]: """ - :return: The shape of this body. + :return: A list containing the position of the link relative to the world frame. """ - ... + return self.pose.position_as_list() + + @property + def orientation(self) -> Quaternion: + """ + :return: A Quaternion object containing the orientation of the link relative to the world frame. + """ + return self.pose.orientation + + @property + def orientation_as_list(self) -> List[float]: + """ + :return: A list containing the orientation of the link relative to the world frame. + """ + return self.pose.orientation_as_list() + + @property + def pose_as_list(self) -> List[List[float]]: + """ + :return: A list containing the position and orientation of the link relative to the world frame. + """ + return self.pose.to_list() + + @property + def transform(self) -> Transform: + """ + The transform of this entity. + + :return: The transform of this entity. + """ + return self.pose.to_transform(self.tf_frame) + + def update_transform(self, transform_time: Optional[Time] = None) -> None: + """ + Update the transformation of this link at the given time. + + :param transform_time: The time at which the transformation should be updated. + """ + self.local_transformer.update_transforms([self.transform], transform_time) - @abstractmethod @property + @abstractmethod def pose(self) -> Pose: """ - :return: The pose of this entity. + :return: A Pose object containing the pose of the link relative to the world frame. """ ... + @property @abstractmethod + def tf_frame(self) -> str: + """ + The tf frame of this entity. + + :return: The tf frame of this entity. + """ + pass + @property def contact_points(self) -> ContactPointsList: """ :return: The contact points of this body with other physical bodies. """ - ... + return self.world.get_body_contact_points(self) - @abstractmethod def get_contact_points_with_body(self, body: 'PhysicalBody') -> ContactPointsList: """ :param body: The body to get the contact points with. :return: The contact points of this body with the given body. """ - ... + return self.world.get_contact_points_between_two_bodies(self, body) - @abstractmethod @property def distances(self) -> Dict['PhysicalBody', float]: """ :return: The closest distances of this body to other physical bodies. """ - ... + return self.world.get_body_distances(self) - @abstractmethod def get_distance_with_body(self, body: 'PhysicalBody') -> float: """ :param body: The body to get the distance with. :return: The closest distance of this body to the given body. """ - ... + return self.world.get_distance_between_two_bodies(self, body) @property def is_moving(self) -> Optional[bool]: @@ -234,3 +308,30 @@ def is_stationary(self) -> Optional[bool]: """ return None if self.is_moving is None else not self.is_moving + @property + @abstractmethod + def color(self) -> Union[Color, Dict[str, Color]]: + """ + :return: A Color object containing the rgba_color of this body or a dictionary if the body is articulated. + """ + ... + + @deprecated("Use color property setter instead") + def set_color(self, color: Color) -> None: + """ + Set the color of this body, could be rgb or rgba. + + :param color: The color as a list of floats, either rgb or rgba. + """ + self.color = color + + @color.setter + @abstractmethod + def color(self, color: Color) -> None: + """ + Set the color of this body, could be rgb or rgba. + + :param color: The color as a list of floats, either rgb or rgba. + """ + ... + diff --git a/src/pycram/description.py b/src/pycram/description.py index 10257a9ab..817bd585c 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -5,18 +5,16 @@ import pathlib from abc import ABC, abstractmethod -from trimesh.parent import Geometry3D - -from .ros.data_types import Time import trimesh -from geometry_msgs.msg import Point, Quaternion -from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, deprecated +from geometry_msgs.msg import Point +from trimesh.parent import Geometry3D +from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, Sequence from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \ - MeshVisualShape, RotatedBoundingBox + MeshVisualShape, RotatedBoundingBox, ContactPointsList from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform -from .datastructures.world_entity import WorldEntity +from .datastructures.world_entity import WorldEntity, PhysicalBody from .failures import ObjectDescriptionNotFound, LinkHasNoGeometry, LinkGeometryHasNoMesh from .local_transformer import LocalTransformer @@ -29,6 +27,12 @@ class EntityDescription(ABC): A description of an entity. This can be a link, joint or object description. """ + def __init__(self, parsed_description: Optional[Any] = None): + """ + :param parsed_description: The parsed description (most likely from a description file) of the entity. + """ + self.parsed_description = parsed_description + @property @abstractmethod def origin(self) -> Pose: @@ -74,7 +78,7 @@ def __init__(self, parsed_joint_description: Optional[Any] = None, is_virtual: b :param parsed_joint_description: The parsed description of the joint (e.g. from urdf or mjcf file). :param is_virtual: True if the joint is virtual (i.e. not a physically existing joint), False otherwise. """ - self.parsed_description = parsed_joint_description + super().__init__(parsed_joint_description) self.is_virtual: Optional[bool] = is_virtual @property @@ -158,14 +162,13 @@ def friction(self) -> float: raise NotImplementedError -class ObjectEntity(WorldEntity): +class ObjectEntity: """ - An abstract base class that represents a physical part/entity of an Object. + An abstract base class that represents a part of an Object. This can be a link or a joint of an Object. """ - def __init__(self, _id: int, obj: Object): - WorldEntity.__init__(self, _id, obj.world) + def __init__(self, obj: Object): self.object: Object = obj @property @@ -175,33 +178,6 @@ def object_name(self) -> str: """ return self.object.name - @property - @abstractmethod - def pose(self) -> Pose: - """ - :return: The pose of this entity relative to the world frame. - """ - pass - - @property - def transform(self) -> Transform: - """ - The transform of this entity. - - :return: The transform of this entity. - """ - return self.pose.to_transform(self.tf_frame) - - @property - @abstractmethod - def tf_frame(self) -> str: - """ - The tf frame of this entity. - - :return: The tf frame of this entity. - """ - pass - @property def object_id(self) -> int: """ @@ -210,18 +186,17 @@ def object_id(self) -> int: return self.object.id -class Link(ObjectEntity, LinkDescription, ABC): +class Link(PhysicalBody, ObjectEntity, LinkDescription, ABC): """ A link of an Object in the World. """ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): - ObjectEntity.__init__(self, _id, obj) + PhysicalBody.__init__(self, _id, obj.world) + ObjectEntity.__init__(self, obj) LinkDescription.__init__(self, link_description.parsed_description, link_description.mesh_dir) self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} - self._current_pose: Optional[Pose] = None - self.update_pose() def get_axis_aligned_bounding_box(self, transform_to_link_pose: bool = True) -> AxisAlignedBoundingBox: """ @@ -303,7 +278,7 @@ def get_mesh_filename(self, geometry: MeshVisualShape) -> str: else: raise LinkGeometryHasNoMesh(self.name, type(geometry).__name__) - def set_pose(self, pose: Pose) -> None: + def set_object_pose_given_link_pose(self, pose: Pose) -> None: """ Set the pose of this link to the given pose. NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints @@ -406,14 +381,6 @@ def is_root(self) -> bool: """ return self.object.get_root_link_id() == self.id - def update_transform(self, transform_time: Optional[Time] = None) -> None: - """ - Update the transformation of this link at the given time. - - :param transform_time: The time at which the transformation should be updated. - """ - self.local_transformer.update_transforms([self.transform], transform_time) - def get_transform_to_link(self, link: 'Link') -> Transform: """ :param link: The link to which the transformation should be returned. @@ -435,61 +402,18 @@ def get_pose_wrt_link(self, link: 'Link') -> Pose: """ return self.local_transformer.transform_pose(self.pose, link.tf_frame) - @property - def position(self) -> Point: - """ - :return: A Point object containing the position of the link relative to the world frame. - """ - return self.pose.position - - @property - def position_as_list(self) -> List[float]: - """ - :return: A list containing the position of the link relative to the world frame. - """ - return self.pose.position_as_list() - - @property - def orientation(self) -> Quaternion: - """ - :return: A Quaternion object containing the orientation of the link relative to the world frame. - """ - return self.pose.orientation - - @property - def orientation_as_list(self) -> List[float]: - """ - :return: A list containing the orientation of the link relative to the world frame. - """ - return self.pose.orientation_as_list() - - def update_pose(self) -> None: + def get_origin_transform(self) -> Transform: """ - Update the current pose of this link from the world. + :return: the transformation between the link frame and the origin frame of this link. """ - self._current_pose = self.world.get_link_pose(self) + return self.origin.to_transform(self.tf_frame) @property def pose(self) -> Pose: """ - :return: A Pose object containing the pose of the link relative to the world frame. + :return: The pose of this link. """ - if self.world.conf.update_poses_from_sim_on_get: - self.update_pose() - return self._current_pose - - @property - def pose_as_list(self) -> List[List[float]]: - """ - :return: A list containing the position and orientation of the link relative to the world frame. - """ - return self.pose.to_list() - - def get_origin_transform(self) -> Transform: - """ - :return: the transformation between the link frame and the origin frame of this link. - """ - return self.origin.to_transform(self.tf_frame) + return self.world.get_link_pose(self) @property def color(self) -> Color: @@ -498,15 +422,6 @@ def color(self) -> Color: """ return self.world.get_link_color(self) - @deprecated("Use color property setter instead") - def set_color(self, color: Color) -> None: - """ - Set the color of this link, could be rgb or rgba. - - :param color: The color as a list of floats, either rgb or rgba. - """ - self.color = color - @color.setter def color(self, color: Color) -> None: """ @@ -516,13 +431,6 @@ def color(self, color: Color) -> None: """ self.world.set_link_color(self, color) - @property - def origin_transform(self) -> Transform: - """ - :return: The transform from world to origin of entity. - """ - return self.origin.to_transform(self.tf_frame) - @property def tf_frame(self) -> str: """ @@ -556,14 +464,18 @@ def tf_frame(self) -> str: """ return self.object.tf_frame - def update_pose(self) -> None: - self._current_pose = self.world.get_object_pose(self.object) + @property + def pose(self) -> Pose: + """ + :return: The pose of the root link, which is the same as the pose of the object. + """ + return self.object.pose def __copy__(self): return RootLink(self.object) -class Joint(ObjectEntity, JointDescription, ABC): +class Joint(WorldEntity, ObjectEntity, JointDescription, ABC): """ Represent a joint of an Object in the World. """ @@ -571,7 +483,8 @@ class Joint(ObjectEntity, JointDescription, ABC): def __init__(self, _id: int, joint_description: JointDescription, obj: Object, is_virtual: Optional[bool] = False): - ObjectEntity.__init__(self, _id, obj) + WorldEntity.__init__(self, _id, obj.world) + ObjectEntity.__init__(self, obj) JointDescription.__init__(self, joint_description.parsed_description, is_virtual) self.acceptable_error = (self.world.conf.revolute_joint_position_tolerance if self.type == JointType.REVOLUTE else self.world.conf.prismatic_joint_position_tolerance) @@ -706,7 +619,7 @@ def __init__(self, path: Optional[str] = None): """ :param path: The path of the file to update the description data from. """ - + super().__init__(None) self._links: Optional[List[LinkDescription]] = None self._joints: Optional[List[JointDescription]] = None self._link_map: Optional[Dict[str, Any]] = None diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index 14449d302..4fadedd5e 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -51,7 +51,7 @@ def set_child_link_pose(self): """ Set the target pose of the child object to the current pose of the child object in the parent object frame. """ - self.child_link.set_pose(self.get_child_link_target_pose()) + self.child_link.set_object_pose_given_link_pose(self.get_child_link_target_pose()) def get_child_link_target_pose(self) -> Pose: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 80cee2c0a..814bbf6bd 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -2,6 +2,7 @@ import logging import os +from functools import cached_property from pathlib import Path import numpy as np @@ -18,7 +19,7 @@ from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform from ..datastructures.world import World -from ..datastructures.world_entity import WorldEntity +from ..datastructures.world_entity import WorldEntity, PhysicalBody from ..description import ObjectDescription, LinkDescription, Joint from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects, UnsupportedFileExtension, \ ObjectDescriptionUndefined @@ -40,7 +41,7 @@ Link = ObjectDescription.Link -class Object(WorldEntity, HasConcept): +class Object(PhysicalBody): """ Represents a spawned Object in the World. """ @@ -55,8 +56,6 @@ class Object(WorldEntity, HasConcept): A dictionary that maps the file extension to the corresponding ObjectDescription type. """ - ontology_concept: Type[PhysicalObject] = PhysicalObject - def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] = None, description: Optional[ObjectDescription] = None, pose: Optional[Pose] = None, @@ -118,8 +117,6 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] self.id = self._spawn_object_and_get_id() - self.tf_frame = (self.tf_prospection_world_prefix if self.world.is_prospection_world else "") + self.name - self._init_joint_name_and_id_map() self._init_link_name_and_id_map() @@ -130,6 +127,49 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] self.world.add_object(self) + @cached_property + def tf_frame(self) -> str: + """ + The tf frame of the object. + """ + return (self.tf_prospection_world_prefix if self.world.is_prospection_world else "") + self.name + + @property + def color(self) -> Union[Color, Dict[str, Color]]: + """ + Return the rgba_color of this object. The return is either: + + 1. A Color object with RGBA values, this is the case if the object only has one link (this + happens for example if the object is spawned from a .obj or .stl file) + 2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object. + Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which + the object is spawned. + + :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and + the rgba_color as value. + """ + link_to_color_dict = self.links_colors + + if len(link_to_color_dict) == 1: + return list(link_to_color_dict.values())[0] + else: + return link_to_color_dict + + @color.setter + def color(self, rgba_color: Color) -> None: + """ + Change the color of this object. + + :param rgba_color: The color as Color object with RGBA values between 0 and 1 + """ + # Check if there is only one link, this is the case for primitive + # forms or if loaded from an .stl or .obj file + if self.links != {}: + for link in self.links.values(): + link.color = rgba_color + else: + self.root_link.color = rgba_color + def get_mesh_path(self) -> str: """ Get the path to the mesh file of the object. @@ -745,15 +785,14 @@ def get_orientation_as_list(self) -> List[float]: """ return self.get_pose().orientation_as_list() + @deprecated("Use property 'pose' instead.") def get_pose(self) -> Pose: """ Return the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. :return: The current pose of this object """ - if self.world.conf.update_poses_from_sim_on_get: - self.update_pose() - return self._current_pose + return self.pose def set_pose(self, pose: Pose, base: bool = False, set_attachments: bool = True) -> None: """ @@ -772,25 +811,8 @@ def set_pose(self, pose: Pose, base: bool = False, set_attachments: bool = True) if set_attachments: self._set_attached_objects_poses() - def reset_base_pose(self, pose: Pose): - if self.world.reset_object_base_pose(self, pose): - self.update_pose() - - def update_pose(self): - """ - Update the current pose of this object from the world, and updates the poses of all links. - """ - self._current_pose = self.world.get_object_pose(self) - # TODO: Probably not needed, need to test - self._update_all_links_poses() - self.update_link_transforms() - - def _update_all_links_poses(self): - """ - Update the poses of all links by getting them from the simulator. - """ - for link in self.links.values(): - link.update_pose() + def reset_base_pose(self, pose: Pose) -> bool: + return self.world.reset_object_base_pose(self, pose) def move_base_to_origin_pose(self) -> None: """ @@ -1152,7 +1174,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ self.clip_joint_positions_to_limits({joint_name: joint_position}) if self.world.reset_joint_position(self.joints[joint_name], joint_position): - self._update_on_joint_position_change() + self._set_attached_objects_poses() @deprecated("Use set_multiple_joint_positions instead") def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: @@ -1168,7 +1190,7 @@ def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> Non joint_positions = {self.joints[joint_name]: joint_position for joint_name, joint_position in joint_positions.items()} if self.world.set_multiple_joint_positions(joint_positions): - self._update_on_joint_position_change() + self._set_attached_objects_poses() def clip_joint_positions_to_limits(self, joint_positions: Dict[str, float]) -> Dict[str, float]: """ @@ -1182,12 +1204,6 @@ def clip_joint_positions_to_limits(self, joint_positions: Dict[str, float]) -> D if self.joints[joint_name].has_limits else joint_position for joint_name, joint_position in joint_positions.items()} - def _update_on_joint_position_change(self): - self.update_pose() - self._update_all_links_poses() - self.update_link_transforms() - self._set_attached_objects_poses() - def get_joint_position(self, joint_name: str) -> float: """ Return the current position of the given joint. @@ -1362,40 +1378,13 @@ def closest_points_with_obj(self, other_object: Object, max_distance: float) -> """ return self.world.get_closest_points_between_objects(self, other_object, max_distance) + @deprecated("Use property setter 'color' instead.") def set_color(self, rgba_color: Color) -> None: - """ - Change the color of this object, the color has to be given as a list - of RGBA values. - - :param rgba_color: The color as Color object with RGBA values between 0 and 1 - """ - # Check if there is only one link, this is the case for primitive - # forms or if loaded from an .stl or .obj file - if self.links != {}: - for link in self.links.values(): - link.color = rgba_color - else: - self.root_link.color = rgba_color + self.color = rgba_color + @deprecated("Use property 'color' instead.") def get_color(self) -> Union[Color, Dict[str, Color]]: - """ - Return the rgba_color of this object. The return is either: - - 1. A Color object with RGBA values, this is the case if the object only has one link (this - happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object. - Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which - the object is spawned. - - :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and - the rgba_color as value. - """ - link_to_color_dict = self.links_colors - - if len(link_to_color_dict) == 1: - return list(link_to_color_dict.values())[0] - else: - return link_to_color_dict + return self.color @property def links_colors(self) -> Dict[str, Color]: diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index b87e7a073..53affb77e 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -53,8 +53,8 @@ def contact( prospection_obj1 = World.current_world.get_prospection_object_for_object(object1) prospection_obj2 = World.current_world.get_prospection_object_for_object(object2) World.current_world.perform_collision_detection() - con_points: ContactPointsList = World.current_world.get_contact_points_between_two_objects(prospection_obj1, - prospection_obj2) + con_points: ContactPointsList = World.current_world.get_contact_points_between_two_bodies(prospection_obj1, + prospection_obj2) objects_are_in_contact = len(con_points) > 0 if return_links: contact_links = [(point.link_a, point.link_b) for point in con_points] diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index dee26a369..afa4275ca 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -8,7 +8,7 @@ import numpy as np import pycram_bullet as p from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict, Any, Callable +from typing_extensions import List, Optional, Dict, Any, Callable, Tuple from pycrap import Floor from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ @@ -16,6 +16,7 @@ from ..datastructures.enums import ObjectType, WorldMode, JointType from ..datastructures.pose import Pose from ..datastructures.world import World +from ..datastructures.world_entity import PhysicalBody from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..object_descriptors.urdf import ObjectDescription from ..ros.logging import logwarn, loginfo @@ -191,7 +192,7 @@ def get_object_number_of_links(self, obj: Object) -> int: def perform_collision_detection(self) -> None: p.performCollisionDetection(physicsClientId=self.id) - def get_object_contact_points(self, obj: Object) -> ContactPointsList: + def get_body_contact_points(self, obj: Object) -> ContactPointsList: """ Get the contact points of the object with akk other objects in the world. The contact points are returned as a ContactPointsList object. @@ -204,14 +205,28 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) - def get_contact_points_between_two_objects(self, obj_a: Object, obj_b: Object) -> ContactPointsList: + def get_contact_points_between_two_bodies(self, obj_a: Object, obj_b: Object) -> ContactPointsList: self.perform_collision_detection() points_list = p.getContactPoints(obj_a.id, obj_b.id, physicsClientId=self.id) return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) - def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, distance: float) -> ClosestPointsList: - points_list = p.getClosestPoints(obj_a.id, obj_b.id, distance, physicsClientId=self.id) + def get_distance_between_two_bodies(self, body_1: PhysicalBody, body_2: PhysicalBody, + max_distance: float) -> Optional[float]: + body_1_id, link_1_id = self.get_body_and_link_id(body_1) + body_2_id, link_2_id = self.get_body_and_link_id(body_2) + points = p.getClosestPoints(body_1_id, body_2_id, max_distance, link_1_id, link_2_id, + physicsClientId=self.id) + closest_point = min(points, key=lambda x: x[8]) if points else None + return closest_point[8] if closest_point else None + + + @staticmethod + def get_body_and_link_id(body: PhysicalBody) -> Tuple[int, Optional[int]]: + return (body.id, None) if isinstance(body, Object) else (body.object_id, body.id) + + def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, max_distance: float) -> ClosestPointsList: + points_list = p.getClosestPoints(obj_a.id, obj_b.id, max_distance, physicsClientId=self.id) return ClosestPointsList([ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 5e17c4899..fc8be0090 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -607,7 +607,7 @@ def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: L contact_force_array = obj_rot_matrix @ np.array(contact_force).reshape(3, 1) return contact_force_array.flatten().tolist()[2] - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: + def get_contact_points_between_two_bodies(self, obj1: Object, obj2: Object) -> ContactPointsList: obj1_contact_points = self.get_object_contact_points(obj1) return obj1_contact_points.get_points_of_object(obj2) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 8bb3cf711..ebc8198aa 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -211,7 +211,6 @@ def test_set_position(self): def test_update_position(self): milk = self.spawn_milk([1, 1, 0.1]) - milk.update_pose() milk_position = milk.get_position_as_list() self.assert_list_is_equal(milk_position[:2], [1, 1], delta=self.multiverse.conf.position_tolerance) @@ -388,7 +387,7 @@ def test_get_robot_contact_points(self): quaternion_from_euler(0, 0, 2.26).tolist(), robot_name="pr2") apartment = self.spawn_apartment() - contact_points = self.multiverse.get_contact_points_between_two_objects(robot, apartment) + contact_points = self.multiverse.get_contact_points_between_two_bodies(robot, apartment) self.assertTrue(len(contact_points) > 0) def test_get_contact_points_between_two_objects(self): @@ -398,7 +397,7 @@ def test_get_contact_points_between_two_objects(self): # This is needed because the cup is spawned in the air so it needs to fall # to get in contact with the milk self.multiverse.simulate(0.4) - contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) + contact_points = self.multiverse.get_contact_points_between_two_bodies(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertTrue(len(contact_points) >= 1) self.assertIsInstance(contact_points[0], ContactPoint) From 26a74b7e1a64fa14841ec83de98b6926afb64773 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 22 Nov 2024 17:15:59 +0100 Subject: [PATCH 18/49] [PhysicalBody] (WIP) most tests are passing, there's a problem with transport action. --- src/pycram/datastructures/dataclasses.py | 35 ++++- src/pycram/datastructures/world.py | 160 ++++------------------ src/pycram/datastructures/world_entity.py | 53 +++++-- src/pycram/description.py | 26 +++- src/pycram/world_concepts/world_object.py | 40 +++--- src/pycram/worlds/bullet_world.py | 59 ++++---- test/test_bullet_world.py | 4 +- test/test_multiverse.py | 2 +- 8 files changed, 171 insertions(+), 208 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 2656cdad3..46e35568b 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -513,10 +513,10 @@ def velocity_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: :param other: The state of the other object. :return: True if the velocities are almost equal, False otherwise. """ - return ((self.velocity is None and other.velocity is None) or - (self.vector_is_almost_equal(self.velocity, other.velocity, self.acceptable_velocity_error[0]) and - self.is_translating == other.is_translating and self.is_rotating == other.is_rotating) - ) + if self.velocity is None or other.velocity is None: + return self.velocity == other.velocity + return (self.vector_is_almost_equal(self.velocity[:3], other.velocity[:3], self.acceptable_velocity_error[0]) + and self.vector_is_almost_equal(self.velocity[3:], other.velocity[3:], self.acceptable_velocity_error[1])) @staticmethod def vector_is_almost_equal(vector1: List[float], vector2: List[float], acceptable_error: float) -> bool: @@ -530,16 +530,26 @@ def vector_is_almost_equal(vector1: List[float], vector2: List[float], acceptabl """ return np.all(np.array(vector1) - np.array(vector2) <= acceptable_error) + def __copy__(self): + return PhysicalBodyState(pose=self.pose.copy(), + is_translating=self.is_translating, is_rotating=self.is_rotating, + velocity=self.velocity.copy(), + acceptable_pose_error=deepcopy(self.acceptable_pose_error), + acceptable_velocity_error=deepcopy(self.acceptable_velocity_error)) + @dataclass class LinkState(State): """ Dataclass for storing the state of a link. """ + body_state: PhysicalBodyState constraint_ids: Dict[Link, int] def __eq__(self, other: 'LinkState'): - return self.all_constraints_exist(other) and self.all_constraints_are_equal(other) + return (self.body_state == other.body_state + and self.all_constraints_exist(other) + and self.all_constraints_are_equal(other)) def all_constraints_exist(self, other: 'LinkState') -> bool: """ @@ -562,7 +572,7 @@ def all_constraints_are_equal(self, other: 'LinkState') -> bool: other.constraint_ids.values())]) def __copy__(self): - return LinkState(constraint_ids=copy(self.constraint_ids)) + return LinkState(copy(self.body_state), constraint_ids=copy(self.constraint_ids)) @dataclass @@ -597,6 +607,10 @@ def __eq__(self, other: 'ObjectState'): and self.link_states == other.link_states and self.joint_states == other.joint_states) + @property + def pose(self) -> Pose: + return self.body_state.pose + def all_attachments_exist(self, other: 'ObjectState') -> bool: """ Check if all attachments exist in the other object state. @@ -778,6 +792,15 @@ def get_points_of_object(self, obj: Object) -> 'ContactPointsList': """ return ContactPointsList([point for point in self if point.link_b.object == obj]) + def get_points_of_link(self, link: Link) -> 'ContactPointsList': + """ + Get the points of the link. + + :param link: An instance of the Link class that represents the link that the points are related to. + :return: A ContactPointsList instance that represents the contact points of the link. + """ + return ContactPointsList([point for point in self if point.link_b == link]) + def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]: """ Return the object that is not in the current points list but was in the initial points list. diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index eecf8235f..cc7be951d 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -30,9 +30,7 @@ from ..robot_description import RobotDescription from ..ros.data_types import Time from ..ros.logging import logwarn -from ..validation.goal_validator import (MultiPoseGoalValidator, - PoseGoalValidator, JointPositionGoalValidator, - MultiJointPositionGoalValidator, GoalValidator, +from ..validation.goal_validator import (GoalValidator, validate_joint_position, validate_multiple_joint_positions, validate_object_pose, validate_multiple_object_poses) from ..world_concepts.constraints import Constraint @@ -131,86 +129,14 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self._current_state: Optional[WorldState] = None - self._init_goal_validators() - self.original_state_id = self.save_state() self.on_add_object_callbacks: List[Callable[[Object], None]] = [] - def get_link_contact_points(self, link: Link) -> ContactPointsList: - """ - Return the contact points of a link with all other objects in the world. - - :param link: The link. - :return: A list of contact points. - """ - raise NotImplementedError - - def get_link_contact_points_with_body(self, link: Link, body: PhysicalBody) -> ContactPointsList: - """ - Return the contact points of a link with a specific body in the world. - - :param link: The link. - :param body: The body. - :return: A list of contact points. - """ - raise NotImplementedError - - def get_link_distance_with_body(self, link: Link, body: PhysicalBody) -> float: + def get_body_convex_hull(self, body: PhysicalBody) -> Geometry3D: """ - Return the distance of a link with a specific body in the world. - - :param link: The link. - :param body: The body. - :return: The distance between the link and the body. - """ - raise NotImplementedError - - def get_link_closest_points_with_body(self, link: Link, body: PhysicalBody, max_distance: float) -> ClosestPointsList: - """ - Return the closest points of a link with a specific body in the world. - - :param link: The link. - :param body: The body. - :param max_distance: The maximum distance between the points. - :return: A list of closest points. - """ - raise NotImplementedError - - def get_link_distances(self, link: Link) -> Dict[PhysicalBody, float]: - """ - Return the distances of a link with all other objects in the world. - - :param link: The link. - :return: A dictionary of object names and distances. - """ - raise NotImplementedError - - def get_link_closest_points(self, link: Link, max_distance: float) -> ClosestPointsList: - """ - Return the closest points of a link with all other objects in the world. - - :param link: The link. - :param max_distance: The maximum distance between the points. - :return: A list of closest points. - """ - raise NotImplementedError - - def get_object_convex_hull(self, obj: Object) -> Geometry3D: - """ - Get the convex hull of an object. - - :param obj: The pycram object. - :return: The convex hull of the object as a list of Points. - """ - raise NotImplementedError - - def get_link_convex_hull(self, link: Link) -> Geometry3D: - """ - Get the convex hull of a link of an articulated object. - - :param link: The link as a AbstractLink object. - :return: The convex hull of the link as a list of Points. + :param body: The body object. + :return: The convex hull of the body as a Geometry3D object. """ raise NotImplementedError @@ -291,30 +217,6 @@ def robot_joint_actuators(self) -> Dict[str, str]: """ return self.robot_description.joint_actuators - def _init_goal_validators(self): - """ - Initialize the goal validators for the World objects' poses, positions, and orientations. - """ - - # Objects Pose goal validators - self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.conf.get_pose_tolerance(), - self.conf.acceptable_percentage_of_goal) - self.multi_pose_goal_validator = MultiPoseGoalValidator( - lambda x: list(self.get_multiple_object_poses(x).values()), - self.conf.get_pose_tolerance(), self.conf.acceptable_percentage_of_goal) - - # Joint Goal validators - self.joint_position_goal_validator = JointPositionGoalValidator( - self.get_joint_position, - acceptable_revolute_joint_position_error=self.conf.revolute_joint_position_tolerance, - acceptable_prismatic_joint_position_error=self.conf.prismatic_joint_position_tolerance, - acceptable_percentage_of_goal_achieved=self.conf.acceptable_percentage_of_goal) - self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( - lambda x: list(self.get_multiple_joint_positions(x).values()), - acceptable_revolute_joint_position_error=self.conf.revolute_joint_position_tolerance, - acceptable_prismatic_joint_position_error=self.conf.prismatic_joint_position_tolerance, - acceptable_percentage_of_goal_achieved=self.conf.acceptable_percentage_of_goal) - def check_object_exists(self, obj: Object) -> bool: """ Check if the object exists in the simulator. @@ -791,10 +693,7 @@ def perform_collision_detection(self) -> None: @deprecated("Use get_body_contact_points instead") def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ - Return a list of contact points of this Object with all other Objects. - - :param obj: The object. - :return: A list of all contact points with other objects + Same as :meth:`get_body_contact_points` but with objects instead of any type of bodies. """ return self.get_body_contact_points(obj) @@ -810,7 +709,7 @@ def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: @deprecated("Use get_contact_points_between_two_bodies instead") def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: """ - Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of bodies. + Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of any type of bodies. """ return self.get_contact_points_between_two_bodies(obj1, obj2) @@ -825,47 +724,25 @@ def get_contact_points_between_two_bodies(self, body_1: PhysicalBody, body_2: Ph """ pass - @abstractmethod - def get_body_distances(self, body: PhysicalBody) -> Dict[PhysicalBody, float]: + def get_body_closest_points(self, body: PhysicalBody, max_distance: float) -> ClosestPointsList: """ - Return the distances of this body with all other bodies in the world. + Return the closest points of this body with all other bodies in the world. :param body: The body. - :return: A dictionary of body names and distances. - """ - pass - - @abstractmethod - def get_distance_between_two_bodies(self, body_1: PhysicalBody, body_2: PhysicalBody) -> float: - """ - Return the distance between two bodies. - - :param body_1: The first body. - :param body_2: The second body. - :return: The distance between the two bodies. - """ - pass - - def get_object_closest_points(self, obj: Object, max_distance: float) -> ClosestPointsList: - """ - Return the closest points of this object with all other objects in the world. - - :param obj: The object. - :param max_distance: The maximum distance between the points. + :param max_distance: The maximum allowed distance between the points. :return: A list of the closest points. """ - all_obj_closest_points = [self.get_closest_points_between_objects(obj, other_obj, max_distance) for other_obj in - self.objects - if other_obj != obj] + all_obj_closest_points = [self.get_closest_points_between_two_bodies(body, other_body, max_distance) + for other_body in self.objects if other_body != body] return ClosestPointsList([point for closest_points in all_obj_closest_points for point in closest_points]) - def get_closest_points_between_objects(self, object_a: Object, object_b: Object, max_distance: float) \ + def get_closest_points_between_two_bodies(self, body_a: PhysicalBody, body_b: PhysicalBody, max_distance: float) \ -> ClosestPointsList: """ Return the closest points between two objects. - :param object_a: The first object. - :param object_b: The second object. + :param body_a: The first body. + :param body_b: The second body. :param max_distance: The maximum distance between the points. :return: A list of the closest points. """ @@ -1772,6 +1649,15 @@ def original_state(self) -> WorldState: def __del__(self): self.exit() + def __eq__(self, other: World): + if not isinstance(other, self.__class__): + return False + return (self.is_prospection_world == other.is_prospection_world + and self.id == other.id) + + def __hash__(self): + return hash((self.__class__.__name__, self.is_prospection_world, self.id)) + class UseProspectionWorld: """ diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index a46eb0ebf..5ba1a7526 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -3,11 +3,12 @@ import os import pickle from abc import ABC, abstractmethod +from copy import copy from trimesh.parent import Geometry3D -from typing_extensions import TYPE_CHECKING, Dict, Optional, List, Any, deprecated, Union +from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union -from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, VisualShape, PhysicalBodyState, \ +from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, PhysicalBodyState, \ AxisAlignedBoundingBox, RotatedBoundingBox from ..local_transformer import LocalTransformer from ..ros.data_types import Time @@ -166,8 +167,17 @@ def get_convex_hull(self) -> Geometry3D: ... @property - def current_state(self) -> PhysicalBodyState: - return PhysicalBodyState(self.pose, self.is_translating, self.is_rotating, self.velocity) + def body_state(self) -> PhysicalBodyState: + return PhysicalBodyState(self.pose.copy(), self.is_translating, self.is_rotating, copy(self.velocity) + , self.world.conf.get_pose_tolerance()) + + @body_state.setter + def body_state(self, state: PhysicalBodyState) -> None: + if self.body_state != state: + self.pose = state.pose + self.is_translating = state.is_translating + self.is_rotating = state.is_rotating + self.velocity = state.velocity @property def velocity(self) -> Optional[List[float]]: @@ -253,6 +263,16 @@ def pose(self) -> Pose: """ ... + @pose.setter + @abstractmethod + def pose(self, pose: Pose) -> None: + """ + Set the pose of this body. + + :param pose: The pose of this body. + """ + ... + @property @abstractmethod def tf_frame(self) -> str: @@ -277,19 +297,22 @@ def get_contact_points_with_body(self, body: 'PhysicalBody') -> ContactPointsLis """ return self.world.get_contact_points_between_two_bodies(self, body) - @property - def distances(self) -> Dict['PhysicalBody', float]: + def closest_points(self, max_distance: float) -> ClosestPointsList: """ - :return: The closest distances of this body to other physical bodies. + :param max_distance: The maximum distance to consider a body as close, only points closer than or equal to this + distance will be returned. + :return: The closest points of this body with other physical bodies within the given maximum distance. """ - return self.world.get_body_distances(self) + return self.world.get_body_closest_points(self, max_distance) - def get_distance_with_body(self, body: 'PhysicalBody') -> float: + def get_closest_points_with_body(self, body: 'PhysicalBody', max_distance: float) -> ClosestPointsList: """ - :param body: The body to get the distance with. - :return: The closest distance of this body to the given body. + :param body: The body to get the points with. + :param max_distance: The maximum distance to consider a body as close, only points closer than or equal to this + distance will be returned. + :return: The closest points of this body with the given body within the given maximum distance. """ - return self.world.get_distance_between_two_bodies(self, body) + return self.world.get_closest_points_between_two_bodies(self, body, max_distance) @property def is_moving(self) -> Optional[bool]: @@ -335,3 +358,9 @@ def color(self, color: Color) -> None: """ ... + @abstractmethod + def __eq__(self, other: PhysicalBody) -> bool: + """ + Check if this body is equal to another body. + """ + ... diff --git a/src/pycram/description.py b/src/pycram/description.py index 817bd585c..001c34cd2 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -11,12 +11,13 @@ from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, Sequence from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \ - MeshVisualShape, RotatedBoundingBox, ContactPointsList + MeshVisualShape, RotatedBoundingBox from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform from .datastructures.world_entity import WorldEntity, PhysicalBody from .failures import ObjectDescriptionNotFound, LinkHasNoGeometry, LinkGeometryHasNoMesh from .local_transformer import LocalTransformer +from .ros.logging import logwarn if TYPE_CHECKING: from .world_concepts.world_object import Object @@ -242,7 +243,7 @@ def get_convex_hull(self) -> Geometry3D: :return: The convex hull of the link geometry. """ try: - return self.world.get_link_convex_hull(self) + return self.world.get_body_convex_hull(self) except NotImplementedError: if isinstance(self.geometry, MeshVisualShape): mesh_path = self.get_mesh_path(self.geometry) @@ -320,7 +321,7 @@ def get_transform_to_root_link(self) -> Transform: @property def current_state(self) -> LinkState: - return LinkState(self.constraint_ids.copy()) + return LinkState(self.body_state, self.constraint_ids.copy()) @current_state.setter def current_state(self, link_state: LinkState) -> None: @@ -328,6 +329,7 @@ def current_state(self, link_state: LinkState) -> None: if not self.all_constraint_links_belong_to_same_world(link_state): raise ValueError("All constraint links must belong to the same world, since the constraint ids" "are unique to the world and cannot be transferred between worlds.") + self.body_state = link_state.body_state self.constraint_ids = link_state.constraint_ids def all_constraint_links_belong_to_same_world(self, other: LinkState) -> bool: @@ -415,6 +417,11 @@ def pose(self) -> Pose: """ return self.world.get_link_pose(self) + @pose.setter + def pose(self, pose: Pose) -> None: + logwarn("Setting the pose of a link is not allowed," + " change object pose and/or joint position to affect the link pose.") + @property def color(self) -> Color: """ @@ -438,7 +445,16 @@ def tf_frame(self) -> str: """ return f"{self.object.tf_frame}/{self.name}" - def __eq__(self, other): + @property + def origin_transform(self) -> Transform: + """ + The transformation between the link frame and the origin frame of this link. + """ + return self.origin.to_transform(self.tf_frame) + + def __eq__(self, other: Link): + if not isinstance(other, Link): + return False return self.id == other.id and self.object == other.object and self.name == other.name def __copy__(self): @@ -455,7 +471,7 @@ class RootLink(Link, ABC): """ def __init__(self, obj: Object): - super().__init__(obj.get_root_link_id(), obj.get_root_link_description(), obj) + Link.__init__(self, obj.get_root_link_id(), obj.get_root_link_description(), obj) @property def tf_frame(self) -> str: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 814bbf6bd..1a94c5f9e 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -19,7 +19,7 @@ from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform from ..datastructures.world import World -from ..datastructures.world_entity import WorldEntity, PhysicalBody +from ..datastructures.world_entity import PhysicalBody from ..description import ObjectDescription, LinkDescription, Joint from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects, UnsupportedFileExtension, \ ObjectDescriptionUndefined @@ -121,6 +121,8 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] self._init_link_name_and_id_map() self._init_links_and_update_transforms() + self.color: Color = color + self._init_joints() self.attachments: Dict[Object, Attachment] = {} @@ -156,7 +158,7 @@ def color(self) -> Union[Color, Dict[str, Color]]: return link_to_color_dict @color.setter - def color(self, rgba_color: Color) -> None: + def color(self, rgba_color: Union[Color, Dict[str, Color]]) -> None: """ Change the color of this object. @@ -164,11 +166,15 @@ def color(self, rgba_color: Color) -> None: """ # Check if there is only one link, this is the case for primitive # forms or if loaded from an .stl or .obj file - if self.links != {}: - for link in self.links.values(): - link.color = rgba_color - else: + if self.has_one_link: self.root_link.color = rgba_color + else: + if isinstance(rgba_color, Color): + for link in self.links.values(): + link.color = rgba_color + else: + for link_name, color in rgba_color.items(): + self.links[link_name].color = color def get_mesh_path(self) -> str: """ @@ -335,7 +341,7 @@ def pose(self): """ The current pose of the object. """ - return self.get_pose() + return self.world.get_object_pose(self) @pose.setter def pose(self, pose: Pose): @@ -821,15 +827,16 @@ def move_base_to_origin_pose(self) -> None: """ self.set_pose(self.get_pose(), base=True) - def save_state(self, state_id) -> None: + def save_state(self, state_id: int, save_dir: Optional[str] = None) -> None: """ Save the state of this object by saving the state of all links and attachments. :param state_id: The unique id of the state. + :param save_dir: The directory in which to save the state. """ self.save_links_states(state_id) self.save_joints_states(state_id) - super().save_state(state_id) + super().save_state(state_id, save_dir) def save_links_states(self, state_id: int) -> None: """ @@ -854,8 +861,8 @@ def current_state(self) -> ObjectState: """ The current state of this object as an ObjectState. """ - return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), - self.joint_states.copy(), self.world.conf.get_pose_tolerance()) + return ObjectState(self.body_state, self.attachments.copy(), self.link_states.copy(), + self.joint_states.copy()) @current_state.setter def current_state(self, state: ObjectState) -> None: @@ -863,7 +870,7 @@ def current_state(self, state: ObjectState) -> None: Set the current state of this object to the given state. """ if self.current_state != state: - self.set_pose(state.pose, base=False, set_attachments=False) + self.body_state = state.body_state self.set_attachments(state.attachments) self.link_states = state.link_states self.joint_states = state.joint_states @@ -1339,6 +1346,7 @@ def update_link_transforms(self, transform_time: Optional[Time] = None) -> None: for link in self.links.values(): link.update_transform(transform_time) + @property def contact_points(self) -> ContactPointsList: """ Return a list of contact points of this Object with other Objects. @@ -1355,7 +1363,7 @@ def contact_points_simulated(self) -> ContactPointsList: """ state_id = self.world.save_state() self.world.step() - contact_points = self.contact_points() + contact_points = self.contact_points self.world.restore_state(state_id) return contact_points @@ -1366,7 +1374,7 @@ def closest_points(self, max_distance: float) -> ClosestPointsList: :param max_distance: The maximum distance between the closest points :return: A list of closest points between this Object and other Objects """ - return self.world.get_object_closest_points(self, max_distance) + return self.world.get_body_closest_points(self, max_distance) def closest_points_with_obj(self, other_object: Object, max_distance: float) -> ClosestPointsList: """ @@ -1376,7 +1384,7 @@ def closest_points_with_obj(self, other_object: Object, max_distance: float) -> :param max_distance: The maximum distance between the closest points :return: A list of closest points between this Object and the other Object """ - return self.world.get_closest_points_between_objects(self, other_object, max_distance) + return self.world.get_closest_points_between_two_bodies(self, other_object, max_distance) @deprecated("Use property setter 'color' instead.") def set_color(self, rgba_color: Color) -> None: @@ -1425,7 +1433,7 @@ def get_convex_hull(self) -> Geometry3D: if self.has_one_link: return self.root_link.get_convex_hull() else: - return self.world.get_object_convex_hull(self) + return self.world.get_body_convex_hull(self) def get_base_origin(self) -> Pose: """ diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index afa4275ca..5f6fbcbb1 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -8,7 +8,7 @@ import numpy as np import pycram_bullet as p from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict, Any, Callable, Tuple +from typing_extensions import List, Optional, Dict, Any, Callable from pycrap import Floor from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ @@ -192,44 +192,45 @@ def get_object_number_of_links(self, obj: Object) -> int: def perform_collision_detection(self) -> None: p.performCollisionDetection(physicsClientId=self.id) - def get_body_contact_points(self, obj: Object) -> ContactPointsList: - """ - Get the contact points of the object with akk other objects in the world. The contact points are returned as a - ContactPointsList object. - - :param obj: The object for which the contact points should be returned. - :return: The contact points of the object with all other objects in the world. - """ + def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: self.perform_collision_detection() - points_list = p.getContactPoints(obj.id, physicsClientId=self.id) + body_data = self.get_body_and_link_id(body, index='A') + kwargs = {**body_data, "physicsClientId": self.id} + points_list = p.getContactPoints(**kwargs) return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) - def get_contact_points_between_two_bodies(self, obj_a: Object, obj_b: Object) -> ContactPointsList: + def get_contact_points_between_two_bodies(self, obj_a: PhysicalBody, obj_b: PhysicalBody) -> ContactPointsList: self.perform_collision_detection() - points_list = p.getContactPoints(obj_a.id, obj_b.id, physicsClientId=self.id) + body_a_data = self.get_body_and_link_id(obj_a, index='A') + body_b_data = self.get_body_and_link_id(obj_b, index='B') + kwargs = {**body_a_data, **body_b_data, "physicsClientId": self.id} + points_list = p.getContactPoints(**kwargs) return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) - def get_distance_between_two_bodies(self, body_1: PhysicalBody, body_2: PhysicalBody, - max_distance: float) -> Optional[float]: - body_1_id, link_1_id = self.get_body_and_link_id(body_1) - body_2_id, link_2_id = self.get_body_and_link_id(body_2) - points = p.getClosestPoints(body_1_id, body_2_id, max_distance, link_1_id, link_2_id, - physicsClientId=self.id) - closest_point = min(points, key=lambda x: x[8]) if points else None - return closest_point[8] if closest_point else None - - - @staticmethod - def get_body_and_link_id(body: PhysicalBody) -> Tuple[int, Optional[int]]: - return (body.id, None) if isinstance(body, Object) else (body.object_id, body.id) - - def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, max_distance: float) -> ClosestPointsList: - points_list = p.getClosestPoints(obj_a.id, obj_b.id, max_distance, physicsClientId=self.id) + def get_body_closest_points(self, body: PhysicalBody, max_distance: float) -> ClosestPointsList: + all_obj_closest_points = [self.get_closest_points_between_two_bodies(body, other_body, max_distance) + for other_body in self.objects if other_body != body] + return ClosestPointsList([point for closest_points in all_obj_closest_points for point in closest_points]) + + def get_closest_points_between_two_bodies(self, obj_a: PhysicalBody, obj_b: PhysicalBody, + max_distance: float) -> ClosestPointsList: + body_a_data = self.get_body_and_link_id(obj_a, index='A') + body_b_data = self.get_body_and_link_id(obj_b, index='B') + kwargs = {**body_a_data, **body_b_data, "physicsClientId": self.id} + points_list = p.getClosestPoints(**kwargs) return ClosestPointsList([ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) + @staticmethod + def get_body_and_link_id(body: PhysicalBody, index='') -> Dict[str, int]: + body_id, link_id = (body.object_id, body.id) if isinstance(body, Link) else (body.id, None) + values_dict = {f"body{index}": body_id} + if link_id is not None: + values_dict.update({f"linkIndex{index}": link_id}) + return values_dict + def parse_points_list_to_args(self, point: List) -> Dict: """ Parses the list of points to a list of dictionaries with the keys as the names of the arguments of the @@ -487,7 +488,7 @@ def __init__(self, world: World, mode: WorldMode): self.mode: WorldMode = mode # Checks if there is a display connected to the system. If not, the simulation will be run in direct mode. - if not "DISPLAY" in os.environ: + if "DISPLAY" not in os.environ: loginfo("No display detected. Running the simulation in direct mode.") self.mode = WorldMode.DIRECT diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index f749a7b01..97d6089fa 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -68,9 +68,9 @@ def test_get_joint_position(self): self.assertAlmostEqual(self.robot.get_joint_position("head_pan_joint"), 0.0, delta=0.01) def test_get_object_contact_points(self): - self.assertEqual(len(self.robot.contact_points()), 0) + self.assertEqual(len(self.robot.contact_points), 0) self.milk.set_position(self.robot.get_position()) - self.assertTrue(len(self.robot.contact_points()) > 0) + self.assertTrue(len(self.robot.contact_points) > 0) def test_enable_joint_force_torque_sensor(self): self.world.enable_joint_force_torque_sensor(self.robot, self.robot.get_joint_id("head_pan_joint")) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ebc8198aa..b083bbb26 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -124,7 +124,7 @@ def test_spawn_mesh_object(self): milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1, 1, 0.1])) self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1])) self.multiverse.simulate(0.2) - contact_points = milk.contact_points() + contact_points = milk.contact_points self.assertTrue(len(contact_points) > 0) def test_parse_mjcf_actuators(self): From 4a3a731511b1e870126febb3c015ecb776706c97 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 22 Nov 2024 18:18:54 +0100 Subject: [PATCH 19/49] [PhysicalBody] (WIP) fixed issue with high position tolerance. --- config/world_conf.py | 2 +- src/pycram/datastructures/world.py | 4 ++-- src/pycram/worlds/bullet_world.py | 9 +++------ test/test_bullet_world.py | 2 +- 4 files changed, 7 insertions(+), 10 deletions(-) diff --git a/config/world_conf.py b/config/world_conf.py index 2c0eacf21..75f335d07 100644 --- a/config/world_conf.py +++ b/config/world_conf.py @@ -70,7 +70,7 @@ class WorldConfig: and the world synchronization. """ - position_tolerance: float = 1e-2 + position_tolerance: float = 1e-3 orientation_tolerance: float = 10 * math.pi / 180 prismatic_joint_position_tolerance: float = 1e-2 revolute_joint_position_tolerance: float = 5 * math.pi / 180 diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index cc7be951d..a1cb210b3 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -431,7 +431,7 @@ def remove_object_from_original_state(self, obj: Object) -> None: :param obj: The object to be removed. """ self.original_state.object_states.pop(obj.name) - self.original_state.simulator_state_id = self.save_physics_simulator_state(use_same_id=True) + self.update_simulator_state_id_in_original_state(use_same_id=True) def add_object_to_original_state(self, obj: Object) -> None: """ @@ -1264,7 +1264,7 @@ def reset_world(self, remove_saved_states=False) -> None: self.restore_state(self.original_state_id) if remove_saved_states: self.remove_saved_states() - self.original_state_id = self.save_state(use_same_id=True) + self.original_state_id = self.save_state(use_same_id=True) def remove_saved_states(self) -> None: """ diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 5f6fbcbb1..376f7ef4f 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -195,8 +195,7 @@ def perform_collision_detection(self) -> None: def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: self.perform_collision_detection() body_data = self.get_body_and_link_id(body, index='A') - kwargs = {**body_data, "physicsClientId": self.id} - points_list = p.getContactPoints(**kwargs) + points_list = p.getContactPoints(**body_data, physicsClientId=self.id) return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) @@ -204,8 +203,7 @@ def get_contact_points_between_two_bodies(self, obj_a: PhysicalBody, obj_b: Phys self.perform_collision_detection() body_a_data = self.get_body_and_link_id(obj_a, index='A') body_b_data = self.get_body_and_link_id(obj_b, index='B') - kwargs = {**body_a_data, **body_b_data, "physicsClientId": self.id} - points_list = p.getContactPoints(**kwargs) + points_list = p.getContactPoints(**body_a_data, **body_b_data, physicsClientId=self.id) return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) @@ -218,8 +216,7 @@ def get_closest_points_between_two_bodies(self, obj_a: PhysicalBody, obj_b: Phys max_distance: float) -> ClosestPointsList: body_a_data = self.get_body_and_link_id(obj_a, index='A') body_b_data = self.get_body_and_link_id(obj_b, index='B') - kwargs = {**body_a_data, **body_b_data, "physicsClientId": self.id} - points_list = p.getClosestPoints(**kwargs) + points_list = p.getClosestPoints(**body_a_data, **body_b_data, distance=max_distance, physicsClientId=self.id) return ClosestPointsList([ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0]) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 97d6089fa..a609de66f 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -68,7 +68,7 @@ def test_get_joint_position(self): self.assertAlmostEqual(self.robot.get_joint_position("head_pan_joint"), 0.0, delta=0.01) def test_get_object_contact_points(self): - self.assertEqual(len(self.robot.contact_points), 0) + self.assertEqual(len(self.robot.contact_points), 0) # 8 because of robot wheels with floor self.milk.set_position(self.robot.get_position()) self.assertTrue(len(self.robot.contact_points) > 0) From bd5c87c91c48b4616dc4d43d6a3e8077cd9692cc Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 27 Nov 2024 16:25:25 +0100 Subject: [PATCH 20/49] [JupyterTests] (WIP) cram_plan_tutorial has problems. --- src/pycram/datastructures/dataclasses.py | 9 +++++++++ src/pycram/description.py | 13 +++++++++++++ 2 files changed, 22 insertions(+) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 46e35568b..ca6eea568 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -930,6 +930,15 @@ def get_axes(self) -> Dict[str, Point]: return {getattr(self, field.name).name: getattr(self, field.name).axes for field in fields(self)} +@dataclass +class MultiverseBoundingBox: + """ + Dataclass for storing the bounding box of a body in the Multiverse simulation. + """ + min_point: List[float] + max_point: List[float] + + @dataclass class MultiverseMetaData: """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" diff --git a/src/pycram/description.py b/src/pycram/description.py index 001c34cd2..8b3cd7a7c 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -238,6 +238,19 @@ def get_axis_aligned_bounding_box_from_geometry(self) -> AxisAlignedBoundingBox: if isinstance(geom, MeshVisualShape) else geom.get_axis_aligned_bounding_box() return bounding_box + def get_axis_aligned_bounding_box_from_geometry(self) -> AxisAlignedBoundingBox: + if isinstance(self.geometry, List): + all_boxes = [geom.get_axis_aligned_bounding_box(self.get_mesh_path(geom)) + if isinstance(geom, MeshVisualShape) else geom.get_axis_aligned_bounding_box() + for geom in self.geometry + ] + bounding_box = AxisAlignedBoundingBox.from_multiple_bounding_boxes(all_boxes) + else: + geom = self.geometry + bounding_box = geom.get_axis_aligned_bounding_box(self.get_mesh_path(geom)) \ + if isinstance(geom, MeshVisualShape) else geom.get_axis_aligned_bounding_box() + return bounding_box + def get_convex_hull(self) -> Geometry3D: """ :return: The convex hull of the link geometry. From 93a30722b63df5039df359c65586246ea9baa558 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 28 Nov 2024 23:01:07 +0100 Subject: [PATCH 21/49] [MultiverseGetImages] Made use of target pose to adjust camera pose. --- test/test_multiverse.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index b083bbb26..9615bd4f0 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -153,7 +153,7 @@ def test_get_images_for_target(self): milk_spawn_position = np.array(camera_front_facing_axis) * 0.5 orientation = camera_pose.to_transform(camera_frame).invert().rotation_as_list() milk = self.spawn_milk(milk_spawn_position.tolist(), orientation, frame=camera_frame) - _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=False) + _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=True) self.assertIsInstance(depth, np.ndarray) self.assertIsInstance(segmentation_mask, np.ndarray) self.assertTrue(depth.shape == (256, 256)) From 36733a9e51a3015a4123b6ef0e2eb7fa7f40b672 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 29 Nov 2024 17:49:33 +0100 Subject: [PATCH 22/49] [MultiverseFallschoolDemo] Merged with dev. --- examples/cram_plan_tutorial.md | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md index c57fefe53..6d8f32d20 100644 --- a/examples/cram_plan_tutorial.md +++ b/examples/cram_plan_tutorial.md @@ -88,6 +88,7 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True): ``` ```python +import pycrap from tf.transformations import quaternion_from_euler import pycrap from pycram.costmaps import SemanticCostmap From 80978425117188a44996d7177be6c3c4fb7274fc Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sat, 30 Nov 2024 22:39:12 +0100 Subject: [PATCH 23/49] [MultiversePycrap] used pycrap in multiverse. --- test/test_multiverse.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 9615bd4f0..b083bbb26 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -153,7 +153,7 @@ def test_get_images_for_target(self): milk_spawn_position = np.array(camera_front_facing_axis) * 0.5 orientation = camera_pose.to_transform(camera_frame).invert().rotation_as_list() milk = self.spawn_milk(milk_spawn_position.tolist(), orientation, frame=camera_frame) - _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=True) + _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=False) self.assertIsInstance(depth, np.ndarray) self.assertIsInstance(segmentation_mask, np.ndarray) self.assertTrue(depth.shape == (256, 256)) From e9982a9337c5288ab2f304c6ae7b5eff44b74ad6 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Nov 2024 13:03:26 +0100 Subject: [PATCH 24/49] [NavigateAction] Add an option that asks if the joint states should be kept while navigating. --- src/pycram/external_interfaces/giskard.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index de6f704dd..8f864a7bf 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -329,6 +329,17 @@ def set_joint_goal(goal_poses: Dict[str, float]) -> None: giskard_wrapper.set_joint_goal(goal_poses) +def set_joint_goal(goal_poses: Dict[str, float]) -> None: + """ + Takes a dictionary of joint position that should be achieved, the keys in the dictionary are the joint names and + values are the goal joint positions. + + :param goal_poses: Dictionary with joint names and position goals + """ + sync_worlds() + giskard_wrapper.set_joint_goal(goal_poses) + + @init_giskard_interface @thread_safe def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str, From ef9129c6db6de985baaa1adbe4b35fbf4cb6d044 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 20 Nov 2024 17:59:45 +0100 Subject: [PATCH 25/49] [PhysicalObject] Added a physical object class. --- src/pycram/datastructures/world_entity.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 5ba1a7526..29fbc9b24 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -9,7 +9,7 @@ from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, PhysicalBodyState, \ - AxisAlignedBoundingBox, RotatedBoundingBox + AxisAlignedBoundingBox, RotatedBoundingBox, VisualShape from ..local_transformer import LocalTransformer from ..ros.data_types import Time From e9401c1a20580a0b373008bda98f06a0a79a0558 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 21 Nov 2024 16:03:42 +0100 Subject: [PATCH 26/49] [PhysicalBody] (WIP) refactoring bullet world to conform with new/changed world methods. --- src/pycram/datastructures/world.py | 18 +++++++++++++++++- src/pycram/datastructures/world_entity.py | 2 +- src/pycram/description.py | 2 +- src/pycram/worlds/bullet_world.py | 2 +- 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index a1cb210b3..4d3a7e786 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -697,6 +697,15 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ return self.get_body_contact_points(obj) + @abstractmethod + def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: + """ + Return the contact points of a body with all other bodies in the world. + + :param body: The body. + """ + return self.get_body_contact_points(obj) + @abstractmethod def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: """ @@ -709,7 +718,7 @@ def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: @deprecated("Use get_contact_points_between_two_bodies instead") def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: """ - Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of any type of bodies. + Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of bodies. """ return self.get_contact_points_between_two_bodies(obj1, obj2) @@ -724,6 +733,13 @@ def get_contact_points_between_two_bodies(self, body_1: PhysicalBody, body_2: Ph """ pass + @deprecated("Use get_contact_points_between_two_bodies instead") + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: + """ + Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of any type of bodies. + """ + return self.get_contact_points_between_two_bodies(obj1, obj2) + def get_body_closest_points(self, body: PhysicalBody, max_distance: float) -> ClosestPointsList: """ Return the closest points of this body with all other bodies in the world. diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 29fbc9b24..f3489a567 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -6,7 +6,7 @@ from copy import copy from trimesh.parent import Geometry3D -from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union +from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union, Any from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, PhysicalBodyState, \ AxisAlignedBoundingBox, RotatedBoundingBox, VisualShape diff --git a/src/pycram/description.py b/src/pycram/description.py index 8b3cd7a7c..079545788 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -11,7 +11,7 @@ from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, Sequence from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \ - MeshVisualShape, RotatedBoundingBox + MeshVisualShape, RotatedBoundingBox, ContactPointsList from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform from .datastructures.world_entity import WorldEntity, PhysicalBody diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 376f7ef4f..d614b2938 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -8,7 +8,7 @@ import numpy as np import pycram_bullet as p from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict, Any, Callable +from typing_extensions import List, Optional, Dict, Any, Callable, Tuple from pycrap import Floor from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ From 34e2de08e7d7db9894e08eec7dd6d71ffd4bdc7a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 22 Nov 2024 17:15:59 +0100 Subject: [PATCH 27/49] [PhysicalBody] (WIP) most tests are passing, there's a problem with transport action. --- src/pycram/datastructures/world.py | 11 +---------- src/pycram/datastructures/world_entity.py | 4 ++-- src/pycram/description.py | 2 +- src/pycram/world_concepts/world_object.py | 1 - src/pycram/worlds/bullet_world.py | 2 +- 5 files changed, 5 insertions(+), 15 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 4d3a7e786..4df664d1b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -697,15 +697,6 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ return self.get_body_contact_points(obj) - @abstractmethod - def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: - """ - Return the contact points of a body with all other bodies in the world. - - :param body: The body. - """ - return self.get_body_contact_points(obj) - @abstractmethod def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: """ @@ -718,7 +709,7 @@ def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: @deprecated("Use get_contact_points_between_two_bodies instead") def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: """ - Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of bodies. + Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of any type of bodies. """ return self.get_contact_points_between_two_bodies(obj1, obj2) diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index f3489a567..5ba1a7526 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -6,10 +6,10 @@ from copy import copy from trimesh.parent import Geometry3D -from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union, Any +from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, PhysicalBodyState, \ - AxisAlignedBoundingBox, RotatedBoundingBox, VisualShape + AxisAlignedBoundingBox, RotatedBoundingBox from ..local_transformer import LocalTransformer from ..ros.data_types import Time diff --git a/src/pycram/description.py b/src/pycram/description.py index 079545788..8b3cd7a7c 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -11,7 +11,7 @@ from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, Sequence from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \ - MeshVisualShape, RotatedBoundingBox, ContactPointsList + MeshVisualShape, RotatedBoundingBox from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform from .datastructures.world_entity import WorldEntity, PhysicalBody diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 1a94c5f9e..be16e0561 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -95,7 +95,6 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] self.name: str = name self.path: Optional[str] = path - self.color: Color = color self._resolve_description(path, description) self.cache_manager = self.world.cache_manager diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index d614b2938..376f7ef4f 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -8,7 +8,7 @@ import numpy as np import pycram_bullet as p from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict, Any, Callable, Tuple +from typing_extensions import List, Optional, Dict, Any, Callable from pycrap import Floor from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ From 0853f02d88c9af3904ba37d18be144ba764a57d8 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 24 Nov 2024 17:33:47 +0100 Subject: [PATCH 28/49] [World] rename argument to is_prospection --- src/pycram/datastructures/world.py | 9 ++++---- src/pycram/datastructures/world_entity.py | 8 ++++++++ src/pycram/worlds/bullet_world.py | 8 ++++---- src/pycram/worlds/multiverse.py | 25 ++++++++++++++++++----- 4 files changed, 36 insertions(+), 14 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 4df664d1b..1f90b6f7b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -78,8 +78,7 @@ class World(StateEntity, ABC): The ontology of this world. """ - def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False, - clear_cache: bool = False): + def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = False, clear_cache: bool = False): """ Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -87,7 +86,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" - :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. + :param is_prospection: For internal usage, decides if this World should be used as a prospection world. :param clear_cache: Whether to clear the cache directory. """ @@ -115,7 +114,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self.objects: List[Object] = [] # List of all Objects in the World - self.is_prospection_world: bool = is_prospection_world + self.is_prospection_world: bool = is_prospection self._init_and_sync_prospection_world() self.local_transformer = LocalTransformer() @@ -263,7 +262,7 @@ def _init_prospection_world(self): if self.is_prospection_world: # then no need to add another prospection world self.prospection_world = None else: - self.prospection_world: World = self.__class__(is_prospection_world=True) + self.prospection_world: World = self.__class__(is_prospection=True) def _sync_prospection_world(self): """ diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 5ba1a7526..c51e9e2aa 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -139,6 +139,14 @@ def __init__(self, body_id: int, world: World): self._is_rotating: Optional[bool] = None self._velocity: Optional[List[float]] = None + @property + @abstractmethod + def name(self) -> str: + """ + :return: The name of this body. + """ + ... + @abstractmethod def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 376f7ef4f..f94cc2a8d 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -37,7 +37,7 @@ class is the main interface to the Bullet Physics Engine and should be used to s manipulate the Bullet World. """ - def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False, + def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = False, use_multiverse_for_real_world_simulation: bool = False): """ Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the @@ -45,10 +45,10 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default is "GUI" - :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. + :param is_prospection: For internal usage, decides if this BulletWorld should be used as a shadow world. :param use_multiverse_for_real_world_simulation: Whether to use the Multiverse for real world simulation. """ - super().__init__(mode=mode, is_prospection_world=is_prospection_world) + super().__init__(mode=mode, is_prospection=is_prospection) if use_multiverse_for_real_world_simulation: self.add_multiverse_resources() @@ -64,7 +64,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo # Some default settings self.set_gravity([0, 0, -9.8]) - if not is_prospection_world: + if not is_prospection: _ = Object("floor", Floor, "plane.urdf", world=self) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index fc8be0090..0a68761ae 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -18,6 +18,7 @@ MultiverseJointCMD from ..datastructures.pose import Pose from ..datastructures.world import World +from ..datastructures.world_entity import PhysicalBody from ..description import Link, Joint from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..object_descriptors.mjcf import ObjectDescription as MJCF, PrimitiveObjectFactory @@ -55,12 +56,12 @@ class Multiverse(World): Add the MJCF description extension to the extension to description type mapping for the objects. """ - def __init__(self, is_prospection_world: Optional[bool] = False, + def __init__(self, is_prospection: Optional[bool] = False, clear_cache: bool = False): """ Initialize the Multiverse Socket and the PyCram World. - :param is_prospection_world: Whether the world is prospection or not. + :param is_prospection: Whether the world is prospection or not. :param clear_cache: Whether to clear the cache or not. """ @@ -68,11 +69,11 @@ def __init__(self, is_prospection_world: Optional[bool] = False, self.saved_simulator_states: Dict = {} self.make_sure_multiverse_resources_are_added(clear_cache=clear_cache) - self.simulation = self.conf.prospection_world_prefix if is_prospection_world else "belief_state" + self.simulation = self.conf.prospection_world_prefix if is_prospection else "belief_state" self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) - self._init_clients(is_prospection=is_prospection_world) + self._init_clients(is_prospection=is_prospection) - World.__init__(self, WorldMode.DIRECT, is_prospection_world) + World.__init__(self, mode=WorldMode.DIRECT, is_prospection=is_prospection) self._init_constraint_and_object_id_name_map_collections() @@ -544,6 +545,20 @@ def remove_constraint(self, constraint_id) -> None: def perform_collision_detection(self) -> None: ... + def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: + if isinstance(body, Object): + return self.get_object_contact_points(body) + else: + multiverse_contact_points = self.api_requester.get_contact_points(body.name) + contact_points = ContactPointsList([]) + for mcp in multiverse_contact_points: + body_object, body_link = self.get_object_with_body_name(mcp.body_2) + obj, obj_link = self.get_object_with_body_name(mcp.body_1) + contact_points.append(ContactPoint(obj_link, body_link)) + contact_points[-1].normal_on_b = mcp.normal + contact_points[-1].position_on_b = mcp.position + return contact_points + def get_object_contact_points(self, obj: Object, ignore_attached_objects: bool = True) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. From 263235e9788243b6343591efd616fb3638ba1647 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 24 Nov 2024 17:34:12 +0100 Subject: [PATCH 29/49] [VirtualJoint] no need for object. --- src/pycram/datastructures/dataclasses.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index ca6eea568..1e09ed36e 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -881,7 +881,6 @@ class VirtualJoint: name: str type_: JointType axes: Optional[Point] = None - object: Optional[Object] = None @property def type(self): @@ -900,6 +899,7 @@ class VirtualMobileBaseJoints: """ Dataclass for storing the names, types and axes of the virtual mobile base joints of a mobile robot. """ + translation_x: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.LINEAR_X.value, JointType.PRISMATIC, Point(1, 0, 0)) From 8d39764b7674a700c5234f4c0a23c712c693eb89 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 24 Nov 2024 17:35:25 +0100 Subject: [PATCH 30/49] [Link] Implement name property as it exists in two parents. --- src/pycram/description.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/pycram/description.py b/src/pycram/description.py index 8b3cd7a7c..0756434fc 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -199,6 +199,13 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} + @property + def name(self) -> str: + """ + :return: The name of this link. + """ + return self.parsed_description.name + def get_axis_aligned_bounding_box(self, transform_to_link_pose: bool = True) -> AxisAlignedBoundingBox: """ :param transform_to_link_pose: If True, return the bounding box transformed to the link pose. From 0828ef0b5f7b2a4b4061b70923673695320bd43f Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 24 Nov 2024 17:36:02 +0100 Subject: [PATCH 31/49] [WorldObject] make color None by default instead of white. --- src/pycram/world_concepts/world_object.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index be16e0561..91eb9ba81 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -60,7 +60,7 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] description: Optional[ObjectDescription] = None, pose: Optional[Pose] = None, world: Optional[World] = None, - color: Color = Color(), + color: Optional[Color] = None, ignore_cached_files: bool = False, scale_mesh: Optional[float] = None, mesh_transform: Optional[Transform] = None): @@ -120,7 +120,9 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] self._init_link_name_and_id_map() self._init_links_and_update_transforms() - self.color: Color = color + + if color is not None: + self.color: Color = color self._init_joints() From cff62f1f266765a1c9328d6caf858c8a1af83f04 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 24 Nov 2024 19:16:49 +0100 Subject: [PATCH 32/49] [LocalTransformer] solved bug where world objects were used instead of prospection objects. --- src/pycram/world_concepts/world_object.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 91eb9ba81..41e98b565 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -130,7 +130,7 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] self.world.add_object(self) - @cached_property + @property def tf_frame(self) -> str: """ The tf frame of the object. From a1a7db806795cb93c1bd8927431695aa661b1938 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 24 Nov 2024 19:17:11 +0100 Subject: [PATCH 33/49] [Logging] added set log level. --- src/pycram/description.py | 6 +++--- src/pycram/ros/logging.py | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 0756434fc..2bf0cbcbf 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -17,7 +17,7 @@ from .datastructures.world_entity import WorldEntity, PhysicalBody from .failures import ObjectDescriptionNotFound, LinkHasNoGeometry, LinkGeometryHasNoMesh from .local_transformer import LocalTransformer -from .ros.logging import logwarn +from .ros.logging import logwarn_once if TYPE_CHECKING: from .world_concepts.world_object import Object @@ -439,8 +439,8 @@ def pose(self) -> Pose: @pose.setter def pose(self, pose: Pose) -> None: - logwarn("Setting the pose of a link is not allowed," - " change object pose and/or joint position to affect the link pose.") + logwarn_once("Setting the pose of a link is not allowed," + " change object pose and/or joint position to affect the link pose.") @property def color(self) -> Color: diff --git a/src/pycram/ros/logging.py b/src/pycram/ros/logging.py index 213d911eb..f284b6598 100644 --- a/src/pycram/ros/logging.py +++ b/src/pycram/ros/logging.py @@ -15,6 +15,7 @@ logger_level_service_caller = LoggerLevelServiceCaller() + def _get_caller_method_name(): """ Get the name of the method that called the function from which this function is called. It is intended as a helper From aef6fe0525b8198e9b8f1b214b2c99a31863773e Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 26 Nov 2024 22:56:10 +0100 Subject: [PATCH 34/49] [PhysicalBody] (WIP) Contact points return bodies not links. --- src/pycram/datastructures/dataclasses.py | 60 ++++++++++++++--------- src/pycram/datastructures/world_entity.py | 14 +++++- src/pycram/description.py | 11 +++-- src/pycram/world_reasoning.py | 2 +- src/pycram/worlds/multiverse.py | 4 +- test/test_multiverse.py | 8 +-- 6 files changed, 64 insertions(+), 35 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 1e09ed36e..9bbee69fe 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -11,6 +11,7 @@ from .enums import JointType, Shape, VirtualMobileBaseJointName from .pose import Pose, Point, Transform +from .world_entity import PhysicalBody from ..validation.error_checkers import calculate_joint_position_error, is_error_acceptable if TYPE_CHECKING: @@ -697,20 +698,20 @@ class LateralFriction: @dataclass class ContactPoint: """ - Dataclass for storing the information of a contact point between two objects. + Dataclass for storing the information of a contact point between two bodies. """ - link_a: Link - link_b: Link - position_on_object_a: Optional[List[float]] = None - position_on_object_b: Optional[List[float]] = None - normal_on_b: Optional[List[float]] = None # the contact normal vector on object b pointing towards object a + body_a: PhysicalBody + body_b: PhysicalBody + position_on_body_a: Optional[List[float]] = None + position_on_body_b: Optional[List[float]] = None + normal_on_body_b: Optional[List[float]] = None # the contact normal vector on object b pointing towards object a distance: Optional[float] = None # distance between the two objects (+ve for separation, -ve for penetration) normal_force: Optional[List[float]] = None # normal force applied during last step simulation lateral_friction_1: Optional[LateralFriction] = None lateral_friction_2: Optional[LateralFriction] = None def __str__(self): - return f"ContactPoint: {self.link_a.object.name} - {self.link_b.object.name}" + return f"ContactPoint: {self.body_a.name} - {self.body_b.name}" def __repr__(self): return self.__str__() @@ -727,24 +728,24 @@ class ContactPointsList(list): A list of contact points. """ - def get_links_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]: + def get_bodies_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]: """ - Return the links that are not in the current points list but were in the initial points list. + Return the bodies that are not in the current points list but were in the initial points list. :param previous_points: The initial points list. - :return: A list of Link instances that represent the links that got removed. + :return: A list of bodies that got removed. """ - initial_links_in_contact = previous_points.get_links_in_contact() - current_links_in_contact = self.get_links_in_contact() - return [link for link in initial_links_in_contact if link not in current_links_in_contact] + initial_bodies_in_contact = previous_points.get_bodies_in_contact() + current_bodies_in_contact = self.get_bodies_in_contact() + return [body for body in initial_bodies_in_contact if body not in current_bodies_in_contact] - def get_links_in_contact(self) -> List[Link]: + def get_bodies_in_contact(self) -> List[PhysicalBody]: """ - Get the links in contact. + Get the bodies in contact. - :return: A list of Link instances that represent the links in contact. + :return: A list of bodies that are in contact. """ - return [point.link_b for point in self] + return [point.body_b for point in self] def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> bool: """ @@ -754,8 +755,19 @@ def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> b :param obj_b: An instance of the Object class that represents the second object. :return: True if the objects are in contact, False otherwise. """ - return (any([point.link_b.object == obj_b and point.link_a.object == obj_a for point in self]) or - any([point.link_a.object == obj_b and point.link_b.object == obj_a for point in self])) + return (any([self.is_body_in_object(point.body_b, obj_b) and self.is_body_in_object(point.body_a, obj_a) for point in self]) or + any([self.is_body_in_object(point.body_a, obj_b) and self.is_body_in_object(point.body_b, obj_a) for point in self])) + + @staticmethod + def is_body_in_object(body: PhysicalBody, obj: Object) -> bool: + """ + Check if the body belongs to the object. + + :param body: The body. + :param obj: The object. + :return: True if the body belongs to the object, False otherwise. + """ + return body in obj.links or body == obj def get_normals_of_object(self, obj: Object) -> List[List[float]]: """ @@ -772,7 +784,7 @@ def get_normals(self) -> List[List[float]]: :return: A list of float vectors that represent the normals of the contact points. """ - return [point.normal_on_b for point in self] + return [point.normal_on_body_b for point in self] def get_links_in_contact_of_object(self, obj: Object) -> List[Link]: """ @@ -781,7 +793,7 @@ def get_links_in_contact_of_object(self, obj: Object) -> List[Link]: :param obj: An instance of the Object class that represents the object. :return: A list of Link instances that represent the links in contact of the object. """ - return [point.link_b for point in self if point.link_b.object == obj] + return [point.body_b for point in self if point.body_b.parent == obj] def get_points_of_object(self, obj: Object) -> 'ContactPointsList': """ @@ -790,7 +802,7 @@ def get_points_of_object(self, obj: Object) -> 'ContactPointsList': :param obj: An instance of the Object class that represents the object that the points are related to. :return: A ContactPointsList instance that represents the contact points of the object. """ - return ContactPointsList([point for point in self if point.link_b.object == obj]) + return ContactPointsList([point for point in self if self.is_body_in_object(point.body_b, obj)]) def get_points_of_link(self, link: Link) -> 'ContactPointsList': """ @@ -799,7 +811,7 @@ def get_points_of_link(self, link: Link) -> 'ContactPointsList': :param link: An instance of the Link class that represents the link that the points are related to. :return: A ContactPointsList instance that represents the contact points of the link. """ - return ContactPointsList([point for point in self if point.link_b == link]) + return ContactPointsList([point for point in self if link == point.body_b]) def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]: """ @@ -846,7 +858,7 @@ def get_objects_that_have_points(self) -> List[Object]: :return: A list of Object instances that represent the objects that have points in the list. """ - return list({point.link_b.object for point in self}) + return list({point.body_b.object for point in self}) def __str__(self): return f"ContactPointsList: {', '.join([point.__str__() for point in self])}" diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index c51e9e2aa..10f7c7a73 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -366,9 +366,21 @@ def color(self, color: Color) -> None: """ ... + @property @abstractmethod + def parent(self) -> Optional[PhysicalBody]: + """ + :return: The parent of this body, if it has one. + """ + pass + def __eq__(self, other: PhysicalBody) -> bool: """ Check if this body is equal to another body. """ - ... + if not isinstance(other, PhysicalBody): + return False + return self.id == other.id, self.name == other.name, self.parent == other.parent + + def __hash__(self) -> int: + return hash(self.id, self.name, self.parent) diff --git a/src/pycram/description.py b/src/pycram/description.py index 2bf0cbcbf..a8c4ba66f 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -199,6 +199,13 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} + @property + def parent(self) -> PhysicalBody: + """ + :return: The parent of this link, which is the object. + """ + return self.object + @property def name(self) -> str: """ @@ -473,9 +480,7 @@ def origin_transform(self) -> Transform: return self.origin.to_transform(self.tf_frame) def __eq__(self, other: Link): - if not isinstance(other, Link): - return False - return self.id == other.id and self.object == other.object and self.name == other.name + return PhysicalBody.__eq__(self, other) def __copy__(self): return Link(self.id, self, self.object) diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 53affb77e..df013d945 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -57,7 +57,7 @@ def contact( prospection_obj2) objects_are_in_contact = len(con_points) > 0 if return_links: - contact_links = [(point.link_a, point.link_b) for point in con_points] + contact_links = [(point.body_a, point.body_b) for point in con_points] return objects_are_in_contact, contact_links else: return objects_are_in_contact diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 0a68761ae..4cb72f5be 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -555,7 +555,7 @@ def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList: body_object, body_link = self.get_object_with_body_name(mcp.body_2) obj, obj_link = self.get_object_with_body_name(mcp.body_1) contact_points.append(ContactPoint(obj_link, body_link)) - contact_points[-1].normal_on_b = mcp.normal + contact_points[-1].normal_on_body_b = mcp.normal contact_points[-1].position_on_b = mcp.position return contact_points @@ -579,7 +579,7 @@ def get_object_contact_points(self, obj: Object, ignore_attached_objects: bool = body_object, body_link = self.get_object_with_body_name(mcp.body_2) obj_link = obj.links[mcp.body_1] if mcp.body_1 in obj.links.keys() else obj.root_link contact_points.append(ContactPoint(obj_link, body_link)) - contact_points[-1].normal_on_b = mcp.normal + contact_points[-1].normal_on_body_b = mcp.normal contact_points[-1].position_on_b = mcp.position return contact_points diff --git a/test/test_multiverse.py b/test/test_multiverse.py index b083bbb26..54541f2cc 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -370,7 +370,7 @@ def test_get_object_contact_points(self): self.assertIsInstance(contact_points, ContactPointsList) self.assertTrue(len(contact_points) >= 1) self.assertIsInstance(contact_points[0], ContactPoint) - self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) + self.assertTrue(contact_points[0].body_b.object, self.multiverse.floor) cup = self.spawn_cup([1, 1, 0.12]) # This is needed because the cup is spawned in the air, so it needs to fall # to get in contact with the milk @@ -379,7 +379,7 @@ def test_get_object_contact_points(self): self.assertIsInstance(contact_points, ContactPointsList) self.assertTrue(len(contact_points) >= 1) self.assertIsInstance(contact_points[0], ContactPoint) - self.assertTrue(contact_points[0].link_b.object, milk) + self.assertTrue(contact_points[0].body_b.object, milk) self.tearDown() def test_get_robot_contact_points(self): @@ -401,8 +401,8 @@ def test_get_contact_points_between_two_objects(self): self.assertIsInstance(contact_points, ContactPointsList) self.assertTrue(len(contact_points) >= 1) self.assertIsInstance(contact_points[0], ContactPoint) - self.assertTrue(contact_points[0].link_a.object, milk) - self.assertTrue(contact_points[0].link_b.object, cup) + self.assertTrue(contact_points[0].body_a.object, milk) + self.assertTrue(contact_points[0].body_b.object, cup) self.tearDown() def test_get_one_ray(self): From 08e8b38a42ecc97c478e15d65a9bc3d0bc0895fe Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Wed, 27 Nov 2024 01:25:57 +0100 Subject: [PATCH 35/49] [PhysicalBody] Everything is a WorldEntity, and they all have parent entities except for world. --- src/pycram/datastructures/dataclasses.py | 12 ++--- src/pycram/datastructures/world.py | 29 ++++++++---- src/pycram/datastructures/world_entity.py | 54 +++++++++++------------ src/pycram/description.py | 36 ++++++++------- src/pycram/world_concepts/world_object.py | 13 +++--- src/pycram/worlds/bullet_world.py | 10 ++--- 6 files changed, 85 insertions(+), 69 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 9bbee69fe..1d97b5f71 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -11,13 +11,13 @@ from .enums import JointType, Shape, VirtualMobileBaseJointName from .pose import Pose, Point, Transform -from .world_entity import PhysicalBody from ..validation.error_checkers import calculate_joint_position_error, is_error_acceptable if TYPE_CHECKING: from ..description import Link from ..world_concepts.world_object import Object from ..world_concepts.constraints import Attachment + from .world_entity import PhysicalBody def get_point_as_list(point: Point) -> List[float]: @@ -755,8 +755,10 @@ def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> b :param obj_b: An instance of the Object class that represents the second object. :return: True if the objects are in contact, False otherwise. """ - return (any([self.is_body_in_object(point.body_b, obj_b) and self.is_body_in_object(point.body_a, obj_a) for point in self]) or - any([self.is_body_in_object(point.body_a, obj_b) and self.is_body_in_object(point.body_b, obj_a) for point in self])) + return (any([self.is_body_in_object(point.body_b, obj_b) + and self.is_body_in_object(point.body_a, obj_a) for point in self]) or + any([self.is_body_in_object(point.body_a, obj_b) + and self.is_body_in_object(point.body_b, obj_a) for point in self])) @staticmethod def is_body_in_object(body: PhysicalBody, obj: Object) -> bool: @@ -786,7 +788,7 @@ def get_normals(self) -> List[List[float]]: """ return [point.normal_on_body_b for point in self] - def get_links_in_contact_of_object(self, obj: Object) -> List[Link]: + def get_links_in_contact_of_object(self, obj: Object) -> List[PhysicalBody]: """ Get the links in contact of the object. @@ -858,7 +860,7 @@ def get_objects_that_have_points(self) -> List[Object]: :return: A list of Object instances that represent the objects that have points in the list. """ - return list({point.body_b.object for point in self}) + return list({point.body_b.parent for point in self if isinstance(point.body_b.parent, Object)}) def __str__(self): return f"ContactPointsList: {', '.join([point.__str__() for point in self])}" diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 1f90b6f7b..13d2a9a9e 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -24,7 +24,7 @@ ContactPointsList, VirtualMobileBaseJoints, RotatedBoundingBox) from ..datastructures.enums import JointType, WorldMode, Arms from ..datastructures.pose import Pose, Transform -from ..datastructures.world_entity import StateEntity, PhysicalBody +from ..datastructures.world_entity import StateEntity, PhysicalBody, WorldEntity from ..failures import ProspectionObjectNotFound, WorldObjectNotFound from ..local_transformer import LocalTransformer from ..robot_description import RobotDescription @@ -42,7 +42,7 @@ from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription -class World(StateEntity, ABC): +class World(WorldEntity, ABC): """ The World Class represents the physics Simulation and belief state, it is the main interface for reasoning about the World. This is implemented as a singleton, the current World can be accessed via the static variable @@ -78,7 +78,8 @@ class World(StateEntity, ABC): The ontology of this world. """ - def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = False, clear_cache: bool = False): + def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = False, clear_cache: bool = False, + id_: int = -1): """ Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -88,12 +89,11 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = Fa "GUI" :param is_prospection: For internal usage, decides if this World should be used as a prospection world. :param clear_cache: Whether to clear the cache directory. + :param id_: The unique id of the world. """ - StateEntity.__init__(self) - + WorldEntity.__init__(self, id_, self) self.ontology = pycrap.Ontology() - self.latest_state_id: Optional[int] = None if clear_cache or (self.conf.clear_cache_at_start and not self.cache_manager.cache_cleared): @@ -106,9 +106,6 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = Fa self.object_lock: threading.Lock = threading.Lock() - self.id: Optional[int] = -1 - # This is used to connect to the physics server (allows multiple clients) - self._init_world(mode) self.objects: List[Object] = [] @@ -132,6 +129,20 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = Fa self.on_add_object_callbacks: List[Callable[[Object], None]] = [] + @property + def parent_entity(self) -> Optional[WorldEntity]: + """ + Return the parent entity of this entity, in this case it is None as the World is the top level entity. + """ + return None + + @property + def name(self) -> str: + """ + Return the name of the world, which is the name of the implementation class (e.g. BulletWorld). + """ + return self.__class__.__name__ + def get_body_convex_hull(self, body: PhysicalBody) -> Geometry3D: """ :param body: The body object. diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 10f7c7a73..ec2fa41a5 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -126,6 +126,33 @@ def __init__(self, _id: int, world: World): self.id = _id self.world: World = world + @property + @abstractmethod + def name(self) -> str: + """ + :return: The name of this body. + """ + ... + + @property + @abstractmethod + def parent_entity(self) -> Optional[WorldEntity]: + """ + :return: The parent entity of this entity, if it has one. + """ + pass + + def __eq__(self, other: WorldEntity) -> bool: + """ + Check if this body is equal to another body. + """ + if not isinstance(other, WorldEntity): + return False + return self.id == other.id and self.name == other.name and self.parent_entity == other.parent_entity + + def __hash__(self) -> int: + return hash((self.id, self.name, self.parent_entity)) + class PhysicalBody(WorldEntity, ABC): """ @@ -139,14 +166,6 @@ def __init__(self, body_id: int, world: World): self._is_rotating: Optional[bool] = None self._velocity: Optional[List[float]] = None - @property - @abstractmethod - def name(self) -> str: - """ - :return: The name of this body. - """ - ... - @abstractmethod def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ @@ -365,22 +384,3 @@ def color(self, color: Color) -> None: :param color: The color as a list of floats, either rgb or rgba. """ ... - - @property - @abstractmethod - def parent(self) -> Optional[PhysicalBody]: - """ - :return: The parent of this body, if it has one. - """ - pass - - def __eq__(self, other: PhysicalBody) -> bool: - """ - Check if this body is equal to another body. - """ - if not isinstance(other, PhysicalBody): - return False - return self.id == other.id, self.name == other.name, self.parent == other.parent - - def __hash__(self) -> int: - return hash(self.id, self.name, self.parent) diff --git a/src/pycram/description.py b/src/pycram/description.py index a8c4ba66f..3c0c5df94 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -196,11 +196,12 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): PhysicalBody.__init__(self, _id, obj.world) ObjectEntity.__init__(self, obj) LinkDescription.__init__(self, link_description.parsed_description, link_description.mesh_dir) + self.description = link_description self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} @property - def parent(self) -> PhysicalBody: + def parent_entity(self) -> Object: """ :return: The parent of this link, which is the object. """ @@ -211,7 +212,7 @@ def name(self) -> str: """ :return: The name of this link. """ - return self.parsed_description.name + return self.description.name def get_axis_aligned_bounding_box(self, transform_to_link_pose: bool = True) -> AxisAlignedBoundingBox: """ @@ -479,14 +480,8 @@ def origin_transform(self) -> Transform: """ return self.origin.to_transform(self.tf_frame) - def __eq__(self, other: Link): - return PhysicalBody.__eq__(self, other) - def __copy__(self): - return Link(self.id, self, self.object) - - def __hash__(self): - return hash((self.id, self.object, self.name)) + return Link(self.id, self.description, self.object) class RootLink(Link, ABC): @@ -527,10 +522,25 @@ def __init__(self, _id: int, WorldEntity.__init__(self, _id, obj.world) ObjectEntity.__init__(self, obj) JointDescription.__init__(self, joint_description.parsed_description, is_virtual) + self.description = joint_description self.acceptable_error = (self.world.conf.revolute_joint_position_tolerance if self.type == JointType.REVOLUTE else self.world.conf.prismatic_joint_position_tolerance) self._update_position() + @property + def name(self) -> str: + """ + :return: The name of this joint. + """ + return self.description.name + + @property + def parent_entity(self) -> Link: + """ + :return: The parent of this joint, which is the object. + """ + return self.parent_link + @property def tf_frame(self) -> str: """ @@ -628,13 +638,7 @@ def current_state(self, joint_state: JointState) -> None: self.position = joint_state.position def __copy__(self): - return Joint(self.id, self, self.object) - - def __eq__(self, other): - return self.id == other.id and self.object == other.object and self.name == other.name - - def __hash__(self): - return hash((self.id, self.object, self.name)) + return Joint(self.id, self.description, self.object, self.is_virtual) class ObjectDescription(EntityDescription): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 41e98b565..11a36b26f 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -19,7 +19,7 @@ from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform from ..datastructures.world import World -from ..datastructures.world_entity import PhysicalBody +from ..datastructures.world_entity import PhysicalBody, WorldEntity from ..description import ObjectDescription, LinkDescription, Joint from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects, UnsupportedFileExtension, \ ObjectDescriptionUndefined @@ -1484,10 +1484,9 @@ def copy_to_world(self, world: World) -> Object: world, self.color) return obj - def __eq__(self, other): - return (isinstance(other, Object) and self.id == other.id and self.name == other.name - and self.world == other.world) - - def __hash__(self): - return hash((self.id, self.name, self.world)) + def parent_entity(self) -> World: + """ + :return: The parent of this object which is the world. + """ + return self.world diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index f94cc2a8d..49a26f14e 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -235,11 +235,11 @@ def parse_points_list_to_args(self, point: List) -> Dict: :param point: The list of points. """ - return {"link_a": self.get_object_by_id(point[1]).get_link_by_id(point[3]), - "link_b": self.get_object_by_id(point[2]).get_link_by_id(point[4]), - "position_on_object_a": point[5], - "position_on_object_b": point[6], - "normal_on_b": point[7], + return {"body_a": self.get_object_by_id(point[1]).get_link_by_id(point[3]), + "body_b": self.get_object_by_id(point[2]).get_link_by_id(point[4]), + "position_on_body_a": point[5], + "position_on_body_b": point[6], + "normal_on_body_b": point[7], "distance": point[8], "normal_force": point[9], "lateral_friction_1": LateralFriction(point[10], point[11]), From e29b49304bd1dff183b47154eb8fb9736763473b Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 2 Dec 2024 16:05:45 +0100 Subject: [PATCH 36/49] [PhysicalBody] used pycrap in physical body instead of object. --- src/pycram/datastructures/world_entity.py | 8 +++++++- src/pycrap/base.py | 5 ++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index ec2fa41a5..8b0185781 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -6,8 +6,9 @@ from copy import copy from trimesh.parent import Geometry3D -from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union +from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union, Type +from pycrap import PhysicalObject from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, PhysicalBodyState, \ AxisAlignedBoundingBox, RotatedBoundingBox from ..local_transformer import LocalTransformer @@ -159,6 +160,11 @@ class PhysicalBody(WorldEntity, ABC): A class that represents a physical body in the world that has some related physical properties. """ + ontology_concept: Type[PhysicalObject] = PhysicalObject + """ + The ontology concept of this entity. + """ + def __init__(self, body_id: int, world: World): WorldEntity.__init__(self, body_id, world) self.local_transformer = LocalTransformer() diff --git a/src/pycrap/base.py b/src/pycrap/base.py index 0c263f15d..d9e6e2090 100644 --- a/src/pycrap/base.py +++ b/src/pycrap/base.py @@ -6,6 +6,7 @@ default_pycrap_ontology_file = tempfile.NamedTemporaryFile() default_pycrap_ontology = owlready2.get_ontology("file://" + default_pycrap_ontology_file.name).load() + class Base(Thing): comment = __doc__ namespace = default_pycrap_ontology @@ -17,5 +18,7 @@ def set_comment_to_docstring(cls): class PhysicalObject(Base): """ - Any Object that has a proper space region. The prototypical physical object has also an associated mass, but the nature of its mass can greatly vary based on the epistemological status of the object (scientifically measured, subjectively possible, imaginary). + Any Object that has a proper space region. The prototypical physical object has also an associated mass, + but the nature of its mass can greatly vary based on the epistemological status of the object (scientifically + measured, subjectively possible, imaginary). """ \ No newline at end of file From 9a3cef62111a6eadb986707c7480fb22fd314277 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 2 Dec 2024 16:18:48 +0100 Subject: [PATCH 37/49] [Giskard] removed duplicate set_joint_goal. --- src/pycram/external_interfaces/giskard.py | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 8f864a7bf..de6f704dd 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -329,17 +329,6 @@ def set_joint_goal(goal_poses: Dict[str, float]) -> None: giskard_wrapper.set_joint_goal(goal_poses) -def set_joint_goal(goal_poses: Dict[str, float]) -> None: - """ - Takes a dictionary of joint position that should be achieved, the keys in the dictionary are the joint names and - values are the goal joint positions. - - :param goal_poses: Dictionary with joint names and position goals - """ - sync_worlds() - giskard_wrapper.set_joint_goal(goal_poses) - - @init_giskard_interface @thread_safe def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str, From 9c37d266f880babc81de690e32bebb4142cd2333 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 4 Dec 2024 10:28:45 +0100 Subject: [PATCH 38/49] [PhysicalBody] fixed some multiverse bugs. --- src/pycram/datastructures/dataclasses.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 1d97b5f71..4530b7e35 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -769,7 +769,7 @@ def is_body_in_object(body: PhysicalBody, obj: Object) -> bool: :param obj: The object. :return: True if the body belongs to the object, False otherwise. """ - return body in obj.links or body == obj + return body in list(obj.links.values()) or body == obj def get_normals_of_object(self, obj: Object) -> List[List[float]]: """ From 2333eda16cf971e1b0f543ae73c48a9a19e645a1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 5 Dec 2024 15:52:08 +0100 Subject: [PATCH 39/49] [MultiverseFallSchoolDemo] added optional argument grasps to CostmapLocation. Check for continuous joint when clipping. Fixed multiverse demo. Don't set link_state as it is not settable/needed. --- src/pycram/pose_generator_and_validator.py | 2 ++ src/pycram/worlds/multiverse.py | 8 ++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index a6d55412c..2dc14f344 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -191,6 +191,8 @@ def reachability_validator(pose: Pose, # TODO Make orientation adhere to grasping orientation res = False arms = [] + found_grasps = [] + original_target = target for description in manipulator_descs: retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame( description.end_effector.tool_frame)) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 4cb72f5be..747416bf8 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -56,12 +56,12 @@ class Multiverse(World): Add the MJCF description extension to the extension to description type mapping for the objects. """ - def __init__(self, is_prospection: Optional[bool] = False, + def __init__(self, is_prospection_world: Optional[bool] = False, clear_cache: bool = False): """ Initialize the Multiverse Socket and the PyCram World. - :param is_prospection: Whether the world is prospection or not. + :param is_prospection_world: Whether the world is prospection or not. :param clear_cache: Whether to clear the cache or not. """ @@ -69,9 +69,9 @@ def __init__(self, is_prospection: Optional[bool] = False, self.saved_simulator_states: Dict = {} self.make_sure_multiverse_resources_are_added(clear_cache=clear_cache) - self.simulation = self.conf.prospection_world_prefix if is_prospection else "belief_state" + self.simulation = self.conf.prospection_world_prefix if is_prospection_world else "belief_state" self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) - self._init_clients(is_prospection=is_prospection) + self._init_clients(is_prospection=is_prospection_world) World.__init__(self, mode=WorldMode.DIRECT, is_prospection=is_prospection) From 9403bcdca297f1522f4388dba4f5f03e9324d4c8 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 11 Dec 2024 18:07:44 +0100 Subject: [PATCH 40/49] [PhysicalObject] corrections after rebasing on dev. --- src/pycram/designators/action_designator.py | 8 +------- src/pycram/process_modules/pr2_process_modules.py | 2 +- src/pycram/worlds/multiverse.py | 8 ++++---- test/test_task_tree.py | 8 ++++---- 4 files changed, 10 insertions(+), 16 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index e8701f9d0..9a502f34e 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -448,13 +448,7 @@ def plan(self) -> None: for pose in pickup_loc: if self.arm in pose.reachable_arms: pickup_pose = pose - NavigateActionPerformable(pickup_pose.pose).perform() - robot = robot_desig.resolve().world_object - if robot.pose.almost_equal(pickup_pose.pose, 0.1, 3): - break - else: - pickup_pose = None - continue + break if not pickup_pose: raise ObjectUnfetchable( f"Found no pose for the robot to grasp the object: {self.object_designator} with arm: {self.arm}") diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 85cd00e0b..8272d0515 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -25,7 +25,7 @@ try: from ..worlds import Multiverse except ImportError: - Multiverse = NoneType + Multiverse = type(None) try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 747416bf8..4cb72f5be 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -56,12 +56,12 @@ class Multiverse(World): Add the MJCF description extension to the extension to description type mapping for the objects. """ - def __init__(self, is_prospection_world: Optional[bool] = False, + def __init__(self, is_prospection: Optional[bool] = False, clear_cache: bool = False): """ Initialize the Multiverse Socket and the PyCram World. - :param is_prospection_world: Whether the world is prospection or not. + :param is_prospection: Whether the world is prospection or not. :param clear_cache: Whether to clear the cache or not. """ @@ -69,9 +69,9 @@ def __init__(self, is_prospection_world: Optional[bool] = False, self.saved_simulator_states: Dict = {} self.make_sure_multiverse_resources_are_added(clear_cache=clear_cache) - self.simulation = self.conf.prospection_world_prefix if is_prospection_world else "belief_state" + self.simulation = self.conf.prospection_world_prefix if is_prospection else "belief_state" self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) - self._init_clients(is_prospection=is_prospection_world) + self._init_clients(is_prospection=is_prospection) World.__init__(self, mode=WorldMode.DIRECT, is_prospection=is_prospection) diff --git a/test/test_task_tree.py b/test/test_task_tree.py index 26fd3002d..8d7fcaca3 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -35,8 +35,8 @@ def test_tree_creation(self): # self.tearDownBulletWorld() tt = pycram.tasktree.task_tree - self.assertEqual(16, len(tt.root)) - self.assertEqual(11, len(tt.root.leaves)) + self.assertEqual(15, len(tt.root)) + self.assertEqual(10, len(tt.root.leaves)) # check that all nodes succeeded for node in anytree.PreOrderIter(tt.root): @@ -74,8 +74,8 @@ def test_simulated_tree(self): self.plan() tt = pycram.tasktree.task_tree - self.assertEqual(16, len(tt.root)) - self.assertEqual(11, len(tt.root.leaves)) + self.assertEqual(15, len(tt.root)) + self.assertEqual(10, len(tt.root.leaves)) self.assertEqual(len(pycram.tasktree.task_tree), 1) From 17301e56fb6246c3e76f4e531de24fb706c6438a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Dec 2024 17:08:55 +0100 Subject: [PATCH 41/49] [Multiverse] Removed extra lock release in clients. --- src/pycram/worlds/multiverse_communication/clients.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 7bca176f6..e02ee92aa 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -485,7 +485,6 @@ def send_data_to_server(self, data: List, if receive_meta_data: self.request_meta_data["receive"] = receive_meta_data self.send_and_receive_meta_data() - self.lock.release() self.send_data = data self.send_and_receive_data() response_meta_data = self.response_meta_data From 5068e125058a97cbeecfed346151c3ca1b57bbba Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Dec 2024 17:16:56 +0100 Subject: [PATCH 42/49] [HasConcept] added the has concept as parent to PhysicalBody --- src/pycram/datastructures/mixins.py | 2 +- src/pycram/datastructures/world_entity.py | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/pycram/datastructures/mixins.py b/src/pycram/datastructures/mixins.py index 5ff4dc555..eef06e1cb 100644 --- a/src/pycram/datastructures/mixins.py +++ b/src/pycram/datastructures/mixins.py @@ -19,4 +19,4 @@ class HasConcept: """ def __init__(self): - self.ontology_individual = self.ontology_concept() \ No newline at end of file + self.ontology_individual = self.ontology_concept() diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 8b0185781..1a1bf9296 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -11,6 +11,7 @@ from pycrap import PhysicalObject from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, PhysicalBodyState, \ AxisAlignedBoundingBox, RotatedBoundingBox +from .mixins import HasConcept from ..local_transformer import LocalTransformer from ..ros.data_types import Time @@ -155,7 +156,7 @@ def __hash__(self) -> int: return hash((self.id, self.name, self.parent_entity)) -class PhysicalBody(WorldEntity, ABC): +class PhysicalBody(WorldEntity, HasConcept, ABC): """ A class that represents a physical body in the world that has some related physical properties. """ @@ -167,6 +168,7 @@ class PhysicalBody(WorldEntity, ABC): def __init__(self, body_id: int, world: World): WorldEntity.__init__(self, body_id, world) + HasConcept.__init__(self) self.local_transformer = LocalTransformer() self._is_translating: Optional[bool] = None self._is_rotating: Optional[bool] = None From b7c951ac407bcbae1320eda0e80ed8021e31e880 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Dec 2024 17:35:27 +0100 Subject: [PATCH 43/49] [MultiverseFallSchoolDemo] merged changes from dev. --- demos/pycram_multiverse_demo/fallschool_demo.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/demos/pycram_multiverse_demo/fallschool_demo.py b/demos/pycram_multiverse_demo/fallschool_demo.py index 22ebacdff..d60276976 100644 --- a/demos/pycram_multiverse_demo/fallschool_demo.py +++ b/demos/pycram_multiverse_demo/fallschool_demo.py @@ -13,7 +13,6 @@ from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ LookAtAction, DetectAction from pycram.designators.object_designator import BelieveObject -from pycram.failures import PerceptionObjectNotFound from pycram.process_module import simulated_robot, with_simulated_robot, real_robot from pycram.ros_utils.robot_state_updater import WorldStateUpdater from pycram.world_concepts.world_object import Object @@ -23,10 +22,6 @@ from pycrap import PhysicalObject -rospy_logger = logging.getLogger('rosout') -rospy_logger.setLevel(logging.DEBUG) - - @with_simulated_robot def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() @@ -64,8 +59,6 @@ def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): fridge_base_pose.position.x += 0.16 fridge_base_pose.position.y += -0.1 milk.set_pose(fridge_base_pose, base=True) -print(milk.get_position_as_list()) -print(milk.get_orientation_as_list()) robot_desig = BelieveObject(names=[robot.name]) @@ -83,10 +76,7 @@ def move_and_detect(obj_type: Type[PhysicalObject], pick_pose: Pose): LookAtAction(targets=[Pose(milk.get_position_as_list())]).resolve().perform() - try: - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - except PerceptionObjectNotFound: - milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() TransportAction(milk_desig, [Pose([2.4, 3, 1.02])], [Arms.LEFT]).resolve().perform() From 89c162dd9a9cf64d8f36ff18a47637bc601dd516 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Dec 2024 17:37:38 +0100 Subject: [PATCH 44/49] [Multiverse] removed unused dataclasses. --- src/pycram/datastructures/dataclasses.py | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 4530b7e35..68ff5b3a3 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -944,15 +944,6 @@ def get_axes(self) -> Dict[str, Point]: return {getattr(self, field.name).name: getattr(self, field.name).axes for field in fields(self)} -@dataclass -class MultiverseBoundingBox: - """ - Dataclass for storing the bounding box of a body in the Multiverse simulation. - """ - min_point: List[float] - max_point: List[float] - - @dataclass class MultiverseMetaData: """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" @@ -982,15 +973,6 @@ def intersected(self) -> bool: return self.distance >= 0 and self.body_name != "" -@dataclass -class MultiverseObjectContactData: - """ - A dataclass to store all the contact data returned from Multiverse for a single object. - """ - body_names: List[str] - data: List[MultiverseContactPoint] - - @dataclass class MultiverseContactPoint: """ From 497ad03efb303a92e8c1d3ee3414f9719a1d6b62 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Dec 2024 17:40:52 +0100 Subject: [PATCH 45/49] [Giskard] removed unused import. --- src/pycram/external_interfaces/giskard.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index de6f704dd..c594660d6 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -22,7 +22,6 @@ try: from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry - from giskard_msgs.srv import UpdateWorld except ModuleNotFoundError as e: logwarn("Failed to import Giskard messages, the real robot will not be available") From 3f1ee359e619bd6400de6a50c2db39085281a059 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Dec 2024 17:42:06 +0100 Subject: [PATCH 46/49] [PR2ProcessModules] removed dublicate import. --- src/pycram/process_modules/pr2_process_modules.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 8272d0515..e9909a20f 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -29,7 +29,6 @@ try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 - from control_msgs.msg import GripperCommandGoal, GripperCommandAction except ImportError: logdebug("Pr2GripperCommandGoal not found") From 898a42c5c71e7f57ed8ee0f5dfda1da5f5d32f7a Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 15 Dec 2024 23:25:48 +0100 Subject: [PATCH 47/49] [PhysicalBody] removed duplicate method, added a normal property for contact points. --- src/pycram/datastructures/dataclasses.py | 19 ++++++++++++++++--- src/pycram/description.py | 13 ------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 68ff5b3a3..7a63668f5 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -710,6 +710,10 @@ class ContactPoint: lateral_friction_1: Optional[LateralFriction] = None lateral_friction_2: Optional[LateralFriction] = None + @property + def normal(self) -> List[float]: + return self.normal_on_body_b + def __str__(self): return f"ContactPoint: {self.body_a.name} - {self.body_b.name}" @@ -795,7 +799,7 @@ def get_links_in_contact_of_object(self, obj: Object) -> List[PhysicalBody]: :param obj: An instance of the Object class that represents the object. :return: A list of Link instances that represent the links in contact of the object. """ - return [point.body_b for point in self if point.body_b.parent == obj] + return [point.body_b for point in self if point.body_b.parent_entity == obj] def get_points_of_object(self, obj: Object) -> 'ContactPointsList': """ @@ -813,7 +817,16 @@ def get_points_of_link(self, link: Link) -> 'ContactPointsList': :param link: An instance of the Link class that represents the link that the points are related to. :return: A ContactPointsList instance that represents the contact points of the link. """ - return ContactPointsList([point for point in self if link == point.body_b]) + return self.get_points_of_body(link) + + def get_points_of_body(self, body: PhysicalBody) -> 'ContactPointsList': + """ + Get the points of the body. + + :param body: An instance of the PhysicalBody class that represents the body that the points are related to. + :return: A ContactPointsList instance that represents the contact points of the body. + """ + return ContactPointsList([point for point in self if body == point.body_b]) def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]: """ @@ -860,7 +873,7 @@ def get_objects_that_have_points(self) -> List[Object]: :return: A list of Object instances that represent the objects that have points in the list. """ - return list({point.body_b.parent for point in self if isinstance(point.body_b.parent, Object)}) + return list({point.body_b.parent_entity for point in self}) def __str__(self): return f"ContactPointsList: {', '.join([point.__str__() for point in self])}" diff --git a/src/pycram/description.py b/src/pycram/description.py index 3c0c5df94..ff14c1f91 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -253,19 +253,6 @@ def get_axis_aligned_bounding_box_from_geometry(self) -> AxisAlignedBoundingBox: if isinstance(geom, MeshVisualShape) else geom.get_axis_aligned_bounding_box() return bounding_box - def get_axis_aligned_bounding_box_from_geometry(self) -> AxisAlignedBoundingBox: - if isinstance(self.geometry, List): - all_boxes = [geom.get_axis_aligned_bounding_box(self.get_mesh_path(geom)) - if isinstance(geom, MeshVisualShape) else geom.get_axis_aligned_bounding_box() - for geom in self.geometry - ] - bounding_box = AxisAlignedBoundingBox.from_multiple_bounding_boxes(all_boxes) - else: - geom = self.geometry - bounding_box = geom.get_axis_aligned_bounding_box(self.get_mesh_path(geom)) \ - if isinstance(geom, MeshVisualShape) else geom.get_axis_aligned_bounding_box() - return bounding_box - def get_convex_hull(self) -> Geometry3D: """ :return: The convex hull of the link geometry. From 3277e0cc8f1a604b6e11651a5ddd6218b474e8d6 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 17 Dec 2024 00:07:08 +0100 Subject: [PATCH 48/49] [PhysicalBody] Added Supporter type in pycrap. --- src/pycrap/objects.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/pycrap/objects.py b/src/pycrap/objects.py index ab49ed20c..e78e33051 100644 --- a/src/pycrap/objects.py +++ b/src/pycrap/objects.py @@ -96,7 +96,13 @@ class Cereal(Food): """ -class Floor(PhysicalObject): +class Supporter(PhysicalObject): + """ + An object that supports another object. + """ + + +class Floor(Supporter): """ The lower surface of a room. """ From 5da450f3576a36493f37768d597228b2babe5d75 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 19 Dec 2024 11:08:59 +0100 Subject: [PATCH 49/49] [PhysicalBody] minor review changes. --- .../demo_euROBIN_industrial_robotics.py | 57 +------------------ src/pycram/datastructures/dataclasses.py | 51 ++++++++--------- src/pycram/datastructures/world_entity.py | 4 +- 3 files changed, 29 insertions(+), 83 deletions(-) diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py index 44272a030..7269caea3 100644 --- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py +++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py @@ -1,7 +1,6 @@ import pycrap -from pycram.datastructures.enums import GripperState, Arms -from pycram.datastructures.world import UseProspectionWorld -from pycram.process_module import simulated_robot, real_robot +from pycram.datastructures.enums import GripperState +from pycram.process_module import real_robot from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose from pycram.worlds.multiverse import Multiverse @@ -30,56 +29,4 @@ with real_robot: SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform() - world.exit() -import logging -import os - -from rospkg import RosPack - -from pycram.datastructures.enums import ObjectType, GripperState, Arms -from pycram.process_module import simulated_robot, real_robot -from pycram.world_concepts.world_object import Object -from pycram.datastructures.pose import Pose -from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor -from pycram.ros_utils.joint_state_publisher import JointStatePublisher -from pycram.worlds.multiverse import Multiverse -from pycram.designators.action_designator import SetGripperAction - -SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__)) -PYCRAM_DIR = os.path.join(SCRIPT_DIR, os.pardir, os.pardir) -RESOURCE_DIR = os.path.join(PYCRAM_DIR, "resources") - -SPAWNING_POSES = { - "robot": Pose([0, 0, 0], [0.0, 0.0, 0.0, 1.0]), # x,y,z,qx,qy,qz,qw - "cereal": Pose([0.5, 0.5, 2.0], [0.0, 0.0, 0.0, 1.0]) -} - - -def spawn_ur5e_with_gripper(): - robot = Object("ur5e", ObjectType.ROBOT, "universal_robot/ur5e/urdf/ur5e.urdf") - gripper = Object("gripper-2F-85", ObjectType.GRIPPER, "robotiq/gripper-2F-85/gripper-2F-85.urdf") - wrist_3_tf_frame = robot.get_link_tf_frame("wrist_3_link") - gripper.set_pose(Pose([0, 0.1, 0], [1.0, 0.0, 0.0, -1.0], frame=wrist_3_tf_frame)) - robot.attach(gripper, parent_link="wrist_3_link") - return robot, gripper - - -if __name__ == '__main__': - root = logging.getLogger() - root.setLevel(logging.INFO) - - world = Multiverse(simulation_name="ur5e_with_task_board") - - # Load environment, robot and objects - rospack = RosPack() - - robot, gripper = spawn_ur5e_with_gripper() - - jsp = JointStatePublisher() - # fts = ForceTorqueSensor("ee_fixed_joint") - robot_arms = [chain.arm_type for chain in robot.robot_description.get_manipulator_chains()] - with real_robot: - SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform() - - world.simulate(1) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 7a63668f5..9d5578c51 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -187,7 +187,7 @@ def from_origin_and_half_extents(cls, origin: Point, half_extents: Point): return cls.from_min_max(min_point, max_point) @classmethod - def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> 'AxisAlignedBoundingBox': + def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> AxisAlignedBoundingBox: """ Set the axis-aligned bounding box from multiple axis-aligned bounding boxes. @@ -201,7 +201,7 @@ def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBo max_z = max([box.max_z for box in bounding_boxes]) return cls(min_x, min_y, min_z, max_x, max_y, max_z) - def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox': + def get_transformed_box(self, transform: Transform) -> AxisAlignedBoundingBox: """ Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box. @@ -250,7 +250,7 @@ def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float], tr @classmethod def from_axis_aligned_bounding_box(cls, axis_aligned_bounding_box: AxisAlignedBoundingBox, - transform: Transform) -> 'RotatedBoundingBox': + transform: Transform) -> RotatedBoundingBox: """ Set the rotated bounding box from an axis-aligned bounding box and a transformation. @@ -491,14 +491,14 @@ class PhysicalBodyState(State): acceptable_pose_error: Tuple[float, float] = (0.001, 0.001) acceptable_velocity_error: Tuple[float, float] = (0.001, 0.001) - def __eq__(self, other: 'PhysicalBodyState'): + def __eq__(self, other: PhysicalBodyState): return (self.pose_is_almost_equal(other) and self.is_translating == other.is_translating and self.is_rotating == other.is_rotating and self.velocity_is_almost_equal(other) ) - def pose_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: + def pose_is_almost_equal(self, other: PhysicalBodyState) -> bool: """ Check if the pose of the object is almost equal to the pose of another object. @@ -507,7 +507,7 @@ def pose_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: """ return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1]) - def velocity_is_almost_equal(self, other: 'PhysicalBodyState') -> bool: + def velocity_is_almost_equal(self, other: PhysicalBodyState) -> bool: """ Check if the velocity of the object is almost equal to the velocity of another object. @@ -547,12 +547,12 @@ class LinkState(State): body_state: PhysicalBodyState constraint_ids: Dict[Link, int] - def __eq__(self, other: 'LinkState'): + def __eq__(self, other: LinkState): return (self.body_state == other.body_state and self.all_constraints_exist(other) and self.all_constraints_are_equal(other)) - def all_constraints_exist(self, other: 'LinkState') -> bool: + def all_constraints_exist(self, other: LinkState) -> bool: """ Check if all constraints exist in the other link state. @@ -562,7 +562,7 @@ def all_constraints_exist(self, other: 'LinkState') -> bool: return (all([cid_k in other.constraint_ids.keys() for cid_k in self.constraint_ids.keys()]) and len(self.constraint_ids.keys()) == len(other.constraint_ids.keys())) - def all_constraints_are_equal(self, other: 'LinkState') -> bool: + def all_constraints_are_equal(self, other: LinkState) -> bool: """ Check if all constraints are equal to the ones in the other link state. @@ -584,7 +584,7 @@ class JointState(State): position: float acceptable_error: float - def __eq__(self, other: 'JointState'): + def __eq__(self, other: JointState): error = calculate_joint_position_error(self.position, other.position) return is_error_acceptable(error, other.acceptable_error) @@ -602,7 +602,7 @@ class ObjectState(State): link_states: Dict[int, LinkState] joint_states: Dict[int, JointState] - def __eq__(self, other: 'ObjectState'): + def __eq__(self, other: ObjectState): return (self.body_state == other.body_state and self.all_attachments_exist(other) and self.all_attachments_are_equal(other) and self.link_states == other.link_states @@ -612,7 +612,7 @@ def __eq__(self, other: 'ObjectState'): def pose(self) -> Pose: return self.body_state.pose - def all_attachments_exist(self, other: 'ObjectState') -> bool: + def all_attachments_exist(self, other: ObjectState) -> bool: """ Check if all attachments exist in the other object state. @@ -622,7 +622,7 @@ def all_attachments_exist(self, other: 'ObjectState') -> bool: return (all([att_k in other.attachments.keys() for att_k in self.attachments.keys()]) and len(self.attachments.keys()) == len(other.attachments.keys())) - def all_attachments_are_equal(self, other: 'ObjectState') -> bool: + def all_attachments_are_equal(self, other: ObjectState) -> bool: """ Check if all attachments are equal to the ones in the other object state. @@ -632,10 +632,9 @@ def all_attachments_are_equal(self, other: 'ObjectState') -> bool: return all([att == other_att for att, other_att in zip(self.attachments.values(), other.attachments.values())]) def __copy__(self): - return ObjectState(pose=self.pose.copy(), attachments=copy(self.attachments), + return ObjectState(body_state=self.body_state.copy(), attachments=copy(self.attachments), link_states=copy(self.link_states), - joint_states=copy(self.joint_states), - acceptable_pose_error=deepcopy(self.acceptable_pose_error)) + joint_states=copy(self.joint_states)) @dataclass @@ -646,11 +645,11 @@ class WorldState(State): object_states: Dict[str, ObjectState] simulator_state_id: Optional[int] = None - def __eq__(self, other: 'WorldState'): + def __eq__(self, other: WorldState): return (self.simulator_state_is_equal(other) and self.all_objects_exist(other) and self.all_objects_states_are_equal(other)) - def simulator_state_is_equal(self, other: 'WorldState') -> bool: + def simulator_state_is_equal(self, other: WorldState) -> bool: """ Check if the simulator state is equal to the simulator state of another world state. @@ -659,7 +658,7 @@ def simulator_state_is_equal(self, other: 'WorldState') -> bool: """ return self.simulator_state_id == other.simulator_state_id - def all_objects_exist(self, other: 'WorldState') -> bool: + def all_objects_exist(self, other: WorldState) -> bool: """ Check if all objects exist in the other world state. @@ -669,7 +668,7 @@ def all_objects_exist(self, other: 'WorldState') -> bool: return (all([obj_name in other.object_states.keys() for obj_name in self.object_states.keys()]) and len(self.object_states.keys()) == len(other.object_states.keys())) - def all_objects_states_are_equal(self, other: 'WorldState') -> bool: + def all_objects_states_are_equal(self, other: WorldState) -> bool: """ Check if all object states are equal to the ones in the other world state. @@ -732,7 +731,7 @@ class ContactPointsList(list): A list of contact points. """ - def get_bodies_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]: + def get_bodies_that_got_removed(self, previous_points: ContactPointsList) -> List[PhysicalBody]: """ Return the bodies that are not in the current points list but were in the initial points list. @@ -801,7 +800,7 @@ def get_links_in_contact_of_object(self, obj: Object) -> List[PhysicalBody]: """ return [point.body_b for point in self if point.body_b.parent_entity == obj] - def get_points_of_object(self, obj: Object) -> 'ContactPointsList': + def get_points_of_object(self, obj: Object) -> ContactPointsList: """ Get the points of the object. @@ -810,7 +809,7 @@ def get_points_of_object(self, obj: Object) -> 'ContactPointsList': """ return ContactPointsList([point for point in self if self.is_body_in_object(point.body_b, obj)]) - def get_points_of_link(self, link: Link) -> 'ContactPointsList': + def get_points_of_link(self, link: Link) -> ContactPointsList: """ Get the points of the link. @@ -819,7 +818,7 @@ def get_points_of_link(self, link: Link) -> 'ContactPointsList': """ return self.get_points_of_body(link) - def get_points_of_body(self, body: PhysicalBody) -> 'ContactPointsList': + def get_points_of_body(self, body: PhysicalBody) -> ContactPointsList: """ Get the points of the body. @@ -828,7 +827,7 @@ def get_points_of_body(self, body: PhysicalBody) -> 'ContactPointsList': """ return ContactPointsList([point for point in self if body == point.body_b]) - def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]: + def get_objects_that_got_removed(self, previous_points: ContactPointsList) -> List[Object]: """ Return the object that is not in the current points list but was in the initial points list. @@ -839,7 +838,7 @@ def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> current_objects_in_contact = self.get_objects_that_have_points() return [obj for obj in initial_objects_in_contact if obj not in current_objects_in_contact] - def get_new_objects(self, previous_points: 'ContactPointsList') -> List[Object]: + def get_new_objects(self, previous_points: ContactPointsList) -> List[Object]: """ Return the object that is not in the initial points list but is in the current points list. diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index 1a1bf9296..fe9756bf2 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -325,7 +325,7 @@ def contact_points(self) -> ContactPointsList: """ return self.world.get_body_contact_points(self) - def get_contact_points_with_body(self, body: 'PhysicalBody') -> ContactPointsList: + def get_contact_points_with_body(self, body: PhysicalBody) -> ContactPointsList: """ :param body: The body to get the contact points with. :return: The contact points of this body with the given body. @@ -340,7 +340,7 @@ def closest_points(self, max_distance: float) -> ClosestPointsList: """ return self.world.get_body_closest_points(self, max_distance) - def get_closest_points_with_body(self, body: 'PhysicalBody', max_distance: float) -> ClosestPointsList: + def get_closest_points_with_body(self, body: PhysicalBody, max_distance: float) -> ClosestPointsList: """ :param body: The body to get the points with. :param max_distance: The maximum distance to consider a body as close, only points closer than or equal to this