From 18d6b8ed6780c0cafd8ed3fb3d8cf34c89a125bb Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 21 Jul 2024 23:38:22 +0200 Subject: [PATCH 01/21] [Multiverse] Implementing goal validation for multiverse. --- src/pycram/datastructures/world.py | 15 +- src/pycram/description.py | 72 +++++-- src/pycram/object_descriptors/urdf.py | 9 +- src/pycram/world_concepts/constraints.py | 43 +++- src/pycram/world_concepts/world_object.py | 58 ++++- src/pycram/worlds/bullet_world.py | 6 +- src/pycram/worlds/multiverse.py | 186 ++++++++++++---- .../client_manager.py | 10 +- .../multiverse_communication/clients.py | 153 ++++++++++--- .../worlds/multiverse_functions/__init__.py | 0 .../multiverse_functions/goal_validator.py | 201 ++++++++++++++++++ test/test_multiverse.py | 6 +- 12 files changed, 641 insertions(+), 118 deletions(-) create mode 100644 src/pycram/worlds/multiverse_functions/__init__.py create mode 100644 src/pycram/worlds/multiverse_functions/goal_validator.py diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index ec061d7b3..e2dbbdf23 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -137,7 +137,7 @@ class World(StateEntity, ABC): the objects. """ - data_directory: List[str] = [resources_path, os.path.join(resources_path, 'robots')] + data_directory: List[str] = [resources_path] """ Global reference for the data directories, this is used to search for the description files of the robot, the objects, and the cached files. @@ -498,7 +498,7 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: goal = self.get_move_base_joint_goal(pose) self.robot.set_joint_positions(goal) - def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: + def get_move_base_joint_goal(self, pose: Pose) -> Dict[Joint, float]: """ Get the goal for the move base joints of a mobile robot to reach a target pose. param pose: The target pose. @@ -508,9 +508,9 @@ def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: angle_diff = self.get_z_angle_diff(self.robot.get_orientation_as_list(), pose.orientation_as_list()) # Get the joints of the base link move_base_joints = self.get_move_base_joints() - return {move_base_joints.translation_x: position_diff[0], - move_base_joints.translation_y: position_diff[1], - move_base_joints.angular_z: angle_diff} + return {self.robot.joints[move_base_joints.translation_x]: position_diff[0], + self.robot.joints[move_base_joints.translation_y]: position_diff[1], + self.robot.joints[move_base_joints.angular_z]: angle_diff} @staticmethod def get_move_base_joints() -> VirtualMoveBaseJoints: @@ -1196,11 +1196,10 @@ def __del__(self): self.exit() @abstractmethod - def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: """ Set the positions of multiple joints of an articulated object. - :param obj: The object. - :param joint_poses: A dictionary with joint names as keys and joint positions as values. + :param joint_positions: A dictionary with joint objects as keys and joint positions as values. """ pass diff --git a/src/pycram/description.py b/src/pycram/description.py index c19c11b3f..83f796ac2 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -62,8 +62,9 @@ class JointDescription(EntityDescription): A class that represents the description of a joint. """ - def __init__(self, parsed_joint_description: Optional[Any] = None): + def __init__(self, parsed_joint_description: Optional[Any] = None, is_virtual: bool = False): self.parsed_description = parsed_joint_description + self.is_virtual: Optional[bool] = is_virtual @property @abstractmethod @@ -158,6 +159,13 @@ def __init__(self, _id: int, obj: Object): WorldEntity.__init__(self, _id, obj.world) self.object: Object = obj + @property + def object_name(self) -> str: + """ + The name of the object to which this joint belongs. + """ + return self.object.name + @property @abstractmethod def pose(self) -> Pose: @@ -205,6 +213,27 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): self.constraint_ids: Dict[Link, int] = {} self._update_pose() + def set_pose(self, pose: Pose) -> None: + """ + Sets 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 + that can allow the link to be at the given pose. + :param pose: The target pose for this link. + """ + self.object.set_pose(self.get_object_pose_given_link_pose(pose)) + + def get_object_pose_given_link_pose(self, pose): + return (pose.to_transform(self.tf_frame) * self.get_transform_to_root_link()).to_pose() + + def get_pose_given_object_pose(self, pose): + return (pose.to_transform(self.object.tf_frame) * self.get_transform_from_root_link()).to_pose() + + def get_transform_from_root_link(self) -> Transform: + return self.get_transform_from_link(self.object.root_link) + + def get_transform_to_root_link(self) -> Transform: + return self.get_transform_to_link(self.object.root_link) + @property def current_state(self) -> LinkState: return LinkState(self.constraint_ids.copy()) @@ -213,17 +242,18 @@ def current_state(self) -> LinkState: def current_state(self, link_state: LinkState) -> None: self.constraint_ids = link_state.constraint_ids - def add_fixed_constraint_with_link(self, child_link: 'Link', transform: Optional[Transform] = None) -> int: + def add_fixed_constraint_with_link(self, child_link: 'Link', + child_to_parent_transform: Optional[Transform] = None) -> int: """ Adds a fixed constraint between this link and the given link, used to create attachments for example. :param child_link: The child link to which a fixed constraint should be added. - :param transform: The transformation between the two links. + :param child_to_parent_transform: The transformation between the two links. :return: The unique id of the constraint. """ - if transform is None: - transform = child_link.get_transform_from_link(self) - constraint_id = self.world.add_fixed_constraint(self, child_link, transform) + if child_to_parent_transform is None: + child_to_parent_transform = child_link.get_transform_to_link(self) + constraint_id = self.world.add_fixed_constraint(self, child_link, child_to_parent_transform) self.constraint_ids[child_link] = constraint_id child_link.constraint_ids[self] = constraint_id return constraint_id @@ -257,17 +287,6 @@ def is_root(self) -> bool: """ return self.object.get_root_link_id() == self.id - def set_pose(self, pose: Pose) -> None: - """ - Sets 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 - that can allow the link to be at the given pose. - :param pose: The target pose for this link. - """ - self.object.set_pose( - (pose.to_transform(self.tf_frame) * self.get_transform_to_link(self.object.root_link)).to_pose() - ) - def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: """ Updates the transformation of this link at the given time. @@ -451,9 +470,9 @@ class Joint(ObjectEntity, JointDescription, ABC): def __init__(self, _id: int, joint_description: JointDescription, - obj: Object): + obj: Object, is_virtual: Optional[bool] = False): ObjectEntity.__init__(self, _id, obj) - JointDescription.__init__(self, joint_description.parsed_description) + JointDescription.__init__(self, joint_description.parsed_description, is_virtual) self._update_position() @property @@ -598,10 +617,22 @@ def __init__(self, path: Optional[str] = None): else: self._parsed_description = None + self.virtual_joint_names: List[str] = [] + + def is_joint_virtual(self, name: str) -> bool: + """ + Returns whether the joint with the given name is virtual. + + :param name: The name of the joint. + :return: True if the joint is virtual, False otherwise. + """ + return name in self.virtual_joint_names + @abstractmethod def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, - lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: """ Adds a joint to this object. @@ -613,6 +644,7 @@ def add_joint(self, name: str, child: str, joint_type: JointType, :param origin: The origin of the joint. :param lower_limit: The lower limit of the joint. :param upper_limit: The upper limit of the joint. + :param is_virtual: True if the joint is virtual, False otherwise. """ pass diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 7288df776..c5b51fb60 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -81,8 +81,8 @@ class JointDescription(AbstractJointDescription): pycram_type_map = {pycram_type: urdf_type for urdf_type, pycram_type in urdf_type_map.items()} - def __init__(self, urdf_description: urdf.Joint): - super().__init__(urdf_description) + def __init__(self, urdf_description: urdf.Joint, is_virtual: Optional[bool] = False): + super().__init__(urdf_description, is_virtual=is_virtual) @property def origin(self) -> Pose: @@ -176,7 +176,8 @@ class Joint(AbstractObjectDescription.Joint, JointDescription): def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, - lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: if lower_limit is not None or upper_limit is not None: limit = urdf.JointLimit(lower=lower_limit, upper=upper_limit) else: @@ -195,6 +196,8 @@ def add_joint(self, name: str, child: str, joint_type: JointType, JointDescription.pycram_type_map[joint_type], axis, origin, limit) self.parsed_description.add_joint(joint) + if is_virtual: + self.virtual_joint_names.append(name) def load_description(self, path) -> URDF: with open(path, 'r') as file: diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index ff4514e0d..955336925 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -30,6 +30,38 @@ def __init__(self, self.child_to_constraint = child_to_constraint self._parent_to_child = None + def get_child_object_pose_given_parent(self, pose: Pose) -> Pose: + """ + Returns the pose of the child object given the parent pose. + + :param pose: The parent object pose. + :return: The pose of the child object. + """ + pose = self.parent_link.get_pose_given_object_pose(pose) + child_link_pose = self.get_child_link_target_pose_given_parent(pose) + return self.child_link.get_object_pose_given_link_pose(child_link_pose) + + def set_child_link_pose(self): + """ + Sets 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()) + + def get_child_link_target_pose(self) -> Pose: + """ + Returns the target pose of the child object. (The pose of the child object in the parent object frame) + """ + return self.parent_to_child_transform.to_pose() + + def get_child_link_target_pose_given_parent(self, parent_pose: Pose) -> Pose: + """ + Returns the target pose of the child object link given the parent link pose. + + :param parent_pose: The parent link pose. + :return: The target pose of the child object link. + """ + return (parent_pose.to_transform(self.parent_link.tf_frame) * self.parent_to_child_transform).to_pose() + @property def parent_to_child_transform(self) -> Union[Transform, None]: if self._parent_to_child is None: @@ -190,6 +222,14 @@ def __init__(self, if self.id is None: self.add_fixed_constraint() + @property + def parent_object(self): + return self.parent_link.object + + @property + def child_object(self): + return self.child_link.object + def update_transform_and_constraint(self) -> None: """ Updates the transform and constraint of this attachment. @@ -214,7 +254,8 @@ def add_fixed_constraint(self) -> None: """ Adds a fixed constraint between the parent link and the child link. """ - self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link, self.parent_to_child_transform) + self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link, + self.parent_to_child_transform.invert()) def calculate_transform(self) -> Transform: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 11405ee4c..f64baa339 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -92,6 +92,31 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + + def get_target_poses_of_attached_objects(self) -> Dict[Object, Pose]: + """ + Get the target poses of the attached objects. + return: The target poses of the attached objects + """ + return self.get_target_poses_of_attached_objects_given_parent(self.get_pose()) + + def get_poses_of_attached_objects(self) -> Dict[Object, Pose]: + """ + Get the poses of the attached objects. + return: The poses of the attached objects + """ + return {child_object: attachment.get_child_object_pose() + for child_object, attachment in self.attachments.items() if not attachment.loose} + + def get_target_poses_of_attached_objects_given_parent(self, pose: Pose) -> Dict[Object, Pose]: + """ + Get the target poses of the attached objects of an object. Given the pose of the parent object. + param pose: The pose of the parent object. + return: The target poses of the attached objects + """ + return {child_object: attachment.get_child_object_pose_given_parent(pose) for child_object, attachment + in self.attachments.items() if not attachment.loose} + @property def pose(self): return self.get_pose() @@ -195,8 +220,20 @@ def _init_joints(self): """ self.joints = {} for joint_name, joint_id in self.joint_name_to_id.items(): - joint_description = self.description.get_joint_by_name(joint_name) - self.joints[joint_name] = self.description.Joint(joint_id, joint_description, self) + parsed_joint_description = self.description.get_joint_by_name(joint_name) + is_virtual = self.is_joint_virtual(joint_name) + self.joints[joint_name] = self.description.Joint(joint_id, parsed_joint_description, self, is_virtual) + + def is_joint_virtual(self, name: str): + return self.description.is_joint_virtual(name) + + @property + def virtual_joint_names(self): + return self.description.virtual_joint_names + + @property + def virtual_joints(self): + return [joint for joint in self.joints.values() if joint.is_virtual] @property def has_one_link(self) -> bool: @@ -420,7 +457,7 @@ def attach(self, if coincide_the_objects: parent_to_child_transform = Transform() else: - parent_to_child_transform = None + parent_to_child_transform = parent_link.get_transform_to_link(child_link) attachment = Attachment(parent_link, child_link, bidirectional, parent_to_child_transform) self.attachments[child_object] = attachment @@ -694,8 +731,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec child.update_attachment_with_object(self) else: - link_to_object = attachment.parent_to_child_transform - child.set_pose(link_to_object.to_pose(), set_attachments=False) + child.set_pose(attachment.get_child_link_target_pose(), set_attachments=False) child._set_attached_objects_poses(already_moved_objects + [self]) def set_position(self, position: Union[Pose, Point, List], base=False) -> None: @@ -833,21 +869,23 @@ def reset_all_joints_positions(self) -> None: """ Sets the current position of all joints to 0. This is useful if the joints should be reset to their default """ - joint_names = list(self.joint_name_to_id.keys()) + joint_names = [joint.name for joint in self.joints.values() if not joint.is_virtual] if len(joint_names) == 0: return joint_positions = [0] * len(joint_names) self.set_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_poses: dict) -> None: + def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. - :param joint_poses: + :param joint_positions: A dictionary with the joint names as keys and the target positions as values. """ - self.world.set_multiple_joint_positions(self, joint_poses) - # self.update_pose() + joint_positions = {self.joints[joint_name]: joint_position + for joint_name, joint_position in joint_positions.items()} + self.world.set_multiple_joint_positions(joint_positions) + self.update_pose() self._update_all_links_poses() self.update_link_transforms() self._set_attached_objects_poses() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 7af5ca325..f18753979 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -167,9 +167,9 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} - def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: - for joint_name, joint_position in joint_poses.items(): - self.reset_joint_position(obj.joints[joint_name], joint_position) + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + for joint, joint_position in joint_positions.items(): + self.reset_joint_position(joint, joint_position) def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: float) -> None: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index cccb57320..7971a0850 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,11 +1,14 @@ import logging import os +from time import sleep, time import numpy as np from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional +from typing_extensions import List, Dict, Optional, Iterable, Tuple from .multiverse_communication.client_manager import MultiverseClientManager +from .multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ + MultiPoseGoalValidator, JointGoalValidator, MultiJointGoalValidator from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -40,6 +43,28 @@ class Multiverse(World): the multiverse configuration file). """ + try: + simulation_wait_time_factor = float(os.environ['Multiverse_Simulation_Wait_Time_Factor']) + except KeyError: + simulation_wait_time_factor = 1.0 + """ + The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for + the time taken by the simulation to process the request, this depends on the computational power of the machine + running the simulation. + TODO: This should be replaced by a feedback mechanism that waits until a certain condition is met, e.g. the action + is completed. + """ + + position_tol: float = 2e-3 + """ + The tolerance for position comparison. (e.g. for checking if the object has reached the desired position) + """ + + orientation_tol: float = 2e-3 + """ + The tolerance for orientation comparison. (e.g. for checking if the object has reached the desired orientation) + """ + def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: Optional[float] = 60.0, @@ -50,6 +75,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, param is_prospection: Whether the world is prospection or not. param simulation_frequency: The frequency of the simulation. param client_addr: The address of the multiverse client. + param simulation: The name of the simulation. """ self._make_sure_multiverse_resources_are_added() @@ -74,7 +100,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self._spawn_floor() def _init_clients(self): - client_manager = MultiverseClientManager() + client_manager = MultiverseClientManager(self.simulation_wait_time_factor) self.reader = client_manager.create_reader(is_prospection_world=self.is_prospection_world) self.writer = client_manager.create_writer(self.simulation, is_prospection_world=self.is_prospection_world) self.api_requester = client_manager.create_api_requester(self.simulation, @@ -102,8 +128,7 @@ def _make_sure_multiverse_resources_are_added(self): if not self.added_multiverse_resources: World.cache_manager.clear_cache() dirname = find_multiverse_resources_path() - resources_paths = get_resource_paths(dirname) - World.data_directory = resources_paths + self.data_directory + World.data_directory = [dirname] + self.data_directory World.cache_manager.data_directory = World.data_directory self.added_multiverse_resources = True @@ -156,32 +181,49 @@ def get_object_joint_names(self, obj: Object) -> List[str]: def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] - def get_joint_position(self, joint: Joint) -> float: - self.check_object_exists_and_issue_warning_if_not(joint.object) - property_name = self.get_joint_position_name(joint) - return self.get_body_property(joint.name, property_name) - - def get_body_property(self, body_name: str, property_name: str) -> float: - return self.reader.get_body_property(body_name, property_name)[0] - def reset_joint_position(self, joint: Joint, joint_position: float) -> None: - attribute = self.get_joint_position_name(joint) - self.send_body_data_to_server(joint.name, {attribute: [joint_position]}) - - def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: - data = {joint.name: {self.get_joint_position_name(joint): [joint_poses[joint.name]]} - for joint in obj.joints.values() if joint.name in joint_poses.keys()} - if len(data) > 0: - self.writer.send_multiple_body_data_to_server(data) - else: - logging.warning(f"No joints found in object {obj.name}") - raise ValueError(f"No joints found in object {obj.name}") - - def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]): - self.writer.send_body_data_to_server(body_name, data) + self.set_multiple_joint_positions({joint: joint_position}) + + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + initial_joint_positions = {joint: self.get_joint_position(joint) for joint in joint_positions.keys()} + data = {joint.name: {self.get_joint_position_name(joint): [position]} + for joint, position in joint_positions.items()} + self.writer.send_multiple_body_data_to_server(data) + self._wait_until_multiple_joint_goals_are_achieved(joint_positions, initial_joint_positions) + + def _wait_until_multiple_joint_goals_are_achieved(self, joint_positions: Dict[Joint, float], + initial_joint_positions: Dict[Joint, float]) -> None: + goal_validator = self._get_multi_joint_goal_validator(joint_positions, initial_joint_positions) + self._wait_until_goal_is_achieved(goal_validator) + + def _get_multi_joint_goal_validator(self, joint_positions: Dict[Joint, float], + initial_joint_positions: Dict[Joint, float]) -> MultiJointGoalValidator: + joints = list(joint_positions.keys()) + target_joint_positions = list(joint_positions.values()) + initial_joint_positions = list(initial_joint_positions.values()) + goal_validator = MultiJointGoalValidator(initial_joint_positions, target_joint_positions, + lambda: list(self.get_multiple_joint_positions(joints).values()), + joints[0].object_name + "_joint_goal") + return goal_validator - def sent_data_to_server(self, data: Dict[str, List[float]]): - self.writer.send_data_to_server(list(data.values())) + def get_joint_position(self, joint: Joint) -> float: + data = self.get_multiple_joint_positions([joint]) + if data is not None: + return data[joint.name] + + def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: + self.check_object_exists_and_issue_warning_if_not(joints[0].object) + joint_names = [joint.name for joint in joints] + data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] + for joint in joints}) + if data is not None: + return {joint_name: list(data[joint_name].values())[0][0] for joint_name in joint_names} + + def _wait_until_joint_goal_is_achieved(self, joint: Joint, joint_position: float, + initial_joint_position: float) -> None: + goal_validator = JointGoalValidator(initial_joint_position, joint_position, + lambda: self.get_joint_position(joint), joint.name + "_joint_goal") + self._wait_until_goal_is_achieved(goal_validator) def get_link_pose(self, link: Link) -> Pose: self.check_object_exists_and_issue_warning_if_not(link.object) @@ -193,23 +235,56 @@ def get_object_pose(self, obj: Object) -> Pose: return Pose() return self._get_body_pose(obj.name) - def _get_body_pose(self, body_name: str) -> Pose: - """ - Get the pose of a body in the simulator. - param body_name: The name of the body. - return: The pose of the body. - """ - return self.reader.get_body_pose(body_name) - def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return - elif (obj.obj_type == ObjectType.ROBOT and - RobotDescription.current_robot_description.virtual_move_base_joints is not None): + if (obj.obj_type == ObjectType.ROBOT and + RobotDescription.current_robot_description.virtual_move_base_joints is not None): self.set_mobile_robot_pose(pose) else: + initial_attached_objects_poses = list(obj.get_poses_of_attached_objects().values()) self._set_body_pose(obj.name, pose) + if len(initial_attached_objects_poses) > 0: + self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + + def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_poses: List[Pose]) -> None: + """ + Wait until all attached objects are set to the desired poses. + param obj: The object to which the attached objects belong. + param initial_poses: The list of initial poses of the attached objects. + """ + target_poses = obj.get_target_poses_of_attached_objects() + self._wait_until_all_pose_goals_are_achieved(list(target_poses.keys()), list(target_poses.values()), + initial_poses) + + def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: List[Pose], + initial_poses: List[Pose], + name: Optional[str] = "all_poses_goal") -> None: + """ + Wait until all poses are set to the desired poses. + param poses: The dictionary of the desired poses + param initial_poses: The dictionary of the initial poses + param name: The name of the goal. + """ + goal_validator = MultiPoseGoalValidator(initial_poses, poses, + lambda: list(self._get_multiple_body_poses(body_names).values()), name) + self._wait_until_goal_is_achieved(goal_validator) + + def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: + """ + Get the pose of a body in the simulator. + param body_name: The name of the body. + :param wait: Whether to wait until the pose is received. + return: The pose of the body. + """ + return self.reader.get_body_pose(body_name, wait) + + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + return self._get_multiple_body_poses([obj.name for obj in objects]) + + def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: + return self.reader.get_multiple_body_poses(body_names) def multiverse_reset_world(self): self.writer.reset_world() @@ -222,7 +297,39 @@ def _set_body_pose(self, body_name: str, pose: Pose) -> None: """ xyzw = pose.orientation_as_list() wxyz = [xyzw[3], *xyzw[:3]] + initial_pose = self._get_body_pose(body_name, wait=False) self.writer.set_body_pose(body_name, pose.position_as_list(), wxyz) + if initial_pose is not None: + self._wait_until_pose_goal_is_achieved(body_name, pose, initial_pose) + + def _wait_until_pose_goal_is_achieved(self, body_name: str, target_pose: Pose, initial_pose: Pose): + """ + Wait until the pose of a body is set. + param body_name: The name of the body. + param target_pose: The target pose of the body. + param initial_pose: The initial pose of the body. + """ + goal_validator = PoseGoalValidator(initial_pose, target_pose, lambda: self._get_body_pose(body_name), + body_name + "_pose_goal") + self._wait_until_goal_is_achieved(goal_validator) + + def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: + """ + Wait until the target is reached. + param goal_validator: The goal validator object to validate and keep track of the goal achievement progress. + """ + start_time = time() + current = goal_validator.current_value + while not goal_validator.goal_achieved: + sleep(0.01) + if time() - start_time > self.reader.MAX_WAIT_TIME_FOR_DATA: + msg = f"Failed to achieve {goal_validator.name} from {goal_validator.initial_value} to"\ + f" {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}"\ + f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage"\ + f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" + logging.error(msg) + raise TimeoutError(msg) + current = goal_validator.current_value def disconnect_from_physics_server(self) -> None: MultiverseClientManager.stop_all_clients() @@ -387,7 +494,8 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def check_object_exists_and_issue_warning_if_not(self, obj: Object): if obj not in self.objects: - logging.warning(f"Object {obj} does not exist in the simulator") + msg = f"Object {obj} does not exist in the simulator" + logging.warning(msg) def check_object_exists_in_multiverse(self, obj: Object) -> bool: return self.api_requester.check_object_exists(obj) diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py index 42f8cffa2..750213ca7 100644 --- a/src/pycram/worlds/multiverse_communication/client_manager.py +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -15,6 +15,13 @@ class MultiverseClientManager: """ last_used_port: int = BASE_PORT + def __init__(self, simulation_wait_time_factor: Optional[float] = 1.0): + """ + Initialize the Multiverse client manager. + param simulation_wait_time_factor: The simulation wait time factor. + """ + self.simulation_wait_time_factor = simulation_wait_time_factor + def create_reader(self, is_prospection_world: Optional[bool] = False) -> 'MultiverseReader': """ Create a Multiverse reader client. @@ -54,7 +61,8 @@ def create_client(self, """ MultiverseClientManager.last_used_port += 1 name = (name or client_type.__name__) + f"_{self.last_used_port}" - client = client_type(name, self.last_used_port, is_prospection_world=is_prospection_world, **kwargs) + client = client_type(name, self.last_used_port, is_prospection_world=is_prospection_world, + simulation_wait_time_factor=self.simulation_wait_time_factor, **kwargs) self.clients[name] = client return client diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index b668522ba..0feb5e1ff 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -15,35 +15,41 @@ class MultiverseClient(MultiverseSocket): - def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, **kwargs): + def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, + simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ Initialize the Multiverse client, which connects to the Multiverse server. param name: The name of the client. param port: The port of the client. param is_prospection_world: Whether the client is connected to the prospection world. + param simulation_wait_time_factor: The simulation wait time factor (default is 1.0), which can be used to + increase or decrease the wait time for the simulation. """ meta_data = MultiverseMetaData() meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + name meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name super().__init__(SocketAddress(port=str(port)), meta_data) + self.simulation_wait_time_factor = simulation_wait_time_factor self.run() class MultiverseReader(MultiverseClient): - MAX_WAIT_TIME_FOR_DATA: Optional[float] = 1 + MAX_WAIT_TIME_FOR_DATA: Optional[float] = 2 """ The maximum wait time for the data in seconds. """ - def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, **kwargs): + def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, + simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ Initialize the Multiverse reader, which reads the data from the Multiverse server in a separate thread. This class provides methods to get data (e.g., position, orientation) from the Multiverse server. param port: The port of the Multiverse reader client. param is_prospection_world: Whether the reader is connected to the prospection world. + param simulation_wait_time_factor: The simulation wait time factor. """ - super().__init__(name, port, is_prospection_world) + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.request_meta_data["receive"][""] = [""] @@ -60,9 +66,22 @@ def get_body_pose(self, name: str, wait: Optional[bool] = True) -> Optional[Pose param wait: Whether to wait for the data. return: The position and orientation of the body. """ - data = self.get_body_data(name, ["position", "quaternion"], wait=wait) + data = self.get_multiple_body_poses([name], wait=wait) if data is not None: - return Pose(data["position"], self.wxyz_to_xyzw(data["quaternion"])) + return data[name] + + def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = True) -> Optional[Dict[str, Pose]]: + """ + Get the body poses from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param wait: Whether to wait for the data. + return: The positions and orientations of the bodies as a dictionary. + """ + data = self.get_multiple_body_data(body_names, {name: ["position", "quaternion"] for name in body_names}, + wait=wait) + if data is not None: + return {name: Pose(data[name]["position"], self.wxyz_to_xyzw(data[name]["quaternion"])) + for name in body_names} def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: """ @@ -71,7 +90,17 @@ def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[ param wait: Whether to wait for the data. return: The position of the body. """ - return self.get_body_data(name, ["position"], wait=wait) + return self.get_multiple_body_positions([name], wait=wait)[name] + + def get_multiple_body_positions(self, body_names: List[str], + wait: Optional[bool] = True) -> Optional[Dict[str, List[float]]]: + """ + Get the body positions from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param wait: Whether to wait for the data. + return: The positions of the bodies as a dictionary. + """ + return self.get_multiple_body_properties(body_names, ["position"], wait=wait) def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: """ @@ -80,9 +109,19 @@ def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Option param wait: Whether to wait for the data. return: The orientation of the body. """ - data = self.get_body_property(name, "quaternion", wait=wait) + return self.get_multiple_body_orientations([name], wait=wait)[name] + + def get_multiple_body_orientations(self, body_names: List[str], + wait: Optional[bool] = True) -> Optional[Dict[str, List[float]]]: + """ + Get the body orientations from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param wait: Whether to wait for the data. + return: The orientations of the bodies as a dictionary. + """ + data = self.get_multiple_body_properties(body_names, ["quaternion"], wait=wait) if data is not None: - return self.wxyz_to_xyzw(data) + return {name: self.wxyz_to_xyzw(data[name]["quaternion"]) for name in body_names} def get_body_property(self, name: str, property_name: str, wait: Optional[bool] = True) -> Optional[List[float]]: """ @@ -92,9 +131,20 @@ def get_body_property(self, name: str, property_name: str, wait: Optional[bool] param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_body_data(name, [property_name], wait=wait) + data = self.get_multiple_body_properties([name], [property_name], wait=wait) if data is not None: - return data[property_name] + return data[name][property_name] + + def get_multiple_body_properties(self, body_names: List[str], properties: List[str], + wait: Optional[bool] = True) -> Dict[str, Dict[str, List[float]]]: + """ + Get the body properties from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param properties: The properties of the bodies. + param wait: Whether to wait for the data. + return: The properties of the bodies as a dictionary. + """ + return self.get_multiple_body_data(body_names, {name: properties for name in body_names}, wait=wait) @staticmethod def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: @@ -115,24 +165,60 @@ def get_body_data(self, name: str, param wait: Whether to wait for the data. return: The body data as a dictionary. """ + properties = {name: properties} if properties is not None else None + return self.get_multiple_body_data([name], properties, wait=wait)[name] + + def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[str]]] = None, + wait: Optional[bool] = True) -> Dict: + """ + Get the body data from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param properties: The properties of the bodies. + param wait: Whether to wait for the data. + return: The body data as a dictionary. + """ + if wait: - return self.wait_for_body_data(name, properties) - elif self.check_for_body_data(name, self.get_received_data(), properties): - return self.get_received_data()[name] + return self.wait_for_multiple_body_data(body_names, properties) + + data = self.get_received_data() + if self.check_multiple_body_data(body_names, data, properties): + return {name: data[name] for name in body_names} - def wait_for_body_data(self, name: str, properties: Optional[List[str]] = None) -> Dict: + def wait_for_multiple_body_data(self, body_names: List[str], + properties: Optional[Dict[str, List[str]]] = None) -> Dict: """ - Wait for the body data from the multiverse server. - param name: The name of the body. - param properties: The properties of the body. + Wait for the body data from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param properties: The properties of the bodies. return: The body data as a dictionary. """ start = time() + data_received_flag = False while time() - start < self.MAX_WAIT_TIME_FOR_DATA: received_data = self.get_received_data() - data_received_flag = self.check_for_body_data(name, received_data, properties) + data_received_flag = self.check_multiple_body_data(body_names, received_data, properties) if data_received_flag: - return received_data[name] + return {name: received_data[name] for name in body_names} + if not data_received_flag: + properties_str = "Data" if properties is None else f"Properties {properties}" + msg = f"{properties_str} for bodies {body_names} not received within {self.MAX_WAIT_TIME_FOR_DATA} seconds" + logging.error(msg) + raise ValueError(msg) + + def check_multiple_body_data(self, body_names: List[str], data: Dict, + properties: Optional[Dict[str, List[str]]] = None) -> bool: + """ + Check if the body data is received from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param data: The data received from the multiverse server. + param properties: The properties of the bodies. + return: Whether the body data is received. + """ + if properties is None: + return all([self.check_for_body_data(name, data) for name in body_names]) + else: + return all([self.check_for_body_data(name, data, properties[name]) for name in body_names]) @staticmethod def check_for_body_data(name: str, data: Dict, properties: Optional[List[str]] = None) -> bool: @@ -175,16 +261,17 @@ def join(self): class MultiverseWriter(MultiverseClient): - time_for_sim_update: Optional[float] = 0.02 + time_for_sim_update: Optional[float] = 0.0 # 0.02 """ Wait time for the sent data to be applied in the simulator. """ - time_for_setting_body_data: Optional[float] = 0.01 + time_for_setting_body_data: Optional[float] = 0.0 # 0.01 """ Wait time for setting body data. """ - def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, **kwargs): + def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, + simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ Initialize the Multiverse writer, which writes the data to the Multiverse server. This class provides methods to send data (e.g., position, orientation) to the Multiverse server. @@ -192,8 +279,10 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: param simulation: The name of the simulation that the writer is connected to (usually the name defined in the .muv file). param is_prospection_world: Whether the writer is connected to the prospection world. + param simulation_wait_time_factor: The wait time factor for the simulation (default is 1.0), which can be used + to increase or decrease the wait time for the simulation. """ - super().__init__(name, port, is_prospection_world) + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation self.time_start = time() @@ -217,7 +306,8 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List """ self.send_body_data_to_server(body_name, {"position": position, - "quaternion": orientation}) + "quaternion": orientation, + "relative_velocity": [0]*6}) def set_body_position(self, body_name: str, position: List[float]) -> None: """ @@ -272,7 +362,7 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[ for value in data] self.send_data = [time() - self.time_start, *flattened_data] self.send_and_receive_data() - sleep(self.time_for_setting_body_data) + sleep(self.time_for_setting_body_data * self.simulation_wait_time_factor) return self.response_meta_data def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: @@ -304,7 +394,7 @@ def send_data_to_server(self, data: List, self.send_and_receive_meta_data() self.send_data = data self.send_and_receive_data() - sleep(self.time_for_sim_update) + sleep(self.time_for_sim_update * self.simulation_wait_time_factor) return self.response_meta_data @@ -319,7 +409,8 @@ class MultiverseAPI(MultiverseClient): The wait time for the API request in seconds. """ - def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False): + def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, + simulation_wait_time_factor: Optional[float] = 1.0): """ Initialize the Multiverse API, which sends API requests to the Multiverse server. This class provides methods like attach and detach objects, get contact points, and other API requests. @@ -327,8 +418,10 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: param simulation: The name of the simulation that the API is connected to (usually the name defined in the .muv file). param is_prospection_world: Whether the API is connected to the prospection world. + param simulation_wait_time_factor: The simulation wait time factor, which can be used to increase or decrease + the wait time for the simulation. """ - super().__init__(name, port, is_prospection_world) + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation def attach(self, constraint: Constraint) -> None: @@ -534,7 +627,7 @@ def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[s self._add_api_request(api_name.value, *params) self._send_api_request() responses = self._get_all_apis_responses() - sleep(self.API_REQUEST_WAIT_TIME) + sleep(self.API_REQUEST_WAIT_TIME * self.simulation_wait_time_factor) return responses def _get_all_apis_responses(self) -> Dict[API, List[str]]: diff --git a/src/pycram/worlds/multiverse_functions/__init__.py b/src/pycram/worlds/multiverse_functions/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py new file mode 100644 index 000000000..423f5c5e5 --- /dev/null +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -0,0 +1,201 @@ +from abc import abstractmethod, ABC + +import numpy as np +from typing_extensions import List, Union, Any, Callable + +from pycram.datastructures.pose import Pose + + +class GoalValidator(ABC): + """ + A class to validate the goal by tracking the goal achievement progress. + """ + + def __init__(self, initial_value: Any, goal_value: Any, current_value_getter: Callable[[], Any], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + self.initial_value: Any = initial_value + self.current_value_getter: Callable[[], Any] = current_value_getter + self.goal_value: Any = goal_value + self.name: str = name + self.acceptable_percentage_of_goal_achieved: float = acceptable_percentage_of_goal_achieved + self.initial_error: float = self.calculate_initial_error() + + def calculate_initial_error(self) -> float: + """ + Calculate the initial error. + """ + return self.calculate_error(self.goal_value, self.initial_value) + + @property + def current_value(self) -> Any: + """ + Get the current value. + """ + return self.current_value_getter() + + @property + def current_error(self) -> float: + """ + Calculate the current error. + """ + return self.calculate_error(self.goal_value, self.current_value) + + @abstractmethod + def calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two values. + """ + pass + + @property + def percentage_of_goal_achieved(self) -> float: + """ + Calculate the percentage of goal achieved. + """ + if self.initial_error > 1e-3: + return 1 - self.current_error / self.initial_error + else: + return 1 + + @property + def goal_achieved(self) -> bool: + """ + Check if the goal is achieved. + return: Whether the goal is achieved. + """ + return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved + + +class SingleValueGoalValidator(GoalValidator): + """ + A class to validate the goal by tracking the goal achievement progress for a single value. + """ + + def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], + current_value_getter: Callable[[], Union[float, int]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + def calculate_error(self, value_1: Union[float, int], value_2: Union[float, int]) -> Union[float, int]: + """ + Calculate the error between two values. + return: The error between the two values. + """ + return abs(value_1 - value_2) + + +class JointGoalValidator(SingleValueGoalValidator): + def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], + current_value_getter: Callable[[], Union[float, int]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + +class IterableGoalValidator(GoalValidator): + """ + A class to validate the goal by tracking the goal achievement progress for an iterable goal. + """ + + def __init__(self, initial_value: List[Any], goal_value: List[Any], + current_value_getter: Callable[[], List[Any]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: + """ + Calculate the error between two iterables. + return: The error between the two iterables. + """ + return np.linalg.norm(np.array(iterable_1) - np.array(iterable_2)) + + +def get_combined_goal_validator(goal_validators: List[GoalValidator], combined_name: str): + """ + Get a combined goal validator. + :param goal_validators: The goal validators to combine. + :param combined_name: The name of the combined goal validator. + return: The combined goal validator. + """ + + class CombinedGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[Any], goal_value: List[Any], + current_value_getter: Callable[[], List[Any]], + name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, + acceptable_percentage_of_goal_achieved) + + def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: + """ + Calculate the error between two iterables. + return: The error between the two iterables. + """ + return (sum([goal_validator.calculate_error(value_1, value_2) + for goal_validator, value_1, value_2 in zip(goal_validators, iterable_1, iterable_2)]) / + len(goal_validators)) + + return CombinedGoalValidator([goal_validator.initial_value for goal_validator in goal_validators], + [goal_validator.goal_value for goal_validator in goal_validators], + [goal_validator.current_value_getter for goal_validator in goal_validators], + combined_name, 0.5) + + +class PositionGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[float], goal_value: List[float], + current_value_getter: Callable[[], List[float]], + name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + +class OrientationGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[float], goal_value: List[float], + current_value_getter: Callable[[], List[float]], + name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + +class MultiJointGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[float], goal_value: List[float], + current_value_getter: Callable[[], List[float]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + +class PoseGoalValidator(GoalValidator): + """ + A class to validate the goal by tracking the goal achievement progress for a pose goal. + """ + + def __init__(self, initial_value: Pose, goal_value: Pose, current_value_getter: Callable[[], Pose], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + def calculate_error(self, pose_1: Pose, pose_2: Pose) -> float: + """ + Calculate the error between two poses. + return: The error between the two poses. + """ + return calculate_pose_error(pose_1, pose_2) + + +class MultiPoseGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[Pose], goal_value: List[Pose], + current_value_getter: Callable[[], List[Pose]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + def calculate_error(self, iterable_1: List[Pose], iterable_2: List[Pose]) -> float: + """ + Calculate the error between two iterables. + return: The error between the two iterables. + """ + return (sum([calculate_pose_error(pose_1, pose_2) for pose_1, pose_2 in zip(iterable_1, iterable_2)]) / + len(iterable_1)) + + +def calculate_pose_error(pose_1: Pose, pose_2: Pose) -> float: + """ + Calculate the error between two poses. + return: The error between the two poses. + """ + return (np.linalg.norm(np.array(pose_1.position_as_list()) - np.array(pose_2.position_as_list())) + + np.linalg.norm(np.array(pose_1.orientation_as_list()) - np.array(pose_2.orientation_as_list()))) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 030208b1f..e2af322f4 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import os import unittest import psutil @@ -42,7 +43,6 @@ def setUpClass(cls): return cls.multiverse = Multiverse(simulation="pycram_test", is_prospection=False) - # cls.big_bowl = cls.spawn_big_bowl() @classmethod def tearDownClass(cls): @@ -164,8 +164,8 @@ def test_set_robot_position(self): self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) def test_attach_object(self): - milk = self.spawn_milk([1, 0, 0.1]) - cup = self.spawn_cup([1, 1, 0.1]) + milk = self.spawn_milk([1, 0.1, 0.1]) + cup = self.spawn_cup([1, 1.1, 0.1]) milk.attach(cup) self.assertTrue(cup in milk.attachments) milk_position = milk.get_position_as_list() From e2c744f5a530f50f1ed9bbd50ca47a201a661256 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 01:11:24 +0200 Subject: [PATCH 02/21] [Multiverse] set_joint_positions takes joint names. --- src/pycram/datastructures/world.py | 6 ++--- src/pycram/worlds/multiverse.py | 8 +++---- .../multiverse_functions/goal_validator.py | 22 +++++++++---------- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index e2dbbdf23..30aa53045 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -508,9 +508,9 @@ def get_move_base_joint_goal(self, pose: Pose) -> Dict[Joint, float]: angle_diff = self.get_z_angle_diff(self.robot.get_orientation_as_list(), pose.orientation_as_list()) # Get the joints of the base link move_base_joints = self.get_move_base_joints() - return {self.robot.joints[move_base_joints.translation_x]: position_diff[0], - self.robot.joints[move_base_joints.translation_y]: position_diff[1], - self.robot.joints[move_base_joints.angular_z]: angle_diff} + return {move_base_joints.translation_x: position_diff[0], + move_base_joints.translation_y: position_diff[1], + move_base_joints.angular_z: angle_diff} @staticmethod def get_move_base_joints() -> VirtualMoveBaseJoints: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 7971a0850..ab24715e0 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -323,10 +323,10 @@ def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: while not goal_validator.goal_achieved: sleep(0.01) if time() - start_time > self.reader.MAX_WAIT_TIME_FOR_DATA: - msg = f"Failed to achieve {goal_validator.name} from {goal_validator.initial_value} to"\ - f" {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}"\ - f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage"\ - f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" + msg = f"Failed to achieve {goal_validator.name} from {goal_validator.initial_value} to" \ + f" {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}" \ + f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage" \ + f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" logging.error(msg) raise TimeoutError(msg) current = goal_validator.current_value diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 423f5c5e5..1a66949fc 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,7 +1,7 @@ from abc import abstractmethod, ABC import numpy as np -from typing_extensions import List, Union, Any, Callable +from typing_extensions import List, Union, Any, Callable, Optional from pycram.datastructures.pose import Pose @@ -12,7 +12,7 @@ class GoalValidator(ABC): """ def __init__(self, initial_value: Any, goal_value: Any, current_value_getter: Callable[[], Any], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): self.initial_value: Any = initial_value self.current_value_getter: Callable[[], Any] = current_value_getter self.goal_value: Any = goal_value @@ -73,7 +73,7 @@ class SingleValueGoalValidator(GoalValidator): def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], current_value_getter: Callable[[], Union[float, int]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) def calculate_error(self, value_1: Union[float, int], value_2: Union[float, int]) -> Union[float, int]: @@ -87,7 +87,7 @@ def calculate_error(self, value_1: Union[float, int], value_2: Union[float, int] class JointGoalValidator(SingleValueGoalValidator): def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], current_value_getter: Callable[[], Union[float, int]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) @@ -98,7 +98,7 @@ class IterableGoalValidator(GoalValidator): def __init__(self, initial_value: List[Any], goal_value: List[Any], current_value_getter: Callable[[], List[Any]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: @@ -120,7 +120,7 @@ def get_combined_goal_validator(goal_validators: List[GoalValidator], combined_n class CombinedGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[Any], goal_value: List[Any], current_value_getter: Callable[[], List[Any]], - name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) @@ -142,21 +142,21 @@ def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float class PositionGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[float], goal_value: List[float], current_value_getter: Callable[[], List[float]], - name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) class OrientationGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[float], goal_value: List[float], current_value_getter: Callable[[], List[float]], - name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) class MultiJointGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[float], goal_value: List[float], current_value_getter: Callable[[], List[float]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) @@ -166,7 +166,7 @@ class PoseGoalValidator(GoalValidator): """ def __init__(self, initial_value: Pose, goal_value: Pose, current_value_getter: Callable[[], Pose], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) def calculate_error(self, pose_1: Pose, pose_2: Pose) -> float: @@ -180,7 +180,7 @@ def calculate_error(self, pose_1: Pose, pose_2: Pose) -> float: class MultiPoseGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[Pose], goal_value: List[Pose], current_value_getter: Callable[[], List[Pose]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) def calculate_error(self, iterable_1: List[Pose], iterable_2: List[Pose]) -> float: From ff85251cf226f0dfaaa183ae59573ee0035528f2 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 01:50:08 +0200 Subject: [PATCH 03/21] [Multiverse] get_child_object_pose in attachment. --- src/pycram/world_concepts/constraints.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index 955336925..5843db56e 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -30,6 +30,14 @@ def __init__(self, self.child_to_constraint = child_to_constraint self._parent_to_child = None + def get_child_object_pose(self) -> Pose: + """ + Returns the pose of the child object. + + :return: The pose of the child object. + """ + return self.child_link.object.pose + def get_child_object_pose_given_parent(self, pose: Pose) -> Pose: """ Returns the pose of the child object given the parent pose. From aa90866b18aa4504c57bd5fbee3ba0f3c925decf Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 22:11:41 +0200 Subject: [PATCH 04/21] [Multiverse] goal_validator.py is tested with bullet_world and is working. --- src/pycram/datastructures/pose.py | 13 +- src/pycram/world_concepts/world_object.py | 1 - src/pycram/worlds/multiverse.py | 3 +- .../multiverse_functions/error_checkers.py | 224 ++++++++++++++++++ .../multiverse_functions/goal_validator.py | 217 ++++++----------- test/test_error_checkers.py | 131 ++++++++++ test/test_goal_validator.py | 224 ++++++++++++++++++ 7 files changed, 656 insertions(+), 157 deletions(-) create mode 100644 src/pycram/worlds/multiverse_functions/error_checkers.py create mode 100644 test/test_error_checkers.py create mode 100644 test/test_goal_validator.py diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 8e57cc0ba..52e2da02f 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -3,7 +3,7 @@ import math import datetime -from typing_extensions import List, Union, Optional +from typing_extensions import List, Union, Optional, Sized import numpy as np import rospy @@ -144,14 +144,17 @@ def orientation(self, value) -> None: :param value: New orientation, either a list or geometry_msgs/Quaternion """ - if not isinstance(value, list) and not isinstance(value, tuple) and not isinstance(value, GeoQuaternion): - rospy.logwarn("Orientation can only be a list or geometry_msgs/Quaternion") + if not isinstance(value, Sized) and not isinstance(value, GeoQuaternion): + rospy.logwarn("Orientation can only be an iterable (list, tuple, ...etc.) or a geometry_msgs/Quaternion") return - if isinstance(value, list) or isinstance(value, tuple) and len(value) == 4: + if isinstance(value, Sized) and len(value) == 4: orientation = np.array(value) - else: + elif isinstance(value, GeoQuaternion): orientation = np.array([value.x, value.y, value.z, value.w]) + else: + rospy.logerr("Orientation has to be a list or geometry_msgs/Quaternion") + raise TypeError("Orientation has to be a list or geometry_msgs/Quaternion") # This is used instead of np.linalg.norm since numpy is too slow on small arrays self.pose.orientation = get_normalized_quaternion(orientation) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index f64baa339..843c70c3a 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -92,7 +92,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) - def get_target_poses_of_attached_objects(self) -> Dict[Object, Pose]: """ Get the target poses of the attached objects. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index ab24715e0..511f28ddd 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -255,7 +255,8 @@ def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_po param initial_poses: The list of initial poses of the attached objects. """ target_poses = obj.get_target_poses_of_attached_objects() - self._wait_until_all_pose_goals_are_achieved(list(target_poses.keys()), list(target_poses.values()), + body_names = [obj.name for obj in target_poses.keys()] + self._wait_until_all_pose_goals_are_achieved(body_names, list(target_poses.values()), initial_poses) def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: List[Pose], diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_functions/error_checkers.py new file mode 100644 index 000000000..94af1efc7 --- /dev/null +++ b/src/pycram/worlds/multiverse_functions/error_checkers.py @@ -0,0 +1,224 @@ +from abc import ABC, abstractmethod +from collections.abc import Iterable + +import numpy as np +from tf.transformations import quaternion_multiply, quaternion_inverse +from typing_extensions import List, Union, Optional, Any, Iterable as TypingIterable, Sized + +from pycram.datastructures.enums import JointType +from pycram.datastructures.pose import Pose + + +class ErrorChecker(ABC): + """ + An abstract class that resembles an error checker. It has two main methods, one for calculating the error between + two values and another for checking if the error is acceptable. + """ + def __init__(self, acceptable_error: Union[float, TypingIterable[float]], is_iterable: Optional[bool] = False): + """ + Initialize the error checker. + :param acceptable_error: The acceptable error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + self.acceptable_error = np.array(acceptable_error) + self.is_iterable = is_iterable + + def get_acceptable_error(self, tile_to_match: Optional[Sized] = None) -> np.ndarray: + """ + Get the acceptable error. + :return: The acceptable error. + """ + if tile_to_match is not None and self.is_iterable: + return self.tiled_acceptable_error(tile_to_match) + else: + return self.acceptable_error + + def tiled_acceptable_error(self, tile_to_match: Sized) -> np.ndarray: + """ + Tile the acceptable error to match the length of the error. + :param tile_to_match: The object to match the length of the error. + :return: The tiled acceptable error. + """ + return np.tile(np.array(self.acceptable_error).flatten(), len(tile_to_match) // self.acceptable_error.size) + + @abstractmethod + def _calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: + """ + Calculate the error between two values. + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. + """ + pass + + def calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: + """ + Calculate the error between two values. + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. + """ + if self.is_iterable: + return [self._calculate_error(v1, v2) for v1, v2 in zip(value_1, value_2)] + else: + return self._calculate_error(value_1, value_2) + + def is_error_acceptable(self, value_1: Any, value_2: Any) -> bool: + """ + Check if the error is acceptable. + :param value_1: The first value. + :param value_2: The second value. + :return: Whether the error is acceptable. + """ + error = self.calculate_error(value_1, value_2) + if self.is_iterable: + error = np.array(error).flatten() + return np.all(error <= self.tiled_acceptable_error(error)) + else: + return is_error_acceptable(error, self.acceptable_error) + + +class PoseErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Union[float, TypingIterable[float]] = (1e-3, np.pi / 180), + is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> List[float]: + """ + Calculate the error between two poses. + """ + return calculate_pose_error(value_1, value_2) + + +class PositionErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two positions. + """ + return calculate_position_error(value_1, value_2) + + +class OrientationErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two quaternions. + """ + return calculate_orientation_error(value_1, value_2) + + +class MultiJointPositionErrorChecker(ErrorChecker): + + def __init__(self, joint_types: List[JointType], acceptable_error: Optional[List[float]] = None): + self.joint_types = joint_types + if acceptable_error is None: + acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] + super().__init__(acceptable_error, True) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two joint positions. + """ + return calculate_joint_position_error(value_1, value_2) + + +class RevoluteJointPositionErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two joint positions. + """ + return calculate_joint_position_error(value_1, value_2) + + +class PrismaticJointPositionErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two joint positions. + """ + return calculate_joint_position_error(value_1, value_2) + + +def calculate_pose_error(pose_1: Pose, pose_2: Pose) -> List[float]: + """ + Calculate the error between two poses. + return: The error between the two poses. + """ + return [calculate_position_error(pose_1.position_as_list(), pose_2.position_as_list()), + calculate_orientation_error(pose_1.orientation_as_list(), pose_2.orientation_as_list())] + + +def calculate_position_error(position_1: List[float], position_2: List[float]) -> float: + """ + Calculate the error between two positions. + return: The error between the two positions. + """ + return np.linalg.norm(np.array(position_1) - np.array(position_2)) + + +def calculate_orientation_error(quat_1: List[float], quat_2: List[float]) -> float: + """ + Calculate the error between two quaternions. + return: The error between the two quaternions. + """ + return calculate_angle_between_quaternions(quat_1, quat_2) + + +def calculate_joint_position_error(joint_position_1: float, joint_position_2: float) -> float: + """ + Calculate the error between two joint positions. + return: The error between the two joint positions. + """ + return abs(joint_position_1 - joint_position_2) + + +def is_error_acceptable(error: Union[float, TypingIterable[float]], + acceptable_error: Union[float, TypingIterable[float]]) -> bool: + """ + Check if the error is acceptable. + :param error: The error. + :param acceptable_error: The acceptable error. + return: Whether the error is acceptable. + """ + if isinstance(error, Iterable): + return all([error_i <= acceptable_error_i for error_i, acceptable_error_i in zip(error, acceptable_error)]) + else: + return error <= acceptable_error + + +def calculate_angle_between_quaternions(quat_1: List[float], quat_2: List[float]) -> float: + """ + Calculates the angle between two quaternions. + :param quat_1: The first quaternion. + :param quat_2: The second quaternion. + :return: A float value that represents the angle between the two quaternions. + """ + quat_diff = calculate_quaternion_difference(quat_1, quat_2) + quat_diff_angle = 2 * np.arctan2(np.linalg.norm(quat_diff[0:3]), quat_diff[3]) + return quat_diff_angle + + +def calculate_quaternion_difference(quat_1: List[float], quat_2: List[float]) -> List[float]: + """ + Calculates the quaternion difference. + :param quat_1: The quaternion of the object at the first time step. + :param quat_2: The quaternion of the object at the second time step. + :return: A list of float values that represent the quaternion difference. + """ + quat_diff = quaternion_multiply(quaternion_inverse(quat_1), quat_2) + return quat_diff diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 1a66949fc..4a6bc38b0 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,30 +1,35 @@ -from abc import abstractmethod, ABC - import numpy as np -from typing_extensions import List, Union, Any, Callable, Optional +from typing_extensions import Any, Callable, Optional, List, Union -from pycram.datastructures.pose import Pose +from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker -class GoalValidator(ABC): +class GoalValidator: """ A class to validate the goal by tracking the goal achievement progress. """ - def __init__(self, initial_value: Any, goal_value: Any, current_value_getter: Callable[[], Any], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - self.initial_value: Any = initial_value - self.current_value_getter: Callable[[], Any] = current_value_getter - self.goal_value: Any = goal_value - self.name: str = name - self.acceptable_percentage_of_goal_achieved: float = acceptable_percentage_of_goal_achieved - self.initial_error: float = self.calculate_initial_error() - - def calculate_initial_error(self) -> float: + def __init__(self, goal_value: Any, current_value_getter: Callable[[], Any], error_checker: ErrorChecker, + initial_value: Any = None, + acceptable_percentage_of_goal_achieved: Optional[float] = None): """ - Calculate the initial error. + Initialize the goal validator. + :param goal_value: The goal value. + :param current_value_getter: The current value getter. + :param error_checker: The error checker object. + :param initial_value: The initial value. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved, if given, will be + used to check if this percentage is achieved instead of the complete goal. """ - return self.calculate_error(self.goal_value, self.initial_value) + self.error_checker: ErrorChecker = error_checker + self.current_value_getter: Callable[[], Any] = current_value_getter + self.goal_value: Any = goal_value + if initial_value is None: + self.initial_error: List[Union[float, List[float]]] = self.current_error + else: + self.initial_error: List[Union[float, List[float]]] = self.calculate_error(goal_value, initial_value) + self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved + self.acceptable_error = self.error_checker.get_acceptable_error(self.initial_error) @property def current_value(self) -> Any: @@ -34,168 +39,80 @@ def current_value(self) -> Any: return self.current_value_getter() @property - def current_error(self) -> float: + def current_error(self) -> np.ndarray: """ Calculate the current error. """ return self.calculate_error(self.goal_value, self.current_value) - @abstractmethod - def calculate_error(self, value_1: Any, value_2: Any) -> float: + def calculate_error(self, value_1: Any, value_2: Any) -> np.ndarray: """ Calculate the error between two values. """ - pass + return np.array(self.error_checker.calculate_error(value_1, value_2)).flatten() @property def percentage_of_goal_achieved(self) -> float: """ Calculate the percentage of goal achieved. """ - if self.initial_error > 1e-3: - return 1 - self.current_error / self.initial_error - else: + percent_array = 1 - self.relative_current_error / self.relative_initial_error + percent_array_filtered = percent_array[self.relative_initial_error > self.acceptable_error] + if len(percent_array_filtered) == 0: return 1 + else: + return np.mean(percent_array_filtered) @property - def goal_achieved(self) -> bool: + def actual_percentage_of_goal_achieved(self) -> float: """ - Check if the goal is achieved. - return: Whether the goal is achieved. + Calculate the percentage of goal achieved. """ - return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved - - -class SingleValueGoalValidator(GoalValidator): - """ - A class to validate the goal by tracking the goal achievement progress for a single value. - """ - - def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], - current_value_getter: Callable[[], Union[float, int]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + percent_array = 1 - self.current_error / np.maximum(self.initial_error, 1e-3) + percent_array_filtered = percent_array[self.initial_error > self.acceptable_error] + if len(percent_array_filtered) == 0: + return 1 + else: + return np.mean(percent_array_filtered) - def calculate_error(self, value_1: Union[float, int], value_2: Union[float, int]) -> Union[float, int]: + @property + def relative_current_error(self) -> np.ndarray: """ - Calculate the error between two values. - return: The error between the two values. + Get the relative current error. """ - return abs(value_1 - value_2) - - -class JointGoalValidator(SingleValueGoalValidator): - def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], - current_value_getter: Callable[[], Union[float, int]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + return self.get_relative_error(self.current_error, threshold=0) - -class IterableGoalValidator(GoalValidator): - """ - A class to validate the goal by tracking the goal achievement progress for an iterable goal. - """ - - def __init__(self, initial_value: List[Any], goal_value: List[Any], - current_value_getter: Callable[[], List[Any]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: + @property + def relative_initial_error(self) -> np.ndarray: """ - Calculate the error between two iterables. - return: The error between the two iterables. + Get the relative initial error. """ - return np.linalg.norm(np.array(iterable_1) - np.array(iterable_2)) - + return self.get_relative_error(self.initial_error) -def get_combined_goal_validator(goal_validators: List[GoalValidator], combined_name: str): - """ - Get a combined goal validator. - :param goal_validators: The goal validators to combine. - :param combined_name: The name of the combined goal validator. - return: The combined goal validator. - """ - - class CombinedGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[Any], goal_value: List[Any], - current_value_getter: Callable[[], List[Any]], - name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, - acceptable_percentage_of_goal_achieved) - - def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: - """ - Calculate the error between two iterables. - return: The error between the two iterables. - """ - return (sum([goal_validator.calculate_error(value_1, value_2) - for goal_validator, value_1, value_2 in zip(goal_validators, iterable_1, iterable_2)]) / - len(goal_validators)) - - return CombinedGoalValidator([goal_validator.initial_value for goal_validator in goal_validators], - [goal_validator.goal_value for goal_validator in goal_validators], - [goal_validator.current_value_getter for goal_validator in goal_validators], - combined_name, 0.5) - - -class PositionGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[float], goal_value: List[float], - current_value_getter: Callable[[], List[float]], - name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - -class OrientationGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[float], goal_value: List[float], - current_value_getter: Callable[[], List[float]], - name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - -class MultiJointGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[float], goal_value: List[float], - current_value_getter: Callable[[], List[float]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - -class PoseGoalValidator(GoalValidator): - """ - A class to validate the goal by tracking the goal achievement progress for a pose goal. - """ - - def __init__(self, initial_value: Pose, goal_value: Pose, current_value_getter: Callable[[], Pose], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - def calculate_error(self, pose_1: Pose, pose_2: Pose) -> float: + def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> np.ndarray: """ - Calculate the error between two poses. - return: The error between the two poses. + Get the relative error by comparing the error with the acceptable error and filtering out the errors that are + less than the threshold. + :param error: The error. + :param threshold: The threshold. """ - return calculate_pose_error(pose_1, pose_2) - - -class MultiPoseGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[Pose], goal_value: List[Pose], - current_value_getter: Callable[[], List[Pose]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + return np.maximum(error-self.acceptable_error, threshold) - def calculate_error(self, iterable_1: List[Pose], iterable_2: List[Pose]) -> float: + @property + def goal_achieved(self) -> bool: """ - Calculate the error between two iterables. - return: The error between the two iterables. + Check if the goal is achieved. + return: Whether the goal is achieved. """ - return (sum([calculate_pose_error(pose_1, pose_2) for pose_1, pose_2 in zip(iterable_1, iterable_2)]) / - len(iterable_1)) - + if self.acceptable_percentage_of_goal_achieved is None: + return self.is_error_acceptable + else: + return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved -def calculate_pose_error(pose_1: Pose, pose_2: Pose) -> float: - """ - Calculate the error between two poses. - return: The error between the two poses. - """ - return (np.linalg.norm(np.array(pose_1.position_as_list()) - np.array(pose_2.position_as_list())) - + np.linalg.norm(np.array(pose_1.orientation_as_list()) - np.array(pose_2.orientation_as_list()))) + @property + def is_error_acceptable(self) -> bool: + """ + Check if the error is acceptable. + return: Whether the error is acceptable. + """ + return self.error_checker.is_error_acceptable(self.current_value, self.goal_value) diff --git a/test/test_error_checkers.py b/test/test_error_checkers.py new file mode 100644 index 000000000..b7a459ff9 --- /dev/null +++ b/test/test_error_checkers.py @@ -0,0 +1,131 @@ +from unittest import TestCase + +import numpy as np +from tf.transformations import quaternion_from_euler + +from pycram.datastructures.enums import JointType +from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ + PoseErrorChecker, PositionErrorChecker, OrientationErrorChecker, RevoluteJointPositionErrorChecker, \ + PrismaticJointPositionErrorChecker, MultiJointPositionErrorChecker + +from pycram.datastructures.pose import Pose + + +class TestErrorCheckers(TestCase): + @classmethod + def setUpClass(cls): + pass + + @classmethod + def tearDownClass(cls): + pass + + def tearDown(self): + pass + + def test_calculate_quaternion_error(self): + quat_1 = [0.0, 0.0, 0.0, 1.0] + quat_2 = [0.0, 0.0, 0.0, 1.0] + error = calculate_angle_between_quaternions(quat_1, quat_2) + self.assertEqual(error, 0.0) + quat_2 = quaternion_from_euler(0, 0, np.pi/2) + error = calculate_angle_between_quaternions(quat_1, quat_2) + self.assertEqual(error, np.pi/2) + + def test_pose_error_checker(self): + pose_1 = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]) + pose_2 = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]) + error_checker = PoseErrorChecker() + error = error_checker.calculate_error(pose_1, pose_2) + self.assertEqual(error, [0.0, 0.0]) + self.assertTrue(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, np.pi/2, 0) + pose_2 = Pose([0, 1, np.sqrt(3)], quat) + error = error_checker.calculate_error(pose_1, pose_2) + self.assertAlmostEqual(error[0], 2, places=5) + self.assertEqual(error[1], np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, 0, np.pi/360) + pose_2 = Pose([0, 0.0001, 0.0001], quat) + self.assertTrue(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, 0, np.pi / 179) + pose_2 = Pose([0, 0.0001, 0.0001], quat) + self.assertFalse(error_checker.is_error_acceptable(pose_1, pose_2)) + + def test_position_error_checker(self): + position_1 = [0.0, 0.0, 0.0] + position_2 = [0.0, 0.0, 0.0] + error_checker = PositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = [1.0, 1.0, 1.0] + error = error_checker.calculate_error(position_1, position_2) + self.assertAlmostEqual(error, np.sqrt(3), places=5) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_orientation_error_checker(self): + quat_1 = [0.0, 0.0, 0.0, 1.0] + quat_2 = [0.0, 0.0, 0.0, 1.0] + error_checker = OrientationErrorChecker() + error = error_checker.calculate_error(quat_1, quat_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(quat_1, quat_2)) + quat_2 = quaternion_from_euler(0, 0, np.pi/2) + error = error_checker.calculate_error(quat_1, quat_2) + self.assertEqual(error, np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(quat_1, quat_2)) + + def test_revolute_joint_position_error_checker(self): + position_1 = 0.0 + position_2 = 0.0 + error_checker = RevoluteJointPositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = np.pi/2 + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_prismatic_joint_position_error_checker(self): + position_1 = 0.0 + position_2 = 0.0 + error_checker = PrismaticJointPositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = 1.0 + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 1.0) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_list_of_poses_error_checker(self): + poses_1 = [Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])] + poses_2 = [Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])] + error_checker = PoseErrorChecker(is_iterable=True) + error = error_checker.calculate_error(poses_1, poses_2) + self.assertEqual(error, [[0.0, 0.0], [0.0, 0.0]]) + self.assertTrue(error_checker.is_error_acceptable(poses_1, poses_2)) + quat = quaternion_from_euler(0, np.pi/2, 0) + poses_2 = [Pose([0, 1, np.sqrt(3)], quat), + Pose([0, 1, np.sqrt(3)], quat)] + error = error_checker.calculate_error(poses_1, poses_2) + self.assertAlmostEqual(error[0][0], 2, places=5) + self.assertEqual(error[0][1], np.pi/2) + self.assertAlmostEqual(error[1][0], 2, places=5) + self.assertEqual(error[1][1], np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(poses_1, poses_2)) + + def test_multi_joint_error_checker(self): + positions_1 = [0.0, 0.0] + positions_2 = [np.pi/2, 0.1] + joint_types = [JointType.REVOLUTE, JointType.PRISMATIC] + error_checker = MultiJointPositionErrorChecker(joint_types) + error = error_checker.calculate_error(positions_1, positions_2) + self.assertEqual(error, [np.pi/2, 0.1]) + self.assertFalse(error_checker.is_error_acceptable(positions_1, positions_2)) + positions_2 = [np.pi/180, 0.0001] + self.assertTrue(error_checker.is_error_acceptable(positions_1, positions_2)) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py new file mode 100644 index 000000000..6ab6a289f --- /dev/null +++ b/test/test_goal_validator.py @@ -0,0 +1,224 @@ +from tf.transformations import quaternion_from_euler + +from bullet_world_testcase import BulletWorldTestCase +import numpy as np + +from pycram.datastructures.enums import JointType +from pycram.robot_description import RobotDescription +from pycram.worlds.multiverse_functions.goal_validator import GoalValidator +from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ + OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ + MultiJointPositionErrorChecker + +from pycram.datastructures.pose import Pose + + +class TestGoalValidator(BulletWorldTestCase): + + def test_single_pose_goal(self): + milk_goal_pose = Pose([1.3, 1.5, 0.9]) + goal_validator = GoalValidator(milk_goal_pose, self.milk.get_pose, PoseErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertAlmostEqual(goal_validator.current_error[0], 0.5, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], 0, places=5) + self.milk.set_pose(milk_goal_pose) + self.assertEqual(self.milk.get_pose(), milk_goal_pose) + self.assertTrue(goal_validator.goal_achieved) + print(goal_validator.current_error) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], 0, places=5) + + def test_single_position_goal(self): + cereal_goal_position = [1.3, 1.5, 0.95] + goal_validator = GoalValidator(cereal_goal_position, self.cereal.get_position_as_list, PositionErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, 0.8) + self.cereal.set_position(cereal_goal_position) + self.assertEqual(self.cereal.get_position_as_list(), cereal_goal_position) + self.assertTrue(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertEqual(goal_validator.current_error, 0) + + def test_single_orientation_goal(self): + cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi/2) + goal_validator = GoalValidator(cereal_goal_orientation, self.cereal.get_orientation_as_list, + OrientationErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, [np.pi/2]) + self.cereal.set_orientation(cereal_goal_orientation) + for v1, v2 in zip(self.cereal.get_orientation_as_list(), cereal_goal_orientation.tolist()): + self.assertAlmostEqual(v1, v2, places=5) + self.assertTrue(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) + + def test_single_revolute_joint_position_goal(self): + goal_joint_position = -np.pi/4 + goal_validator = GoalValidator(goal_joint_position, + lambda: self.robot.get_joint_position('l_shoulder_lift_joint'), + RevoluteJointPositionErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) + + for percent in [0.5, 1]: + self.robot.set_joint_position('l_shoulder_lift_joint', goal_joint_position*percent) + self.assertEqual(self.robot.get_joint_position('l_shoulder_lift_joint'), goal_joint_position*percent) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + + def test_single_prismatic_joint_position_goal(self): + goal_joint_position = 0.2 + torso = RobotDescription.current_robot_description.torso_joint + goal_validator = GoalValidator(goal_joint_position, + lambda: self.robot.get_joint_position(torso), + PrismaticJointPositionErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) + + for percent in [0.5, 1]: + self.robot.set_joint_position(torso, goal_joint_position*percent) + self.assertEqual(self.robot.get_joint_position(torso), goal_joint_position*percent) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + + def test_multi_joint_goal(self): + goal_joint_positions = [0.2, -np.pi/4] + joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] + joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] + goal_validator = GoalValidator(goal_joint_positions, + lambda: [self.robot.get_joint_position('torso_lift_joint'), + self.robot.get_joint_position('l_shoulder_lift_joint')], + MultiJointPositionErrorChecker(joint_types)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi/4)]), atol=0.001)) + + for percent in [0.5, 1]: + current_joint_positions = [0.2*percent, -np.pi/4*percent] + self.robot.set_joint_positions(dict(zip(joint_names, current_joint_positions))) + self.assertTrue(np.allclose(self.robot.get_joint_position('torso_lift_joint'), current_joint_positions[0], + atol=0.001)) + self.assertTrue(np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_positions[1], + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(0.2)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[1], abs(-np.pi/4)*(1-percent), places=5) + + def test_list_of_poses_goal(self): + position_goal = [0.0, 1.0, 0.0] + orientation_goal = np.array([0, 0, np.pi/2]) + poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), + Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] + goal_validator = GoalValidator(poses_goal, lambda: [self.robot.get_pose(), self.robot.get_pose()], + PoseErrorChecker(is_iterable=True)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, np.pi/2, 1.0, np.pi/2]), atol=0.001)) + + for percent in [0.5, 1]: + current_orientation_goal = orientation_goal * percent + current_pose_goal = Pose([0.0, 1.0*percent, 0.0], + quaternion_from_euler(*current_orientation_goal.tolist())) + self.robot.set_pose(current_pose_goal) + self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_pose_goal.position_as_list(), + atol=0.001)) + self.assertTrue(np.allclose(self.robot.get_orientation_as_list(), current_pose_goal.orientation_as_list(), + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], 1-percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1-percent) /2, places=5) + self.assertAlmostEqual(goal_validator.current_error[2], (1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[3], np.pi * (1-percent) /2, places=5) + + def test_list_of_positions_goal(self): + position_goal = [0.0, 1.0, 0.0] + positions_goal = [position_goal, position_goal] + goal_validator = GoalValidator(positions_goal, lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()], + PositionErrorChecker(is_iterable=True)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) + + for percent in [0.5, 1]: + current_position_goal = [0.0, 1.0*percent, 0.0] + self.robot.set_position(current_position_goal) + self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_position_goal, atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], 1-percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], 1-percent, places=5) + + def test_list_of_orientations_goal(self): + orientation_goal = np.array([0, 0, np.pi/2]) + orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), + quaternion_from_euler(*orientation_goal.tolist())] + goal_validator = GoalValidator(orientations_goals, lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()], + OrientationErrorChecker(is_iterable=True)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi/2, np.pi/2]), atol=0.001)) + + for percent in [0.5, 1]: + current_orientation_goal = orientation_goal * percent + self.robot.set_orientation(quaternion_from_euler(*current_orientation_goal.tolist())) + self.assertTrue(np.allclose(self.robot.get_orientation_as_list(), + quaternion_from_euler(*current_orientation_goal.tolist()), + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], np.pi*(1-percent)/2, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], np.pi*(1-percent)/2, places=5) + + def test_list_of_revolute_joint_positions_goal(self): + goal_joint_position = -np.pi/4 + goal_joint_positions = [goal_joint_position, goal_joint_position] + goal_validator = GoalValidator(goal_joint_positions, + lambda: [self.robot.get_joint_position('l_shoulder_lift_joint'), + self.robot.get_joint_position('l_shoulder_lift_joint')], + RevoluteJointPositionErrorChecker(is_iterable=True)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, + np.array([abs(goal_joint_position), abs(goal_joint_position)]), atol=0.001)) + + for percent in [0.5, 1]: + current_joint_position = goal_joint_position * percent + self.robot.set_joint_position('l_shoulder_lift_joint', current_joint_position) + self.assertTrue(np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_position, atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[1], abs(goal_joint_position)*(1-percent), places=5) + From 516eaecfe8c8a81a8a87b9738635d232fb9aec6a Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 23:13:59 +0200 Subject: [PATCH 05/21] [Multiverse] debugging goal_validator in multiverse. --- src/pycram/worlds/multiverse.py | 39 ++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 511f28ddd..d19b84bf4 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -4,11 +4,12 @@ import numpy as np from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Iterable, Tuple +from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager -from .multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ - MultiPoseGoalValidator, JointGoalValidator, MultiJointGoalValidator +from .multiverse_functions.error_checkers import PoseErrorChecker, MultiJointPositionErrorChecker, \ + RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker +from .multiverse_functions.goal_validator import GoalValidator from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -197,13 +198,15 @@ def _wait_until_multiple_joint_goals_are_achieved(self, joint_positions: Dict[Jo self._wait_until_goal_is_achieved(goal_validator) def _get_multi_joint_goal_validator(self, joint_positions: Dict[Joint, float], - initial_joint_positions: Dict[Joint, float]) -> MultiJointGoalValidator: + initial_joint_positions: Dict[Joint, float]) -> GoalValidator: joints = list(joint_positions.keys()) + joint_types = [joint.type for joint in joints] target_joint_positions = list(joint_positions.values()) initial_joint_positions = list(initial_joint_positions.values()) - goal_validator = MultiJointGoalValidator(initial_joint_positions, target_joint_positions, - lambda: list(self.get_multiple_joint_positions(joints).values()), - joints[0].object_name + "_joint_goal") + goal_validator = GoalValidator(target_joint_positions, + lambda: list(self.get_multiple_joint_positions(joints).values()), + MultiJointPositionErrorChecker(joint_types), + initial_value=initial_joint_positions) return goal_validator def get_joint_position(self, joint: Joint) -> float: @@ -221,8 +224,10 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str def _wait_until_joint_goal_is_achieved(self, joint: Joint, joint_position: float, initial_joint_position: float) -> None: - goal_validator = JointGoalValidator(initial_joint_position, joint_position, - lambda: self.get_joint_position(joint), joint.name + "_joint_goal") + error_checker = RevoluteJointPositionErrorChecker() if joint.type == JointType.REVOLUTE else \ + PrismaticJointPositionErrorChecker() + goal_validator = GoalValidator(joint_position, lambda: self.get_joint_position(joint), + error_checker, initial_value=initial_joint_position) self._wait_until_goal_is_achieved(goal_validator) def get_link_pose(self, link: Link) -> Pose: @@ -260,16 +265,14 @@ def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_po initial_poses) def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: List[Pose], - initial_poses: List[Pose], - name: Optional[str] = "all_poses_goal") -> None: + initial_poses: List[Pose]) -> None: """ Wait until all poses are set to the desired poses. param poses: The dictionary of the desired poses param initial_poses: The dictionary of the initial poses - param name: The name of the goal. """ - goal_validator = MultiPoseGoalValidator(initial_poses, poses, - lambda: list(self._get_multiple_body_poses(body_names).values()), name) + goal_validator = GoalValidator(poses, lambda: list(self._get_multiple_body_poses(body_names).values()), + PoseErrorChecker(), initial_value=initial_poses) self._wait_until_goal_is_achieved(goal_validator) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: @@ -310,8 +313,8 @@ def _wait_until_pose_goal_is_achieved(self, body_name: str, target_pose: Pose, i param target_pose: The target pose of the body. param initial_pose: The initial pose of the body. """ - goal_validator = PoseGoalValidator(initial_pose, target_pose, lambda: self._get_body_pose(body_name), - body_name + "_pose_goal") + goal_validator = GoalValidator(target_pose, lambda: self._get_body_pose(body_name), + PoseErrorChecker(), initial_value=initial_pose) self._wait_until_goal_is_achieved(goal_validator) def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: @@ -324,8 +327,8 @@ def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: while not goal_validator.goal_achieved: sleep(0.01) if time() - start_time > self.reader.MAX_WAIT_TIME_FOR_DATA: - msg = f"Failed to achieve {goal_validator.name} from {goal_validator.initial_value} to" \ - f" {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}" \ + msg = f"Failed to achieve goal from initial error {goal_validator.initial_error} with" \ + f" goal {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}" \ f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage" \ f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" logging.error(msg) From b27049803c31b1b8df0ed226c4668431840212aa Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 23 Jul 2024 03:51:47 +0200 Subject: [PATCH 06/21] [Multiverse] debugging goal_validator in multiverse, attach is working. --- src/pycram/datastructures/world.py | 2 +- src/pycram/world_concepts/world_object.py | 3 ++- src/pycram/worlds/multiverse.py | 2 +- src/pycram/worlds/multiverse_communication/clients.py | 4 ++-- src/pycram/worlds/multiverse_functions/goal_validator.py | 2 +- 5 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 30aa53045..41293c562 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -951,7 +951,7 @@ def reset_world_and_remove_objects(self, exclude_objects: Optional[List[Object]] :param exclude_objects: A list of objects that should not be removed. """ self.reset_world() - objs_copy = copy(self.objects) + objs_copy = [obj for obj in self.objects] exclude_objects = [] if exclude_objects is None else exclude_objects [self.remove_object(obj) for obj in objs_copy if obj not in exclude_objects] diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 843c70c3a..a719fff8d 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -456,7 +456,8 @@ def attach(self, if coincide_the_objects: parent_to_child_transform = Transform() else: - parent_to_child_transform = parent_link.get_transform_to_link(child_link) + # parent_to_child_transform = parent_link.get_transform_to_link(child_link) + parent_to_child_transform = None attachment = Attachment(parent_link, child_link, bidirectional, parent_to_child_transform) self.attachments[child_object] = attachment diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index d19b84bf4..908a7143b 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -272,7 +272,7 @@ def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: param initial_poses: The dictionary of the initial poses """ goal_validator = GoalValidator(poses, lambda: list(self._get_multiple_body_poses(body_names).values()), - PoseErrorChecker(), initial_value=initial_poses) + PoseErrorChecker(is_iterable=True), initial_value=initial_poses) self._wait_until_goal_is_achieved(goal_validator) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 0feb5e1ff..6e3f9f113 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -306,8 +306,8 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List """ self.send_body_data_to_server(body_name, {"position": position, - "quaternion": orientation, - "relative_velocity": [0]*6}) + "quaternion": orientation} + ) def set_body_position(self, body_name: str, position: List[float]) -> None: """ diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 4a6bc38b0..db7852d8a 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -11,7 +11,7 @@ class GoalValidator: def __init__(self, goal_value: Any, current_value_getter: Callable[[], Any], error_checker: ErrorChecker, initial_value: Any = None, - acceptable_percentage_of_goal_achieved: Optional[float] = None): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. :param goal_value: The goal value. From 9d7f7390ae07b589176653bace59f0e755366757 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jul 2024 15:16:41 +0200 Subject: [PATCH 07/21] [Multiverse] updated goal validator with register goal value function. --- .../multiverse_functions/error_checkers.py | 37 +++++----- .../multiverse_functions/goal_validator.py | 70 ++++++++++++++----- test/test_goal_validator.py | 51 ++++++++------ 3 files changed, 103 insertions(+), 55 deletions(-) diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_functions/error_checkers.py index 94af1efc7..cf85425d9 100644 --- a/src/pycram/worlds/multiverse_functions/error_checkers.py +++ b/src/pycram/worlds/multiverse_functions/error_checkers.py @@ -3,7 +3,7 @@ import numpy as np from tf.transformations import quaternion_multiply, quaternion_inverse -from typing_extensions import List, Union, Optional, Any, Iterable as TypingIterable, Sized +from typing_extensions import List, Union, Optional, Any, Sized, Iterable as T_Iterable from pycram.datastructures.enums import JointType from pycram.datastructures.pose import Pose @@ -14,32 +14,34 @@ class ErrorChecker(ABC): An abstract class that resembles an error checker. It has two main methods, one for calculating the error between two values and another for checking if the error is acceptable. """ - def __init__(self, acceptable_error: Union[float, TypingIterable[float]], is_iterable: Optional[bool] = False): + def __init__(self, acceptable_error: Union[float, T_Iterable[float]], is_iterable: Optional[bool] = False): """ Initialize the error checker. :param acceptable_error: The acceptable error. :param is_iterable: Whether the error is iterable (i.e. list of errors). """ - self.acceptable_error = np.array(acceptable_error) + self.acceptable_error: np.ndarray = np.array(acceptable_error) + self.tiled_acceptable_error: Optional[np.ndarray] = None self.is_iterable = is_iterable - def get_acceptable_error(self, tile_to_match: Optional[Sized] = None) -> np.ndarray: + def update_acceptable_error(self, new_acceptable_error: Optional[T_Iterable[float]] = None, + tile_to_match: Optional[Sized] = None,) -> None: """ - Get the acceptable error. - :return: The acceptable error. + Update the acceptable error with a new value, and tile it to match the length of the error if needed. """ + if new_acceptable_error is not None: + self.acceptable_error = np.array(new_acceptable_error) if tile_to_match is not None and self.is_iterable: - return self.tiled_acceptable_error(tile_to_match) - else: - return self.acceptable_error + self.update_tiled_acceptable_error(tile_to_match) - def tiled_acceptable_error(self, tile_to_match: Sized) -> np.ndarray: + def update_tiled_acceptable_error(self, tile_to_match: Sized) -> None: """ Tile the acceptable error to match the length of the error. :param tile_to_match: The object to match the length of the error. :return: The tiled acceptable error. """ - return np.tile(np.array(self.acceptable_error).flatten(), len(tile_to_match) // self.acceptable_error.size) + self.tiled_acceptable_error = np.tile(self.acceptable_error.flatten(), + len(tile_to_match) // self.acceptable_error.size) @abstractmethod def _calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: @@ -73,14 +75,17 @@ def is_error_acceptable(self, value_1: Any, value_2: Any) -> bool: error = self.calculate_error(value_1, value_2) if self.is_iterable: error = np.array(error).flatten() - return np.all(error <= self.tiled_acceptable_error(error)) + if self.tiled_acceptable_error is None or\ + len(error) != len(self.tiled_acceptable_error): + self.update_tiled_acceptable_error(error) + return np.all(error <= self.tiled_acceptable_error) else: return is_error_acceptable(error, self.acceptable_error) class PoseErrorChecker(ErrorChecker): - def __init__(self, acceptable_error: Union[float, TypingIterable[float]] = (1e-3, np.pi / 180), + def __init__(self, acceptable_error: Union[float, T_Iterable[float]] = (1e-3, np.pi / 180), is_iterable: Optional[bool] = False): super().__init__(acceptable_error, is_iterable) @@ -117,7 +122,7 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: class MultiJointPositionErrorChecker(ErrorChecker): - def __init__(self, joint_types: List[JointType], acceptable_error: Optional[List[float]] = None): + def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_Iterable[float]] = None): self.joint_types = joint_types if acceptable_error is None: acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] @@ -187,8 +192,8 @@ def calculate_joint_position_error(joint_position_1: float, joint_position_2: fl return abs(joint_position_1 - joint_position_2) -def is_error_acceptable(error: Union[float, TypingIterable[float]], - acceptable_error: Union[float, TypingIterable[float]]) -> bool: +def is_error_acceptable(error: Union[float, T_Iterable[float]], + acceptable_error: Union[float, T_Iterable[float]]) -> bool: """ Check if the error is acceptable. :param error: The error. diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index db7852d8a..0483dc3cd 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,5 +1,5 @@ import numpy as np -from typing_extensions import Any, Callable, Optional, List, Union +from typing_extensions import Any, Callable, Optional, Union, Iterable from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker @@ -9,27 +9,65 @@ class GoalValidator: A class to validate the goal by tracking the goal achievement progress. """ - def __init__(self, goal_value: Any, current_value_getter: Callable[[], Any], error_checker: ErrorChecker, - initial_value: Any = None, + def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[], Any], acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. - :param goal_value: The goal value. + :param error_checker: The error checker. :param current_value_getter: The current value getter. - :param error_checker: The error checker object. - :param initial_value: The initial value. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved, if given, will be used to check if this percentage is achieved instead of the complete goal. """ self.error_checker: ErrorChecker = error_checker self.current_value_getter: Callable[[], Any] = current_value_getter - self.goal_value: Any = goal_value + self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved + self.goal_value: Any = None + self.initial_error: Optional[np.ndarray] = None + + @property + def _acceptable_error(self) -> np.ndarray: + """ + Get the acceptable error. + """ + if self.error_checker.is_iterable: + return self.tiled_acceptable_error + else: + return self.acceptable_error + + @property + def acceptable_error(self) -> np.ndarray: + """ + Get the acceptable error. + """ + return self.error_checker.acceptable_error + + @property + def tiled_acceptable_error(self) -> Optional[np.ndarray]: + """ + Get the tiled acceptable error. + """ + return self.error_checker.tiled_acceptable_error + + def register_goal_value(self, goal_value: Any, initial_value: Optional[Any] = None, + acceptable_error: Optional[Union[float, Iterable[float]]] = None): + """ + Register the goal value. + :param goal_value: The goal value. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + """ + self.goal_value = goal_value + self.update_initial_error(goal_value, initial_value=initial_value) + self.error_checker.update_acceptable_error(acceptable_error, self.initial_error) + + def update_initial_error(self, goal_value: Any, initial_value: Optional[Any] = None) -> None: + """ + Calculate the initial error. + """ if initial_value is None: - self.initial_error: List[Union[float, List[float]]] = self.current_error + self.initial_error: np.ndarray = self.current_error else: - self.initial_error: List[Union[float, List[float]]] = self.calculate_error(goal_value, initial_value) - self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved - self.acceptable_error = self.error_checker.get_acceptable_error(self.initial_error) + self.initial_error: np.ndarray = self.calculate_error(goal_value, initial_value) @property def current_value(self) -> Any: @@ -57,7 +95,7 @@ def percentage_of_goal_achieved(self) -> float: Calculate the percentage of goal achieved. """ percent_array = 1 - self.relative_current_error / self.relative_initial_error - percent_array_filtered = percent_array[self.relative_initial_error > self.acceptable_error] + percent_array_filtered = percent_array[self.relative_initial_error > self._acceptable_error] if len(percent_array_filtered) == 0: return 1 else: @@ -69,7 +107,7 @@ def actual_percentage_of_goal_achieved(self) -> float: Calculate the percentage of goal achieved. """ percent_array = 1 - self.current_error / np.maximum(self.initial_error, 1e-3) - percent_array_filtered = percent_array[self.initial_error > self.acceptable_error] + percent_array_filtered = percent_array[self.initial_error > self._acceptable_error] if len(percent_array_filtered) == 0: return 1 else: @@ -96,7 +134,7 @@ def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> n :param error: The error. :param threshold: The threshold. """ - return np.maximum(error-self.acceptable_error, threshold) + return np.maximum(error-self._acceptable_error, threshold) @property def goal_achieved(self) -> bool: @@ -105,12 +143,12 @@ def goal_achieved(self) -> bool: return: Whether the goal is achieved. """ if self.acceptable_percentage_of_goal_achieved is None: - return self.is_error_acceptable + return self.is_current_error_acceptable else: return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved @property - def is_error_acceptable(self) -> bool: + def is_current_error_acceptable(self) -> bool: """ Check if the error is acceptable. return: Whether the error is acceptable. diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index 6ab6a289f..f2c115315 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -17,7 +17,8 @@ class TestGoalValidator(BulletWorldTestCase): def test_single_pose_goal(self): milk_goal_pose = Pose([1.3, 1.5, 0.9]) - goal_validator = GoalValidator(milk_goal_pose, self.milk.get_pose, PoseErrorChecker()) + goal_validator = GoalValidator(PoseErrorChecker(), self.milk.get_pose) + goal_validator.register_goal_value(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertAlmostEqual(goal_validator.current_error[0], 0.5, places=5) @@ -32,7 +33,8 @@ def test_single_pose_goal(self): def test_single_position_goal(self): cereal_goal_position = [1.3, 1.5, 0.95] - goal_validator = GoalValidator(cereal_goal_position, self.cereal.get_position_as_list, PositionErrorChecker()) + goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) + goal_validator.register_goal_value(cereal_goal_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, 0.8) @@ -44,8 +46,8 @@ def test_single_position_goal(self): def test_single_orientation_goal(self): cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi/2) - goal_validator = GoalValidator(cereal_goal_orientation, self.cereal.get_orientation_as_list, - OrientationErrorChecker()) + goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) + goal_validator.register_goal_value(cereal_goal_orientation) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, [np.pi/2]) @@ -58,9 +60,9 @@ def test_single_orientation_goal(self): def test_single_revolute_joint_position_goal(self): goal_joint_position = -np.pi/4 - goal_validator = GoalValidator(goal_joint_position, - lambda: self.robot.get_joint_position('l_shoulder_lift_joint'), - RevoluteJointPositionErrorChecker()) + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), + lambda: self.robot.get_joint_position('l_shoulder_lift_joint')) + goal_validator.register_goal_value(goal_joint_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -78,9 +80,9 @@ def test_single_revolute_joint_position_goal(self): def test_single_prismatic_joint_position_goal(self): goal_joint_position = 0.2 torso = RobotDescription.current_robot_description.torso_joint - goal_validator = GoalValidator(goal_joint_position, - lambda: self.robot.get_joint_position(torso), - PrismaticJointPositionErrorChecker()) + goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), + lambda: self.robot.get_joint_position(torso)) + goal_validator.register_goal_value(goal_joint_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -99,10 +101,10 @@ def test_multi_joint_goal(self): goal_joint_positions = [0.2, -np.pi/4] joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] - goal_validator = GoalValidator(goal_joint_positions, + goal_validator = GoalValidator(MultiJointPositionErrorChecker(joint_types), lambda: [self.robot.get_joint_position('torso_lift_joint'), - self.robot.get_joint_position('l_shoulder_lift_joint')], - MultiJointPositionErrorChecker(joint_types)) + self.robot.get_joint_position('l_shoulder_lift_joint')]) + goal_validator.register_goal_value(goal_joint_positions) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi/4)]), atol=0.001)) @@ -127,8 +129,9 @@ def test_list_of_poses_goal(self): orientation_goal = np.array([0, 0, np.pi/2]) poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] - goal_validator = GoalValidator(poses_goal, lambda: [self.robot.get_pose(), self.robot.get_pose()], - PoseErrorChecker(is_iterable=True)) + goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), + lambda: [self.robot.get_pose(), self.robot.get_pose()]) + goal_validator.register_goal_value(poses_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, np.pi/2, 1.0, np.pi/2]), atol=0.001)) @@ -155,8 +158,9 @@ def test_list_of_poses_goal(self): def test_list_of_positions_goal(self): position_goal = [0.0, 1.0, 0.0] positions_goal = [position_goal, position_goal] - goal_validator = GoalValidator(positions_goal, lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()], - PositionErrorChecker(is_iterable=True)) + goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), + lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) + goal_validator.register_goal_value(positions_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) @@ -177,9 +181,10 @@ def test_list_of_orientations_goal(self): orientation_goal = np.array([0, 0, np.pi/2]) orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), quaternion_from_euler(*orientation_goal.tolist())] - goal_validator = GoalValidator(orientations_goals, lambda: [self.robot.get_orientation_as_list(), - self.robot.get_orientation_as_list()], - OrientationErrorChecker(is_iterable=True)) + goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), + lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()]) + goal_validator.register_goal_value(orientations_goals) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi/2, np.pi/2]), atol=0.001)) @@ -201,10 +206,10 @@ def test_list_of_orientations_goal(self): def test_list_of_revolute_joint_positions_goal(self): goal_joint_position = -np.pi/4 goal_joint_positions = [goal_joint_position, goal_joint_position] - goal_validator = GoalValidator(goal_joint_positions, + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), lambda: [self.robot.get_joint_position('l_shoulder_lift_joint'), - self.robot.get_joint_position('l_shoulder_lift_joint')], - RevoluteJointPositionErrorChecker(is_iterable=True)) + self.robot.get_joint_position('l_shoulder_lift_joint')]) + goal_validator.register_goal_value(goal_joint_positions) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, From 81b96ec219d42b3f3c0ad95d639433bdf5f63873 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jul 2024 16:48:07 +0200 Subject: [PATCH 08/21] [Multiverse] updated register goal value function to also take current_value_getter_input. --- src/pycram/datastructures/world.py | 23 ++-- src/pycram/world_concepts/world_object.py | 9 ++ src/pycram/worlds/bullet_world.py | 3 + src/pycram/worlds/multiverse.py | 4 +- .../multiverse_functions/goal_validator.py | 25 ++-- test/test_goal_validator.py | 117 +++++++++--------- 6 files changed, 104 insertions(+), 77 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 41293c562..8051313e5 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -606,6 +606,21 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: """ pass + @abstractmethod + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + """ + Set the positions of multiple joints of an articulated object. + :param joint_positions: A dictionary with joint objects as keys and joint positions as values. + """ + pass + + @abstractmethod + def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: + """ + Get the positions of multiple joints of an articulated object. + """ + pass + @abstractmethod def reset_object_base_pose(self, obj: Object, pose: Pose): """ @@ -1195,14 +1210,6 @@ def resume_world_sync(self) -> None: def __del__(self): self.exit() - @abstractmethod - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: - """ - Set the positions of multiple joints of an articulated object. - :param joint_positions: A dictionary with joint objects as keys and joint positions as values. - """ - pass - class UseProspectionWorld: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index a719fff8d..d08918b56 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -984,6 +984,15 @@ def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") return container_joint + def get_multiple_joint_positions(self, joint_names: List[str]) -> Dict[str, float]: + """ + Returns the positions of multiple joints at once. + + :param joint_names: A list of joint names. + :return: A dictionary with the joint names as keys and the joint positions as values. + """ + return self.world.get_multiple_joint_positions([self.joints[joint_name] for joint_name in joint_names]) + def get_positions_of_all_joints(self) -> Dict[str, float]: """ Returns the positions of all joints of the object as a dictionary of joint names and joint positions. diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index f18753979..8be0deaec 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -171,6 +171,9 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> N for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) + def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: + return {joint.name: self.get_joint_position(joint) for joint in joints} + def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: float) -> None: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 908a7143b..b4aabd858 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -313,8 +313,8 @@ def _wait_until_pose_goal_is_achieved(self, body_name: str, target_pose: Pose, i param target_pose: The target pose of the body. param initial_pose: The initial pose of the body. """ - goal_validator = GoalValidator(target_pose, lambda: self._get_body_pose(body_name), - PoseErrorChecker(), initial_value=initial_pose) + goal_validator = GoalValidator(PoseErrorChecker(), lambda: self._get_body_pose(body_name)) + goal_validator.register_goal(target_pose, initial_pose) self._wait_until_goal_is_achieved(goal_validator) def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 0483dc3cd..b6fedf2fb 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,5 +1,5 @@ import numpy as np -from typing_extensions import Any, Callable, Optional, Union, Iterable +from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, Protocol from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker @@ -9,20 +9,22 @@ class GoalValidator: A class to validate the goal by tracking the goal achievement progress. """ - def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[], Any], + def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[Optional[Any]], Any], acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. :param error_checker: The error checker. - :param current_value_getter: The current value getter. + :param current_value_getter: The current value getter function which takes an optional input and returns the + current value. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved, if given, will be used to check if this percentage is achieved instead of the complete goal. """ self.error_checker: ErrorChecker = error_checker - self.current_value_getter: Callable[[], Any] = current_value_getter + self.current_value_getter: Callable[[Optional[Any]], Any] = current_value_getter self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved - self.goal_value: Any = None + self.goal_value: Optional[Any] = None self.initial_error: Optional[np.ndarray] = None + self.current_value_getter_input: Optional[Any] = None @property def _acceptable_error(self) -> np.ndarray: @@ -48,15 +50,19 @@ def tiled_acceptable_error(self) -> Optional[np.ndarray]: """ return self.error_checker.tiled_acceptable_error - def register_goal_value(self, goal_value: Any, initial_value: Optional[Any] = None, - acceptable_error: Optional[Union[float, Iterable[float]]] = None): + def register_goal(self, goal_value: Any, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Union[float, Iterable[float]]] = None): """ Register the goal value. :param goal_value: The goal value. + :param current_value_getter_input: The values that are used as input to the current value getter. :param initial_value: The initial value. :param acceptable_error: The acceptable error. """ self.goal_value = goal_value + self.current_value_getter_input = current_value_getter_input self.update_initial_error(goal_value, initial_value=initial_value) self.error_checker.update_acceptable_error(acceptable_error, self.initial_error) @@ -74,7 +80,10 @@ def current_value(self) -> Any: """ Get the current value. """ - return self.current_value_getter() + if self.current_value_getter_input is not None: + return self.current_value_getter(self.current_value_getter_input) + else: + return self.current_value_getter() @property def current_error(self) -> np.ndarray: diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index f2c115315..5f8c6b1a2 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -1,16 +1,14 @@ +import numpy as np from tf.transformations import quaternion_from_euler from bullet_world_testcase import BulletWorldTestCase -import numpy as np - from pycram.datastructures.enums import JointType +from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescription -from pycram.worlds.multiverse_functions.goal_validator import GoalValidator from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ MultiJointPositionErrorChecker - -from pycram.datastructures.pose import Pose +from pycram.worlds.multiverse_functions.goal_validator import GoalValidator class TestGoalValidator(BulletWorldTestCase): @@ -18,7 +16,7 @@ class TestGoalValidator(BulletWorldTestCase): def test_single_pose_goal(self): milk_goal_pose = Pose([1.3, 1.5, 0.9]) goal_validator = GoalValidator(PoseErrorChecker(), self.milk.get_pose) - goal_validator.register_goal_value(milk_goal_pose) + goal_validator.register_goal(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertAlmostEqual(goal_validator.current_error[0], 0.5, places=5) @@ -34,7 +32,7 @@ def test_single_pose_goal(self): def test_single_position_goal(self): cereal_goal_position = [1.3, 1.5, 0.95] goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) - goal_validator.register_goal_value(cereal_goal_position) + goal_validator.register_goal(cereal_goal_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, 0.8) @@ -45,12 +43,12 @@ def test_single_position_goal(self): self.assertEqual(goal_validator.current_error, 0) def test_single_orientation_goal(self): - cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi/2) + cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi / 2) goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) - goal_validator.register_goal_value(cereal_goal_orientation) + goal_validator.register_goal(cereal_goal_orientation) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertEqual(goal_validator.current_error, [np.pi/2]) + self.assertEqual(goal_validator.current_error, [np.pi / 2]) self.cereal.set_orientation(cereal_goal_orientation) for v1, v2 in zip(self.cereal.get_orientation_as_list(), cereal_goal_orientation.tolist()): self.assertAlmostEqual(v1, v2, places=5) @@ -59,86 +57,87 @@ def test_single_orientation_goal(self): self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) def test_single_revolute_joint_position_goal(self): - goal_joint_position = -np.pi/4 + goal_joint_position = -np.pi / 4 goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), lambda: self.robot.get_joint_position('l_shoulder_lift_joint')) - goal_validator.register_goal_value(goal_joint_position) + goal_validator.register_goal(goal_joint_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) for percent in [0.5, 1]: - self.robot.set_joint_position('l_shoulder_lift_joint', goal_joint_position*percent) - self.assertEqual(self.robot.get_joint_position('l_shoulder_lift_joint'), goal_joint_position*percent) + self.robot.set_joint_position('l_shoulder_lift_joint', goal_joint_position * percent) + self.assertEqual(self.robot.get_joint_position('l_shoulder_lift_joint'), goal_joint_position * percent) if percent == 1: self.assertTrue(goal_validator.goal_achieved) else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) def test_single_prismatic_joint_position_goal(self): goal_joint_position = 0.2 torso = RobotDescription.current_robot_description.torso_joint goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), lambda: self.robot.get_joint_position(torso)) - goal_validator.register_goal_value(goal_joint_position) + goal_validator.register_goal(goal_joint_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) for percent in [0.5, 1]: - self.robot.set_joint_position(torso, goal_joint_position*percent) - self.assertEqual(self.robot.get_joint_position(torso), goal_joint_position*percent) + self.robot.set_joint_position(torso, goal_joint_position * percent) + self.assertEqual(self.robot.get_joint_position(torso), goal_joint_position * percent) if percent == 1: self.assertTrue(goal_validator.goal_achieved) else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) def test_multi_joint_goal(self): - goal_joint_positions = [0.2, -np.pi/4] + goal_joint_positions = np.array([0.2, -np.pi / 4]) joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] goal_validator = GoalValidator(MultiJointPositionErrorChecker(joint_types), - lambda: [self.robot.get_joint_position('torso_lift_joint'), - self.robot.get_joint_position('l_shoulder_lift_joint')]) - goal_validator.register_goal_value(goal_joint_positions) + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi/4)]), atol=0.001)) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi / 4)]), atol=0.001)) for percent in [0.5, 1]: - current_joint_positions = [0.2*percent, -np.pi/4*percent] - self.robot.set_joint_positions(dict(zip(joint_names, current_joint_positions))) + current_joint_positions = goal_joint_positions * percent + self.robot.set_joint_positions(dict(zip(joint_names, current_joint_positions.tolist()))) self.assertTrue(np.allclose(self.robot.get_joint_position('torso_lift_joint'), current_joint_positions[0], atol=0.001)) - self.assertTrue(np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_positions[1], - atol=0.001)) + self.assertTrue( + np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_positions[1], + atol=0.001)) if percent == 1: self.assertTrue(goal_validator.goal_achieved) else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(0.2)*(1-percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[1], abs(-np.pi/4)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(0.2) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[1], abs(-np.pi / 4) * (1 - percent), places=5) def test_list_of_poses_goal(self): position_goal = [0.0, 1.0, 0.0] - orientation_goal = np.array([0, 0, np.pi/2]) + orientation_goal = np.array([0, 0, np.pi / 2]) poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), lambda: [self.robot.get_pose(), self.robot.get_pose()]) - goal_validator.register_goal_value(poses_goal) + goal_validator.register_goal(poses_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, np.pi/2, 1.0, np.pi/2]), atol=0.001)) + self.assertTrue( + np.allclose(goal_validator.current_error, np.array([1.0, np.pi / 2, 1.0, np.pi / 2]), atol=0.001)) for percent in [0.5, 1]: current_orientation_goal = orientation_goal * percent - current_pose_goal = Pose([0.0, 1.0*percent, 0.0], + current_pose_goal = Pose([0.0, 1.0 * percent, 0.0], quaternion_from_euler(*current_orientation_goal.tolist())) self.robot.set_pose(current_pose_goal) self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_pose_goal.position_as_list(), @@ -150,23 +149,23 @@ def test_list_of_poses_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], 1-percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1-percent) /2, places=5) - self.assertAlmostEqual(goal_validator.current_error[2], (1-percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[3], np.pi * (1-percent) /2, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error[2], (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[3], np.pi * (1 - percent) / 2, places=5) def test_list_of_positions_goal(self): position_goal = [0.0, 1.0, 0.0] positions_goal = [position_goal, position_goal] goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) - goal_validator.register_goal_value(positions_goal) + goal_validator.register_goal(positions_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) for percent in [0.5, 1]: - current_position_goal = [0.0, 1.0*percent, 0.0] + current_position_goal = [0.0, 1.0 * percent, 0.0] self.robot.set_position(current_position_goal) self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_position_goal, atol=0.001)) if percent == 1: @@ -174,20 +173,20 @@ def test_list_of_positions_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], 1-percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], 1-percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], 1 - percent, places=5) def test_list_of_orientations_goal(self): - orientation_goal = np.array([0, 0, np.pi/2]) + orientation_goal = np.array([0, 0, np.pi / 2]) orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), - quaternion_from_euler(*orientation_goal.tolist())] + quaternion_from_euler(*orientation_goal.tolist())] goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), lambda: [self.robot.get_orientation_as_list(), self.robot.get_orientation_as_list()]) - goal_validator.register_goal_value(orientations_goals) + goal_validator.register_goal(orientations_goals) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi/2, np.pi/2]), atol=0.001)) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi / 2, np.pi / 2]), atol=0.001)) for percent in [0.5, 1]: current_orientation_goal = orientation_goal * percent @@ -200,30 +199,30 @@ def test_list_of_orientations_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], np.pi*(1-percent)/2, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], np.pi*(1-percent)/2, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1 - percent) / 2, places=5) def test_list_of_revolute_joint_positions_goal(self): - goal_joint_position = -np.pi/4 - goal_joint_positions = [goal_joint_position, goal_joint_position] + goal_joint_position = -np.pi / 4 + goal_joint_positions = np.array([goal_joint_position, goal_joint_position]) + joint_names = ['l_shoulder_lift_joint', 'r_shoulder_lift_joint'] goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), - lambda: [self.robot.get_joint_position('l_shoulder_lift_joint'), - self.robot.get_joint_position('l_shoulder_lift_joint')]) - goal_validator.register_goal_value(goal_joint_positions) + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([abs(goal_joint_position), abs(goal_joint_position)]), atol=0.001)) for percent in [0.5, 1]: - current_joint_position = goal_joint_position * percent - self.robot.set_joint_position('l_shoulder_lift_joint', current_joint_position) - self.assertTrue(np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_position, atol=0.001)) + current_joint_position = goal_joint_positions * percent + self.robot.set_joint_positions(dict(zip(joint_names, current_joint_position))) + self.assertTrue(np.allclose(list(self.robot.get_multiple_joint_positions(joint_names).values()), + current_joint_position, atol=0.001)) if percent == 1: self.assertTrue(goal_validator.goal_achieved) else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[1], abs(goal_joint_position)*(1-percent), places=5) - + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[1], abs(goal_joint_position) * (1 - percent), places=5) From 77a17579bfae0ffc8c4bb983cb99ad79546b0e69 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jul 2024 16:54:59 +0200 Subject: [PATCH 09/21] [Multiverse] updated register goal value function to also take current_value_getter_input. --- test/test_goal_validator.py | 53 ++++++++++++++++++++----------------- 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index 5f8c6b1a2..a5be07e65 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -19,15 +19,15 @@ def test_single_pose_goal(self): goal_validator.register_goal(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertAlmostEqual(goal_validator.current_error[0], 0.5, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0.5, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) self.milk.set_pose(milk_goal_pose) self.assertEqual(self.milk.get_pose(), milk_goal_pose) self.assertTrue(goal_validator.goal_achieved) print(goal_validator.current_error) self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) - self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) def test_single_position_goal(self): cereal_goal_position = [1.3, 1.5, 0.95] @@ -54,13 +54,13 @@ def test_single_orientation_goal(self): self.assertAlmostEqual(v1, v2, places=5) self.assertTrue(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) - self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) def test_single_revolute_joint_position_goal(self): goal_joint_position = -np.pi / 4 - goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), - lambda: self.robot.get_joint_position('l_shoulder_lift_joint')) - goal_validator.register_goal(goal_joint_position) + joint_name = 'l_shoulder_lift_joint' + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), self.robot.get_joint_position) + goal_validator.register_goal(goal_joint_position, joint_name) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -73,14 +73,14 @@ def test_single_revolute_joint_position_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) def test_single_prismatic_joint_position_goal(self): goal_joint_position = 0.2 torso = RobotDescription.current_robot_description.torso_joint - goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), - lambda: self.robot.get_joint_position(torso)) - goal_validator.register_goal(goal_joint_position) + goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), self.robot.get_joint_position) + goal_validator.register_goal(goal_joint_position, torso) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -93,7 +93,8 @@ def test_single_prismatic_joint_position_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) def test_multi_joint_goal(self): goal_joint_positions = np.array([0.2, -np.pi / 4]) @@ -119,8 +120,8 @@ def test_multi_joint_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(0.2) * (1 - percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[1], abs(-np.pi / 4) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(0.2) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], abs(-np.pi / 4) * (1 - percent), places=5) def test_list_of_poses_goal(self): position_goal = [0.0, 1.0, 0.0] @@ -149,10 +150,10 @@ def test_list_of_poses_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], 1 - percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1 - percent) / 2, places=5) - self.assertAlmostEqual(goal_validator.current_error[2], (1 - percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[3], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[2], (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[3], np.pi * (1 - percent) / 2, places=5) def test_list_of_positions_goal(self): position_goal = [0.0, 1.0, 0.0] @@ -173,8 +174,8 @@ def test_list_of_positions_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], 1 - percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 1 - percent, places=5) def test_list_of_orientations_goal(self): orientation_goal = np.array([0, 0, np.pi / 2]) @@ -199,8 +200,8 @@ def test_list_of_orientations_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], np.pi * (1 - percent) / 2, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], np.pi * (1 - percent) / 2, places=5) def test_list_of_revolute_joint_positions_goal(self): goal_joint_position = -np.pi / 4 @@ -224,5 +225,7 @@ def test_list_of_revolute_joint_positions_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[1], abs(goal_joint_position) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], abs(goal_joint_position) * (1 - percent), + places=5) From 0724c89390b8e093bee8622a3af9e557b3b40358 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jul 2024 22:32:45 +0200 Subject: [PATCH 10/21] [Multiverse] tests are working, demo sometimes fails due to wierd behaviour of bowl object. --- src/pycram/datastructures/world.py | 153 ++++++++++- src/pycram/world_concepts/world_object.py | 48 +++- src/pycram/worlds/bullet_world.py | 37 ++- src/pycram/worlds/multiverse.py | 150 +++++------ .../multiverse_communication/clients.py | 34 +-- .../multiverse_functions/error_checkers.py | 52 ++-- .../multiverse_functions/goal_validator.py | 237 +++++++++++++++++- test/test_goal_validator.py | 136 ++++++++-- test/test_multiverse.py | 2 - 9 files changed, 689 insertions(+), 160 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 8051313e5..e6adba567 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -7,7 +7,6 @@ from abc import ABC, abstractmethod from copy import copy - import numpy as np import rospy from geometry_msgs.msg import Point @@ -28,6 +27,9 @@ from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event +from ..worlds.multiverse_functions.goal_validator import GoalValidator, MultiPoseGoalValidator, \ + MultiPositionGoalValidator, MultiOrientationGoalValidator, PoseGoalValidator, PositionGoalValidator, \ + OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator if TYPE_CHECKING: from ..world_concepts.world_object import Object @@ -158,6 +160,13 @@ class World(StateEntity, ABC): The prefix for the prospection world name. """ + acceptable_position_error: float = 1e-2 + acceptable_orientation_error: float = 5*np.pi / 180 + acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) + """ + The acceptable error for the position and orientation of an object. + """ + def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): """ Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the @@ -204,6 +213,45 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._current_state: Optional[WorldState] = None + self._init_goal_validators() + + def _init_goal_validators(self): + """ + Initializes the goal validators for the World objects' poses, positions, and orientations. + """ + # Goal validators for multiple objects + self.multi_pose_goal_validator = MultiPoseGoalValidator( + lambda x: list(self.get_multiple_object_poses(x).values()), self.acceptable_pose_error) + self.multi_position_goal_validator = MultiPositionGoalValidator( + lambda x: list(self.get_multiple_object_positions(x).values()), self.acceptable_position_error) + self.multi_orientation_goal_validator = MultiOrientationGoalValidator( + lambda x: list(self.get_multiple_object_orientations(x).values()), self.acceptable_orientation_error) + + # Goal validators for an object + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error) + self.position_goal_validator = PositionGoalValidator(self.get_object_position, self.acceptable_position_error) + self.orientation_goal_validator = OrientationGoalValidator(self.get_object_orientation, + self.acceptable_orientation_error) + + # Goal validators for the links of an object + self.link_pose_goal_validator = PoseGoalValidator(self.get_link_pose, self.acceptable_pose_error) + self.link_position_goal_validator = PositionGoalValidator(self.get_link_position, + self.acceptable_position_error) + self.link_orientation_goal_validator = OrientationGoalValidator(self.get_link_orientation, + self.acceptable_orientation_error) + + self.multi_link_pose_goal_validator = MultiPoseGoalValidator( + lambda x: list(self.get_multiple_link_poses(x).values()), self.acceptable_pose_error) + self.multi_link_position_goal_validator = MultiPositionGoalValidator( + lambda x: list(self.get_multiple_link_positions(x).values()), self.acceptable_position_error) + self.multi_link_orientation_goal_validator = MultiOrientationGoalValidator( + lambda x: list(self.get_multiple_link_orientations(x).values()), self.acceptable_orientation_error) + + # Goal validators for the joints of an object + self.joint_position_goal_validator = JointPositionGoalValidator(self.get_joint_position) + self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.get_multiple_joint_positions(x).values())) + @abstractmethod def _init_world(self, mode: WorldMode): """ @@ -440,6 +488,56 @@ def get_link_pose(self, link: Link) -> Pose: """ pass + @abstractmethod + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + """ + Get the poses of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and Pose objects as values. + """ + pass + + @abstractmethod + def get_link_position(self, link: Link) -> List[float]: + """ + Get the position of a link of an articulated object with respect to the world frame. + + :param link: The link as a AbstractLink object. + :return: The position of the link as a list of floats. + """ + pass + + @abstractmethod + def get_link_orientation(self, link: Link) -> List[float]: + """ + Get the orientation of a link of an articulated object with respect to the world frame. + + :param link: The link as a AbstractLink object. + :return: The orientation of the link as a list of floats. + """ + pass + + @abstractmethod + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the positions of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and lists of floats as values. + """ + pass + + @abstractmethod + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and lists of floats as values. + """ + pass + @abstractmethod def get_object_link_names(self, obj: Object) -> List[str]: """ @@ -489,6 +587,41 @@ def get_object_pose(self, obj: Object) -> Pose: """ pass + @abstractmethod + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + """ + Get the poses of multiple objects in the world frame from the current object poses in the simulator. + """ + pass + + @abstractmethod + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + """ + Get the positions of multiple objects in the world frame from the current object poses in the simulator. + """ + pass + + @abstractmethod + def get_object_position(self, obj: Object) -> List[float]: + """ + Get the position of an object in the world frame from the current object pose in the simulator. + """ + pass + + @abstractmethod + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple objects in the world frame from the current object poses in the simulator. + """ + pass + + @abstractmethod + def get_object_orientation(self, obj: Object) -> List[float]: + """ + Get the orientation of an object in the world frame from the current object pose in the simulator. + """ + pass + def set_mobile_robot_pose(self, pose: Pose) -> None: """ Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when @@ -496,9 +629,9 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_joint_positions(goal) + self.robot.set_joint_positions(goal, validate=False) - def get_move_base_joint_goal(self, pose: Pose) -> Dict[Joint, float]: + def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ Get the goal for the move base joints of a mobile robot to reach a target pose. param pose: The target pose. @@ -607,10 +740,12 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: pass @abstractmethod - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], + validate: Optional[bool] = True) -> None: """ Set the positions of multiple joints of an articulated object. :param joint_positions: A dictionary with joint objects as keys and joint positions as values. + :param validate: If the joint position goals should be validated. """ pass @@ -632,6 +767,16 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): """ pass + @abstractmethod + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + """ + Reset the world position and orientation of the base of multiple objects instantaneously, + not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. + + :param objects: A dictionary with objects as keys and poses as values. + """ + pass + @abstractmethod def step(self): """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index d08918b56..11da4ce10 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -8,17 +8,22 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from ..description import ObjectDescription, LinkDescription, Joint, JointDescription -from ..object_descriptors.urdf import ObjectDescription as URDFObject -from ..datastructures.world import WorldEntity, World -from ..world_concepts.constraints import Attachment from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape, ClosestPointsList, ContactPointsList) from ..datastructures.enums import ObjectType, JointType -from ..local_transformer import LocalTransformer from ..datastructures.pose import Pose, Transform +from ..datastructures.world import WorldEntity, World +from ..description import ObjectDescription, LinkDescription, Joint +from ..local_transformer import LocalTransformer +from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription +from ..world_concepts.constraints import Attachment +from ..worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ + OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, IterableErrorChecker +from ..worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, OrientationGoalValidator, \ + PositionGoalValidator, MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator, \ + MultiJointPositionGoalValidator, JointPositionGoalValidator Link = ObjectDescription.Link @@ -63,6 +68,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if name in [obj.name for obj in self.world.objects]: rospy.logerr(f"An object with the name {name} already exists in the world.") raise ValueError(f"An object with the name {name} already exists in the world.") + self.name: str = name self.obj_type: ObjectType = obj_type self.color: Color = color @@ -92,6 +98,30 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the positions of multiple links of the object. + param link_names: The names of the links. + return: The positions of the links. + """ + return self.world.get_multiple_link_positions(links) + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple links of the object. + param link_names: The names of the links. + return: The orientations of the links. + """ + return self.world.get_multiple_link_orientations(links) + + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + """ + Get the poses of multiple links of the object. + param link_names: The names of the links. + return: The poses of the links. + """ + return self.world.get_multiple_link_poses(links) + def get_target_poses_of_attached_objects(self) -> Dict[Object, Pose]: """ Get the target poses of the attached objects. @@ -614,7 +644,8 @@ def save_joints_states(self, state_id: int) -> None: @property def current_state(self) -> ObjectState: - return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), self.joint_states.copy()) + return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), + self.joint_states.copy()) @current_state.setter def current_state(self, state: ObjectState) -> None: @@ -875,16 +906,17 @@ def reset_all_joints_positions(self) -> None: joint_positions = [0] * len(joint_names) self.set_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: + def set_joint_positions(self, joint_positions: Dict[str, float], validate: Optional[bool] = True) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. :param joint_positions: A dictionary with the joint names as keys and the target positions as values. + :param validate: If True the joint position goals will be validated. """ joint_positions = {self.joints[joint_name]: joint_position for joint_name, joint_position in joint_positions.items()} - self.world.set_multiple_joint_positions(joint_positions) + self.world.set_multiple_joint_positions(joint_positions, validate) self.update_pose() self._update_all_links_poses() self.update_link_transforms() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 8be0deaec..e809aff2a 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -113,6 +113,21 @@ def get_object_joint_names(self, obj: Object) -> List[str]: return [p.getJointInfo(obj.id, i, physicsClientId=self.id)[1].decode('utf-8') for i in range(self.get_object_number_of_joints(obj))] + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + return {link.name: self.get_link_pose(link) for link in links} + + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + return {link.name: self.get_link_position(link) for link in links} + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + return {link.name: self.get_link_orientation(link) for link in links} + + def get_link_position(self, link: Link) -> List[float]: + return self.get_link_pose(link).position_as_list() + + def get_link_orientation(self, link: Link) -> List[float]: + return self.get_link_pose(link).orientation_as_list() + def get_link_pose(self, link: ObjectDescription.Link) -> Pose: bullet_link_state = p.getLinkState(link.object_id, link.id, physicsClientId=self.id) return Pose(*bullet_link_state[4:6]) @@ -167,7 +182,8 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], + validate: Optional[bool] = True) -> None: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) @@ -181,9 +197,28 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> None: p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), physicsClientId=self.id) + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + for obj, pose in objects.items(): + self.reset_object_base_pose(obj, pose) + def step(self): p.stepSimulation(physicsClientId=self.id) + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + return {obj.name: self.get_object_pose(obj) for obj in objects} + + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + return {obj.name: self.get_object_pose(obj).position_as_list() for obj in objects} + + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + return {obj.name: self.get_object_pose(obj).orientation_as_list() for obj in objects} + + def get_object_position(self, obj: Object) -> List[float]: + return self.get_object_pose(obj).position_as_list() + + def get_object_orientation(self, obj: Object) -> List[float]: + return self.get_object_pose(obj).orientation_as_list() + def get_object_pose(self, obj: Object) -> Pose: return Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.id)) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index b4aabd858..414b2e024 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -7,9 +7,7 @@ from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager -from .multiverse_functions.error_checkers import PoseErrorChecker, MultiJointPositionErrorChecker, \ - RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker -from .multiverse_functions.goal_validator import GoalValidator + from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -182,37 +180,42 @@ def get_object_joint_names(self, obj: Object) -> List[str]: def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] + def get_link_position(self, link: Link) -> List[float]: + return self.reader.get_body_position(link.name) + + def get_link_orientation(self, link: Link) -> List[float]: + return self.reader.get_body_orientation(link.name) + + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_positions([link.name for link in links]) + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_orientations([link.name for link in links]) + def reset_joint_position(self, joint: Joint, joint_position: float) -> None: - self.set_multiple_joint_positions({joint: joint_position}) + self.joint_position_goal_validator.register_goal(joint_position, joint.type, joint) + self.writer.send_body_data_to_server(joint.name, + {self.get_joint_position_name(joint): [joint_position]}) + self.joint_position_goal_validator.wait_until_goal_is_achieved() - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: - initial_joint_positions = {joint: self.get_joint_position(joint) for joint in joint_positions.keys()} + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], + validate: Optional[bool] = True) -> None: data = {joint.name: {self.get_joint_position_name(joint): [position]} for joint, position in joint_positions.items()} + + if validate: + self.multi_joint_position_goal_validator.register_goal(list(joint_positions.values()), + [joint.type for joint in joint_positions.keys()], + list(joint_positions.keys())) self.writer.send_multiple_body_data_to_server(data) - self._wait_until_multiple_joint_goals_are_achieved(joint_positions, initial_joint_positions) - - def _wait_until_multiple_joint_goals_are_achieved(self, joint_positions: Dict[Joint, float], - initial_joint_positions: Dict[Joint, float]) -> None: - goal_validator = self._get_multi_joint_goal_validator(joint_positions, initial_joint_positions) - self._wait_until_goal_is_achieved(goal_validator) - - def _get_multi_joint_goal_validator(self, joint_positions: Dict[Joint, float], - initial_joint_positions: Dict[Joint, float]) -> GoalValidator: - joints = list(joint_positions.keys()) - joint_types = [joint.type for joint in joints] - target_joint_positions = list(joint_positions.values()) - initial_joint_positions = list(initial_joint_positions.values()) - goal_validator = GoalValidator(target_joint_positions, - lambda: list(self.get_multiple_joint_positions(joints).values()), - MultiJointPositionErrorChecker(joint_types), - initial_value=initial_joint_positions) - return goal_validator + + if validate: + self.multi_joint_position_goal_validator.wait_until_goal_is_achieved() def get_joint_position(self, joint: Joint) -> float: - data = self.get_multiple_joint_positions([joint]) + data = self.reader.get_body_data(joint.name, [self.get_joint_position_name(joint)]) if data is not None: - return data[joint.name] + return data[self.get_joint_position_name(joint)][0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: self.check_object_exists_and_issue_warning_if_not(joints[0].object) @@ -222,36 +225,45 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str if data is not None: return {joint_name: list(data[joint_name].values())[0][0] for joint_name in joint_names} - def _wait_until_joint_goal_is_achieved(self, joint: Joint, joint_position: float, - initial_joint_position: float) -> None: - error_checker = RevoluteJointPositionErrorChecker() if joint.type == JointType.REVOLUTE else \ - PrismaticJointPositionErrorChecker() - goal_validator = GoalValidator(joint_position, lambda: self.get_joint_position(joint), - error_checker, initial_value=initial_joint_position) - self._wait_until_goal_is_achieved(goal_validator) - def get_link_pose(self, link: Link) -> Pose: self.check_object_exists_and_issue_warning_if_not(link.object) return self._get_body_pose(link.name) + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + return self._get_multiple_body_poses([link.name for link in links]) + def get_object_pose(self, obj: Object) -> Pose: self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return Pose() return self._get_body_pose(obj.name) + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + return self._get_multiple_body_poses([obj.name for obj in objects]) + def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return + + self.pose_goal_validator.register_goal(pose, obj) + initial_attached_objects_poses = list(obj.get_poses_of_attached_objects().values()) + if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): self.set_mobile_robot_pose(pose) else: - initial_attached_objects_poses = list(obj.get_poses_of_attached_objects().values()) self._set_body_pose(obj.name, pose) - if len(initial_attached_objects_poses) > 0: - self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + + self.pose_goal_validator.wait_until_goal_is_achieved() + if len(initial_attached_objects_poses) > 0: + self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + # TODO: Implement a more efficient way to reset multiple objects' poses by sending all the poses at once, + # instead of sending them one by one, this can be done constructing the metadata and data dictionaries. + for obj, pose in objects.items(): + self.reset_object_base_pose(obj, pose) def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_poses: List[Pose]) -> None: """ @@ -260,20 +272,9 @@ def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_po param initial_poses: The list of initial poses of the attached objects. """ target_poses = obj.get_target_poses_of_attached_objects() - body_names = [obj.name for obj in target_poses.keys()] - self._wait_until_all_pose_goals_are_achieved(body_names, list(target_poses.values()), - initial_poses) - - def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: List[Pose], - initial_poses: List[Pose]) -> None: - """ - Wait until all poses are set to the desired poses. - param poses: The dictionary of the desired poses - param initial_poses: The dictionary of the initial poses - """ - goal_validator = GoalValidator(poses, lambda: list(self._get_multiple_body_poses(body_names).values()), - PoseErrorChecker(is_iterable=True), initial_value=initial_poses) - self._wait_until_goal_is_achieved(goal_validator) + self.multi_pose_goal_validator.register_goal_and_wait_until_achieved(list(target_poses.values()), + list(target_poses.keys()), + initial_poses) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: """ @@ -284,12 +285,21 @@ def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optiona """ return self.reader.get_body_pose(body_name, wait) - def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: - return self._get_multiple_body_poses([obj.name for obj in objects]) - def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: return self.reader.get_multiple_body_poses(body_names) + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_positions([obj.name for obj in objects]) + + def get_object_position(self, obj: Object) -> List[float]: + return self.reader.get_body_position(obj.name) + + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_orientations([obj.name for obj in objects]) + + def get_object_orientation(self, obj: Object) -> List[float]: + return self.reader.get_body_orientation(obj.name) + def multiverse_reset_world(self): self.writer.reset_world() @@ -301,39 +311,7 @@ def _set_body_pose(self, body_name: str, pose: Pose) -> None: """ xyzw = pose.orientation_as_list() wxyz = [xyzw[3], *xyzw[:3]] - initial_pose = self._get_body_pose(body_name, wait=False) self.writer.set_body_pose(body_name, pose.position_as_list(), wxyz) - if initial_pose is not None: - self._wait_until_pose_goal_is_achieved(body_name, pose, initial_pose) - - def _wait_until_pose_goal_is_achieved(self, body_name: str, target_pose: Pose, initial_pose: Pose): - """ - Wait until the pose of a body is set. - param body_name: The name of the body. - param target_pose: The target pose of the body. - param initial_pose: The initial pose of the body. - """ - goal_validator = GoalValidator(PoseErrorChecker(), lambda: self._get_body_pose(body_name)) - goal_validator.register_goal(target_pose, initial_pose) - self._wait_until_goal_is_achieved(goal_validator) - - def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: - """ - Wait until the target is reached. - param goal_validator: The goal validator object to validate and keep track of the goal achievement progress. - """ - start_time = time() - current = goal_validator.current_value - while not goal_validator.goal_achieved: - sleep(0.01) - if time() - start_time > self.reader.MAX_WAIT_TIME_FOR_DATA: - msg = f"Failed to achieve goal from initial error {goal_validator.initial_error} with" \ - f" goal {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}" \ - f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage" \ - f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" - logging.error(msg) - raise TimeoutError(msg) - current = goal_validator.current_value def disconnect_from_physics_server(self) -> None: MultiverseClientManager.stop_all_clients() diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 6e3f9f113..c6cfecfb2 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -59,7 +59,7 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = self.thread.start() - def get_body_pose(self, name: str, wait: Optional[bool] = True) -> Optional[Pose]: + def get_body_pose(self, name: str, wait: Optional[bool] = False) -> Optional[Pose]: """ Get the body pose from the multiverse server. param name: The name of the body. @@ -70,7 +70,7 @@ def get_body_pose(self, name: str, wait: Optional[bool] = True) -> Optional[Pose if data is not None: return data[name] - def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = True) -> Optional[Dict[str, Pose]]: + def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, Pose]]: """ Get the body poses from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -83,7 +83,7 @@ def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = return {name: Pose(data[name]["position"], self.wxyz_to_xyzw(data[name]["quaternion"])) for name in body_names} - def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + def get_body_position(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: """ Get the body position from the multiverse server. param name: The name of the body. @@ -93,7 +93,7 @@ def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[ return self.get_multiple_body_positions([name], wait=wait)[name] def get_multiple_body_positions(self, body_names: List[str], - wait: Optional[bool] = True) -> Optional[Dict[str, List[float]]]: + wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: """ Get the body positions from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -102,7 +102,7 @@ def get_multiple_body_positions(self, body_names: List[str], """ return self.get_multiple_body_properties(body_names, ["position"], wait=wait) - def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + def get_body_orientation(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: """ Get the body orientation from the multiverse server. param name: The name of the body. @@ -112,7 +112,7 @@ def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Option return self.get_multiple_body_orientations([name], wait=wait)[name] def get_multiple_body_orientations(self, body_names: List[str], - wait: Optional[bool] = True) -> Optional[Dict[str, List[float]]]: + wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: """ Get the body orientations from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -123,7 +123,7 @@ def get_multiple_body_orientations(self, body_names: List[str], if data is not None: return {name: self.wxyz_to_xyzw(data[name]["quaternion"]) for name in body_names} - def get_body_property(self, name: str, property_name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + def get_body_property(self, name: str, property_name: str) -> Optional[List[float]]: """ Get the body property from the multiverse server. param name: The name of the body. @@ -131,12 +131,12 @@ def get_body_property(self, name: str, property_name: str, wait: Optional[bool] param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_multiple_body_properties([name], [property_name], wait=wait) - if data is not None: + data = self.get_received_data() + if self.check_for_body_data(name, data, [property_name]): return data[name][property_name] def get_multiple_body_properties(self, body_names: List[str], properties: List[str], - wait: Optional[bool] = True) -> Dict[str, Dict[str, List[float]]]: + wait: Optional[bool] = False) -> Optional[Dict[str, Dict[str, List[float]]]]: """ Get the body properties from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -156,8 +156,7 @@ def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: return quaternion[1:] + [quaternion[0]] def get_body_data(self, name: str, - properties: Optional[List[str]] = None, - wait: Optional[bool] = True) -> Optional[Dict]: + properties: Optional[List[str]] = None) -> Optional[Dict]: """ Get the body data from the multiverse server. param name: The name of the body. @@ -165,11 +164,12 @@ def get_body_data(self, name: str, param wait: Whether to wait for the data. return: The body data as a dictionary. """ - properties = {name: properties} if properties is not None else None - return self.get_multiple_body_data([name], properties, wait=wait)[name] + data = self.get_received_data() + if self.check_for_body_data(name, data, properties): + return data[name] def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[str]]] = None, - wait: Optional[bool] = True) -> Dict: + wait: Optional[bool] = False) -> Optional[Dict]: """ Get the body data from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -347,7 +347,9 @@ def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]) param data: The data to be sent. return: The response from the server. """ - return self.send_multiple_body_data_to_server({body_name: data}) + flattened_data = [value for values in data.values() for value in values] + return self.send_data_to_server([time() - self.time_start, *flattened_data], + send_meta_data={body_name: list(data.keys())}) def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[float]]]) -> Dict: """ diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_functions/error_checkers.py index cf85425d9..2b49a5787 100644 --- a/src/pycram/worlds/multiverse_functions/error_checkers.py +++ b/src/pycram/worlds/multiverse_functions/error_checkers.py @@ -20,17 +20,25 @@ def __init__(self, acceptable_error: Union[float, T_Iterable[float]], is_iterabl :param acceptable_error: The acceptable error. :param is_iterable: Whether the error is iterable (i.e. list of errors). """ - self.acceptable_error: np.ndarray = np.array(acceptable_error) + self._acceptable_error: np.ndarray = np.array(acceptable_error) self.tiled_acceptable_error: Optional[np.ndarray] = None self.is_iterable = is_iterable + @property + def acceptable_error(self) -> np.ndarray: + return self._acceptable_error + + @acceptable_error.setter + def acceptable_error(self, new_acceptable_error: Union[float, T_Iterable[float]]) -> None: + self._acceptable_error = np.array(new_acceptable_error) + def update_acceptable_error(self, new_acceptable_error: Optional[T_Iterable[float]] = None, tile_to_match: Optional[Sized] = None,) -> None: """ Update the acceptable error with a new value, and tile it to match the length of the error if needed. """ if new_acceptable_error is not None: - self.acceptable_error = np.array(new_acceptable_error) + self.acceptable_error = new_acceptable_error if tile_to_match is not None and self.is_iterable: self.update_tiled_acceptable_error(tile_to_match) @@ -120,37 +128,49 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: return calculate_orientation_error(value_1, value_2) -class MultiJointPositionErrorChecker(ErrorChecker): +class SingleValueErrorChecker(ErrorChecker): - def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_Iterable[float]] = None): - self.joint_types = joint_types - if acceptable_error is None: - acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] - super().__init__(acceptable_error, True) + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ - Calculate the error between two joint positions. + Calculate the error between two values. """ - return calculate_joint_position_error(value_1, value_2) + return abs(value_1 - value_2) -class RevoluteJointPositionErrorChecker(ErrorChecker): +class RevoluteJointPositionErrorChecker(SingleValueErrorChecker): def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): super().__init__(acceptable_error, is_iterable) + +class PrismaticJointPositionErrorChecker(SingleValueErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + +class IterableErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[T_Iterable[float]] = None): + super().__init__(acceptable_error, True) + def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ - Calculate the error between two joint positions. + Calculate the error between two values. """ - return calculate_joint_position_error(value_1, value_2) + return abs(value_1 - value_2) -class PrismaticJointPositionErrorChecker(ErrorChecker): +class MultiJointPositionErrorChecker(IterableErrorChecker): - def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): - super().__init__(acceptable_error, is_iterable) + def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_Iterable[float]] = None): + self.joint_types = joint_types + if acceptable_error is None: + acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] + super().__init__(acceptable_error) def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index b6fedf2fb..423a8a9cd 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,7 +1,14 @@ +import logging +from time import sleep, time + import numpy as np -from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, Protocol +from typing_extensions import Any, Callable, Optional, Union, Iterable + +from pycram.datastructures.enums import JointType +from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ + OrientationErrorChecker, SingleValueErrorChecker -from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker +OptionalArgCallable = Union[Callable[[], Any], Callable[[Any], Any]] class GoalValidator: @@ -9,7 +16,7 @@ class GoalValidator: A class to validate the goal by tracking the goal achievement progress. """ - def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[Optional[Any]], Any], + def __init__(self, error_checker: ErrorChecker, current_value_getter: OptionalArgCallable, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. @@ -26,6 +33,44 @@ def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[ self.initial_error: Optional[np.ndarray] = None self.current_value_getter_input: Optional[Any] = None + def register_goal_and_wait_until_achieved(self, goal_value: Any, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Union[float, Iterable[float]]] = None, + max_wait_time: Optional[float] = 2, + time_per_read: Optional[float] = 0.01) -> None: + """ + Register the goal value and wait until the target is reached. + :param goal_value: The goal value. + :param current_value_getter_input: The values that are used as input to the current value getter. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + :param max_wait_time: The maximum time to wait. + :param time_per_read: The time to wait between each read. + """ + self.register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + self.wait_until_goal_is_achieved(max_wait_time, time_per_read) + + def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, + time_per_read: Optional[float] = 0.01) -> None: + """ + Wait until the target is reached. + :param max_wait_time: The maximum time to wait. + :param time_per_read: The time to wait between each read. + """ + start_time = time() + current = self.current_value + while not self.goal_achieved: + sleep(time_per_read) + if time() - start_time > max_wait_time: + msg = f"Failed to achieve goal from initial error {self.initial_error} with" \ + f" goal {self.goal_value} within {max_wait_time}" \ + f" seconds, the current value is {current}, error is {self.current_error}, percentage" \ + f" of goal achieved is {self.percentage_of_goal_achieved}" + logging.error(msg) + raise TimeoutError(msg) + current = self.current_value + @property def _acceptable_error(self) -> np.ndarray: """ @@ -143,7 +188,7 @@ def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> n :param error: The error. :param threshold: The threshold. """ - return np.maximum(error-self._acceptable_error, threshold) + return np.maximum(error - self._acceptable_error, threshold) @property def goal_achieved(self) -> bool: @@ -163,3 +208,187 @@ def is_current_error_acceptable(self) -> bool: return: Whether the error is acceptable. """ return self.error_checker.is_error_acceptable(self.current_value, self.goal_value) + + +class PoseGoalValidator(GoalValidator): + """ + A class to validate the pose goal by tracking the goal achievement progress. + """ + + def __init__(self, current_pose_getter: OptionalArgCallable = None, + acceptable_error: Union[float, Iterable[float]] = (1e-3, np.pi / 180), + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the pose goal validator. + :param current_pose_getter: The current pose getter function which takes an optional input and returns the + current pose. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(PoseErrorChecker(acceptable_error, is_iterable=is_iterable), current_pose_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiPoseGoalValidator(PoseGoalValidator): + """ + A class to validate the multi-pose goal by tracking the goal achievement progress. + """ + + def __init__(self, current_poses_getter: OptionalArgCallable = None, + acceptable_error: Union[float, Iterable[float]] = (1e-2, 5*np.pi / 180), + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-pose goal validator. + :param current_poses_getter: The current poses getter function which takes an optional input and returns the + current poses. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_poses_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class PositionGoalValidator(GoalValidator): + """ + A class to validate the position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_position_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = 1e-3, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the position goal validator. + :param current_position_getter: The current position getter function which takes an optional input and + returns the current position. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of position vectors. + """ + super().__init__(PositionErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiPositionGoalValidator(PositionGoalValidator): + """ + A class to validate the multi-position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_positions_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = 1e-3, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-position goal validator. + :param current_positions_getter: The current positions getter function which takes an optional input and + returns the current positions. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_positions_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class OrientationGoalValidator(GoalValidator): + """ + A class to validate the orientation goal by tracking the goal achievement progress. + """ + + def __init__(self, current_orientation_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = np.pi / 180, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the orientation goal validator. + :param current_orientation_getter: The current orientation getter function which takes an optional input and + returns the current orientation. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of quaternions. + """ + super().__init__(OrientationErrorChecker(acceptable_error, is_iterable=is_iterable), current_orientation_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiOrientationGoalValidator(OrientationGoalValidator): + """ + A class to validate the multi-orientation goal by tracking the goal achievement progress. + """ + + def __init__(self, current_orientations_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = np.pi / 180, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-orientation goal validator. + :param current_orientations_getter: The current orientations getter function which takes an optional input and + returns the current orientations. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_orientations_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class JointPositionGoalValidator(GoalValidator): + """ + A class to validate the joint position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_position_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = None, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the joint position goal validator. + :param current_position_getter: The current position getter function which takes an optional input and returns + the current position. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of joint positions. + """ + super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + def register_goal(self, goal_value: Any, joint_type: JointType, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[float] = None): + """ + Register the goal value. + :param goal_value: The goal value. + :param joint_type: The joint type (e.g. REVOLUTE, PRISMATIC). + :param current_value_getter_input: The values that are used as input to the current value getter. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + """ + if acceptable_error is None: + self.error_checker.acceptable_error = np.pi / 180 if joint_type == JointType.REVOLUTE else 1e-3 + super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + + +class MultiJointPositionGoalValidator(GoalValidator): + """ + A class to validate the multi-joint position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_positions_getter: OptionalArgCallable = None, + acceptable_error: Optional[Iterable[float]] = None, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-joint position goal validator. + :param current_positions_getter: The current positions getter function which takes an optional input and + returns the current positions. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=True), current_positions_getter, + acceptable_percentage_of_goal_achieved) + + def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Iterable[float]] = None): + if acceptable_error is None: + self.error_checker.acceptable_error = [np.pi / 180 if jt == JointType.REVOLUTE else 1e-3 for jt in + joint_type] + super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index a5be07e65..eed9e8203 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -1,5 +1,6 @@ import numpy as np from tf.transformations import quaternion_from_euler +from typing_extensions import Optional, List from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.enums import JointType @@ -8,14 +9,23 @@ from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ MultiJointPositionErrorChecker -from pycram.worlds.multiverse_functions.goal_validator import GoalValidator +from pycram.worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ + PositionGoalValidator, OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, \ + MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator class TestGoalValidator(BulletWorldTestCase): def test_single_pose_goal(self): + pose_goal_validators = PoseGoalValidator(self.milk.get_pose) + self.validate_pose_goal(pose_goal_validators) + + def test_single_pose_goal_generic(self): + pose_goal_validators = GoalValidator(PoseErrorChecker(), self.milk.get_pose) + self.validate_pose_goal(pose_goal_validators) + + def validate_pose_goal(self, goal_validator): milk_goal_pose = Pose([1.3, 1.5, 0.9]) - goal_validator = GoalValidator(PoseErrorChecker(), self.milk.get_pose) goal_validator.register_goal(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -24,14 +34,20 @@ def test_single_pose_goal(self): self.milk.set_pose(milk_goal_pose) self.assertEqual(self.milk.get_pose(), milk_goal_pose) self.assertTrue(goal_validator.goal_achieved) - print(goal_validator.current_error) self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) + def test_single_position_goal_generic(self): + goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) + self.validate_position_goal(goal_validator) + def test_single_position_goal(self): + goal_validator = PositionGoalValidator(self.cereal.get_position_as_list) + self.validate_position_goal(goal_validator) + + def validate_position_goal(self, goal_validator): cereal_goal_position = [1.3, 1.5, 0.95] - goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) goal_validator.register_goal(cereal_goal_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -42,9 +58,16 @@ def test_single_position_goal(self): self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) self.assertEqual(goal_validator.current_error, 0) + def test_single_orientation_goal_generic(self): + goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) + self.validate_orientation_goal(goal_validator) + def test_single_orientation_goal(self): + goal_validator = OrientationGoalValidator(self.cereal.get_orientation_as_list) + self.validate_orientation_goal(goal_validator) + + def validate_orientation_goal(self, goal_validator): cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi / 2) - goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) goal_validator.register_goal(cereal_goal_orientation) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -56,11 +79,21 @@ def test_single_orientation_goal(self): self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) + def test_single_revolute_joint_position_goal_generic(self): + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), self.robot.get_joint_position) + self.validate_revolute_joint_position_goal(goal_validator) + def test_single_revolute_joint_position_goal(self): + goal_validator = JointPositionGoalValidator(self.robot.get_joint_position) + self.validate_revolute_joint_position_goal(goal_validator, JointType.REVOLUTE) + + def validate_revolute_joint_position_goal(self, goal_validator, joint_type: Optional[JointType] = None): goal_joint_position = -np.pi / 4 joint_name = 'l_shoulder_lift_joint' - goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), self.robot.get_joint_position) - goal_validator.register_goal(goal_joint_position, joint_name) + if joint_type is not None: + goal_validator.register_goal(goal_joint_position, joint_type, joint_name) + else: + goal_validator.register_goal(goal_joint_position, joint_name) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -76,11 +109,21 @@ def test_single_revolute_joint_position_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), places=5) + def test_single_prismatic_joint_position_goal_generic(self): + goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), self.robot.get_joint_position) + self.validate_prismatic_joint_position_goal(goal_validator) + def test_single_prismatic_joint_position_goal(self): + goal_validator = JointPositionGoalValidator(self.robot.get_joint_position) + self.validate_prismatic_joint_position_goal(goal_validator, JointType.PRISMATIC) + + def validate_prismatic_joint_position_goal(self, goal_validator, joint_type: Optional[JointType] = None): goal_joint_position = 0.2 torso = RobotDescription.current_robot_description.torso_joint - goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), self.robot.get_joint_position) - goal_validator.register_goal(goal_joint_position, torso) + if joint_type is not None: + goal_validator.register_goal(goal_joint_position, joint_type, torso) + else: + goal_validator.register_goal(goal_joint_position, torso) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -96,13 +139,25 @@ def test_single_prismatic_joint_position_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), places=5) - def test_multi_joint_goal(self): - goal_joint_positions = np.array([0.2, -np.pi / 4]) - joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] + def test_multi_joint_goal_generic(self): joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] goal_validator = GoalValidator(MultiJointPositionErrorChecker(joint_types), lambda x: list(self.robot.get_multiple_joint_positions(x).values())) - goal_validator.register_goal(goal_joint_positions, joint_names) + self.validate_multi_joint_goal(goal_validator) + + def test_multi_joint_goal(self): + joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] + goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_multi_joint_goal(goal_validator, joint_types) + + def validate_multi_joint_goal(self, goal_validator, joint_types: Optional[List[JointType]] = None): + goal_joint_positions = np.array([0.2, -np.pi / 4]) + joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] + if joint_types is not None: + goal_validator.register_goal(goal_joint_positions, joint_types, joint_names) + else: + goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi / 4)]), atol=0.001)) @@ -123,13 +178,20 @@ def test_multi_joint_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(0.2) * (1 - percent), places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], abs(-np.pi / 4) * (1 - percent), places=5) + def test_list_of_poses_goal_generic(self): + goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), + lambda: [self.robot.get_pose(), self.robot.get_pose()]) + self.validate_list_of_poses_goal(goal_validator) + def test_list_of_poses_goal(self): + goal_validator = MultiPoseGoalValidator(lambda: [self.robot.get_pose(), self.robot.get_pose()]) + self.validate_list_of_poses_goal(goal_validator) + + def validate_list_of_poses_goal(self, goal_validator): position_goal = [0.0, 1.0, 0.0] orientation_goal = np.array([0, 0, np.pi / 2]) poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] - goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), - lambda: [self.robot.get_pose(), self.robot.get_pose()]) goal_validator.register_goal(poses_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -155,11 +217,19 @@ def test_list_of_poses_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[2], (1 - percent), places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[3], np.pi * (1 - percent) / 2, places=5) + def test_list_of_positions_goal_generic(self): + goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), + lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) + self.validate_list_of_positions_goal(goal_validator) + def test_list_of_positions_goal(self): + goal_validator = MultiPositionGoalValidator(lambda: [self.robot.get_position_as_list(), + self.robot.get_position_as_list()]) + self.validate_list_of_positions_goal(goal_validator) + + def validate_list_of_positions_goal(self, goal_validator): position_goal = [0.0, 1.0, 0.0] positions_goal = [position_goal, position_goal] - goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), - lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) goal_validator.register_goal(positions_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -177,13 +247,21 @@ def test_list_of_positions_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 1 - percent, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 1 - percent, places=5) + def test_list_of_orientations_goal_generic(self): + goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), + lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()]) + self.validate_list_of_orientations_goal(goal_validator) + def test_list_of_orientations_goal(self): + goal_validator = MultiOrientationGoalValidator(lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()]) + self.validate_list_of_orientations_goal(goal_validator) + + def validate_list_of_orientations_goal(self, goal_validator): orientation_goal = np.array([0, 0, np.pi / 2]) orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), quaternion_from_euler(*orientation_goal.tolist())] - goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), - lambda: [self.robot.get_orientation_as_list(), - self.robot.get_orientation_as_list()]) goal_validator.register_goal(orientations_goals) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -203,13 +281,25 @@ def test_list_of_orientations_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], np.pi * (1 - percent) / 2, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], np.pi * (1 - percent) / 2, places=5) + def test_list_of_revolute_joint_positions_goal_generic(self): + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_list_of_revolute_joint_positions_goal(goal_validator) + def test_list_of_revolute_joint_positions_goal(self): + goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_list_of_revolute_joint_positions_goal(goal_validator, [JointType.REVOLUTE, JointType.REVOLUTE]) + + def validate_list_of_revolute_joint_positions_goal(self, goal_validator, + joint_types: Optional[List[JointType]] = None): goal_joint_position = -np.pi / 4 goal_joint_positions = np.array([goal_joint_position, goal_joint_position]) joint_names = ['l_shoulder_lift_joint', 'r_shoulder_lift_joint'] - goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), - lambda x: list(self.robot.get_multiple_joint_positions(x).values())) - goal_validator.register_goal(goal_joint_positions, joint_names) + if joint_types is not None: + goal_validator.register_goal(goal_joint_positions, joint_types, joint_names) + else: + goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, diff --git a/test/test_multiverse.py b/test/test_multiverse.py index e2af322f4..54faf4203 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -49,9 +49,7 @@ def tearDownClass(cls): cls.multiverse.exit() def tearDown(self): - # self.multiverse.multiverse_reset_world() self.multiverse.reset_world_and_remove_objects() - # MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() def test_demo(self): extension = ObjectDescription.get_file_extension() From b82a84f3d1d6b46a6352feeef0ccaf5795b8c1d0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 13:52:06 +0200 Subject: [PATCH 11/21] [Multiverse] tests are working. --- src/pycram/datastructures/world.py | 42 ++++++++++++------- src/pycram/world_concepts/world_object.py | 7 ++++ src/pycram/worlds/multiverse.py | 25 ++++++++--- .../multiverse_communication/clients.py | 9 ++-- .../multiverse_functions/goal_validator.py | 2 +- test/test_multiverse.py | 8 ++-- 6 files changed, 65 insertions(+), 28 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index e6adba567..0f6a37501 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -160,9 +160,11 @@ class World(StateEntity, ABC): The prefix for the prospection world name. """ - acceptable_position_error: float = 1e-2 - acceptable_orientation_error: float = 5*np.pi / 180 + acceptable_position_error: float = 5e-3 # 5 cm + acceptable_orientation_error: float = 10 * np.pi / 180 # 5 degrees acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) + use_percentage_of_goal: bool = True + acceptable_percentage_of_goal: float = 0.5 if use_percentage_of_goal else None """ The acceptable error for the position and orientation of an object. """ @@ -221,36 +223,48 @@ def _init_goal_validators(self): """ # Goal validators for multiple objects self.multi_pose_goal_validator = MultiPoseGoalValidator( - lambda x: list(self.get_multiple_object_poses(x).values()), self.acceptable_pose_error) + lambda x: list(self.get_multiple_object_poses(x).values()), + self.acceptable_pose_error, self.acceptable_percentage_of_goal) self.multi_position_goal_validator = MultiPositionGoalValidator( - lambda x: list(self.get_multiple_object_positions(x).values()), self.acceptable_position_error) + lambda x: list(self.get_multiple_object_positions(x).values()), + self.acceptable_position_error, self.acceptable_percentage_of_goal) self.multi_orientation_goal_validator = MultiOrientationGoalValidator( - lambda x: list(self.get_multiple_object_orientations(x).values()), self.acceptable_orientation_error) + lambda x: list(self.get_multiple_object_orientations(x).values()), + self.acceptable_orientation_error, self.acceptable_percentage_of_goal) # Goal validators for an object - self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error) - self.position_goal_validator = PositionGoalValidator(self.get_object_position, self.acceptable_position_error) + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, + self.acceptable_percentage_of_goal) + self.position_goal_validator = PositionGoalValidator(self.get_object_position, self.acceptable_position_error, + self.acceptable_percentage_of_goal) self.orientation_goal_validator = OrientationGoalValidator(self.get_object_orientation, - self.acceptable_orientation_error) + self.acceptable_orientation_error, + self.acceptable_percentage_of_goal) # Goal validators for the links of an object self.link_pose_goal_validator = PoseGoalValidator(self.get_link_pose, self.acceptable_pose_error) self.link_position_goal_validator = PositionGoalValidator(self.get_link_position, self.acceptable_position_error) self.link_orientation_goal_validator = OrientationGoalValidator(self.get_link_orientation, - self.acceptable_orientation_error) + self.acceptable_orientation_error, + self.acceptable_percentage_of_goal) self.multi_link_pose_goal_validator = MultiPoseGoalValidator( - lambda x: list(self.get_multiple_link_poses(x).values()), self.acceptable_pose_error) + lambda x: list(self.get_multiple_link_poses(x).values()), self.acceptable_pose_error, + self.acceptable_percentage_of_goal) self.multi_link_position_goal_validator = MultiPositionGoalValidator( - lambda x: list(self.get_multiple_link_positions(x).values()), self.acceptable_position_error) + lambda x: list(self.get_multiple_link_positions(x).values()), self.acceptable_position_error, + self.acceptable_percentage_of_goal) self.multi_link_orientation_goal_validator = MultiOrientationGoalValidator( - lambda x: list(self.get_multiple_link_orientations(x).values()), self.acceptable_orientation_error) + lambda x: list(self.get_multiple_link_orientations(x).values()), self.acceptable_orientation_error, + self.acceptable_percentage_of_goal) # Goal validators for the joints of an object - self.joint_position_goal_validator = JointPositionGoalValidator(self.get_joint_position) + self.joint_position_goal_validator = JointPositionGoalValidator( + self.get_joint_position, acceptable_percentage_of_goal_achieved= self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( - lambda x: list(self.get_multiple_joint_positions(x).values())) + lambda x: list(self.get_multiple_joint_positions(x).values()), + acceptable_percentage_of_goal_achieved= self.acceptable_percentage_of_goal) @abstractmethod def _init_world(self, mode: WorldMode): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 11da4ce10..bb313e735 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -458,6 +458,13 @@ def reset(self, remove_saved_states=True) -> None: if remove_saved_states: self.remove_saved_states() + def has_type_environment(self) -> bool: + """ + Check if the object is of type environment. + :return: True if the object is of type environment, False otherwise. + """ + return self.obj_type == ObjectType.ENVIRONMENT + def attach(self, child_object: Object, parent_link: Optional[str] = None, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 414b2e024..ed950d632 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -212,10 +212,10 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], if validate: self.multi_joint_position_goal_validator.wait_until_goal_is_achieved() - def get_joint_position(self, joint: Joint) -> float: - data = self.reader.get_body_data(joint.name, [self.get_joint_position_name(joint)]) - if data is not None: - return data[self.get_joint_position_name(joint)][0] + def get_joint_position(self, joint: Joint) -> Optional[float]: + data = self.reader.get_body_property(joint.name, self.get_joint_position_name(joint)) + if data is not None and len(data) > 0: + return data[0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: self.check_object_exists_and_issue_warning_if_not(joints[0].object) @@ -242,8 +242,10 @@ def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: return self._get_multiple_body_poses([obj.name for obj in objects]) def reset_object_base_pose(self, obj: Object, pose: Pose): + self.check_object_exists_and_issue_warning_if_not(obj) - if obj.obj_type == ObjectType.ENVIRONMENT: + + if obj.has_type_environment(): return self.pose_goal_validator.register_goal(pose, obj) @@ -259,6 +261,19 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): if len(initial_attached_objects_poses) > 0: self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: + """ + Check if the object is a child in a fixed joint constraint. This means that the object is not free to move. + It should be moved according to the parent object. + :param obj: The object to check. + :return: True if the object is a child in a fixed joint constraint, False otherwise. + """ + constraints = list(self.constraints.values()) + for c in constraints: + if c.child_link.object == obj and c.type == JointType.FIXED: + return True + return False + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: # TODO: Implement a more efficient way to reset multiple objects' poses by sending all the poses at once, # instead of sending them one by one, this can be done constructing the metadata and data dictionaries. diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index c6cfecfb2..718fd3ad3 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -131,8 +131,8 @@ def get_body_property(self, name: str, property_name: str) -> Optional[List[floa param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_received_data() - if self.check_for_body_data(name, data, [property_name]): + data = self.get_multiple_body_properties([name], [property_name]) + if data is not None: return data[name][property_name] def get_multiple_body_properties(self, body_names: List[str], properties: List[str], @@ -164,8 +164,9 @@ def get_body_data(self, name: str, param wait: Whether to wait for the data. return: The body data as a dictionary. """ - data = self.get_received_data() - if self.check_for_body_data(name, data, properties): + properties = {name: properties} if properties is not None else None + data = self.get_multiple_body_data([name], properties) + if data is not None: return data[name] def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[str]]] = None, diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 423a8a9cd..d7e43adf0 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -179,7 +179,7 @@ def relative_initial_error(self) -> np.ndarray: """ Get the relative initial error. """ - return self.get_relative_error(self.initial_error) + return np.maximum(self.initial_error, 1e-3) def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> np.ndarray: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 54faf4203..942bcc3d4 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -59,12 +59,12 @@ def test_demo(self): milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.5, 2, 1.02]), color=Color(1, 0, 0, 1)) - spoon = Object("soup_spoon", ObjectType.SPOON, f"SoupSpoon.obj", pose=Pose([2.5, 2.5, 1.02]), - color=Color(0, 0, 1, 1)) - bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.2, 1.02]), + bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.3, 1]), color=Color(1, 1, 0, 1)) + spoon = Object("soup_spoon", ObjectType.SPOON, f"SoupSpoon.obj", pose=Pose([2.6, 2.6, 1]), + color=Color(0, 0, 1, 1)) bowl.attach(spoon) - bowl.set_position([2.5, 2.3, 1.02]) + bowl.set_position([2.5, 2.4, 1.02]) pick_pose = Pose([2.7, 2.15, 1]) From 885eb1f92d245a5436e49ad6a87de9428aa4ba29 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 14:52:07 +0200 Subject: [PATCH 12/21] [Multiverse] fixed goal validator and cache manager test issue. --- src/pycram/datastructures/world.py | 6 +++--- src/pycram/world_concepts/world_object.py | 5 ----- test/test_cache_manager.py | 10 ++++----- test/test_goal_validator.py | 26 +++++++++++------------ 4 files changed, 21 insertions(+), 26 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 0f6a37501..aa526839b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -161,12 +161,12 @@ class World(StateEntity, ABC): """ acceptable_position_error: float = 5e-3 # 5 cm - acceptable_orientation_error: float = 10 * np.pi / 180 # 5 degrees + acceptable_orientation_error: float = 10 * np.pi / 180 # 10 degrees acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) use_percentage_of_goal: bool = True - acceptable_percentage_of_goal: float = 0.5 if use_percentage_of_goal else None + acceptable_percentage_of_goal: float = 0.5 if use_percentage_of_goal else None # 50% """ - The acceptable error for the position and orientation of an object. + The acceptable error for the position and orientation of an object/link. """ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index bb313e735..9dccfdd8f 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -19,11 +19,6 @@ from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription from ..world_concepts.constraints import Attachment -from ..worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ - OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, IterableErrorChecker -from ..worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, OrientationGoalValidator, \ - PositionGoalValidator, MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator, \ - MultiJointPositionGoalValidator, JointPositionGoalValidator Link = ObjectDescription.Link diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index 95549d85e..b0557f971 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -2,7 +2,7 @@ from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.enums import ObjectType -from pycram.world_concepts.world_object import Object +from pycram.object_descriptors.urdf import ObjectDescription as URDFObject import pathlib @@ -14,7 +14,7 @@ def test_generate_description_and_write_to_cache(self): path = str(file_path) + "/../resources/apartment.urdf" extension = Path(path).suffix cache_path = self.world.cache_dir + "/apartment.urdf" - apartment = Object("apartment", ObjectType.ENVIRONMENT, path) - cache_manager.generate_description_and_write_to_cache(path, apartment.name, extension, cache_path, - apartment.description) - self.assertTrue(cache_manager.is_cached(path, apartment.description)) + apartment = URDFObject(path) + cache_manager.generate_description_and_write_to_cache(path, "apartment", extension, cache_path, + apartment) + self.assertTrue(cache_manager.is_cached(path, apartment)) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index eed9e8203..c183cd488 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -28,13 +28,13 @@ def validate_pose_goal(self, goal_validator): milk_goal_pose = Pose([1.3, 1.5, 0.9]) goal_validator.register_goal(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0.5, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) self.milk.set_pose(milk_goal_pose) self.assertEqual(self.milk.get_pose(), milk_goal_pose) self.assertTrue(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 1) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) @@ -50,12 +50,12 @@ def validate_position_goal(self, goal_validator): cereal_goal_position = [1.3, 1.5, 0.95] goal_validator.register_goal(cereal_goal_position) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, 0.8) self.cereal.set_position(cereal_goal_position) self.assertEqual(self.cereal.get_position_as_list(), cereal_goal_position) self.assertTrue(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 1) self.assertEqual(goal_validator.current_error, 0) def test_single_orientation_goal_generic(self): @@ -70,13 +70,13 @@ def validate_orientation_goal(self, goal_validator): cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi / 2) goal_validator.register_goal(cereal_goal_orientation) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, [np.pi / 2]) self.cereal.set_orientation(cereal_goal_orientation) for v1, v2 in zip(self.cereal.get_orientation_as_list(), cereal_goal_orientation.tolist()): self.assertAlmostEqual(v1, v2, places=5) self.assertTrue(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, 1, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) def test_single_revolute_joint_position_goal_generic(self): @@ -95,7 +95,7 @@ def validate_revolute_joint_position_goal(self, goal_validator, joint_type: Opti else: goal_validator.register_goal(goal_joint_position, joint_name) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) for percent in [0.5, 1]: @@ -125,7 +125,7 @@ def validate_prismatic_joint_position_goal(self, goal_validator, joint_type: Opt else: goal_validator.register_goal(goal_joint_position, torso) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) for percent in [0.5, 1]: @@ -159,7 +159,7 @@ def validate_multi_joint_goal(self, goal_validator, joint_types: Optional[List[J else: goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi / 4)]), atol=0.001)) for percent in [0.5, 1]: @@ -194,7 +194,7 @@ def validate_list_of_poses_goal(self, goal_validator): Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] goal_validator.register_goal(poses_goal) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue( np.allclose(goal_validator.current_error, np.array([1.0, np.pi / 2, 1.0, np.pi / 2]), atol=0.001)) @@ -232,7 +232,7 @@ def validate_list_of_positions_goal(self, goal_validator): positions_goal = [position_goal, position_goal] goal_validator.register_goal(positions_goal) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) for percent in [0.5, 1]: @@ -264,7 +264,7 @@ def validate_list_of_orientations_goal(self, goal_validator): quaternion_from_euler(*orientation_goal.tolist())] goal_validator.register_goal(orientations_goals) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi / 2, np.pi / 2]), atol=0.001)) for percent in [0.5, 1]: @@ -301,7 +301,7 @@ def validate_list_of_revolute_joint_positions_goal(self, goal_validator, else: goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([abs(goal_joint_position), abs(goal_joint_position)]), atol=0.001)) From e4a94e57163966b15e07855a03db4e8305c79858 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 15:16:41 +0200 Subject: [PATCH 13/21] [Multiverse] attachmed objects pose goals are registered before setting the pose of the parent. Also added a reset for the goal validator and the error checker. --- src/pycram/worlds/multiverse.py | 18 ++++-------------- .../multiverse_functions/error_checkers.py | 6 ++++++ .../multiverse_functions/goal_validator.py | 14 ++++++++++++++ test/test_cache_manager.py | 1 - 4 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index ed950d632..1e978e56d 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -249,7 +249,9 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): return self.pose_goal_validator.register_goal(pose, obj) - initial_attached_objects_poses = list(obj.get_poses_of_attached_objects().values()) + attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) + self.multi_pose_goal_validator.register_goal(list(attachments_pose_goal.values()), + list(attachments_pose_goal.keys())) if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): @@ -258,8 +260,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): self._set_body_pose(obj.name, pose) self.pose_goal_validator.wait_until_goal_is_achieved() - if len(initial_attached_objects_poses) > 0: - self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + self.multi_pose_goal_validator.wait_until_goal_is_achieved() def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: """ @@ -280,17 +281,6 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None for obj, pose in objects.items(): self.reset_object_base_pose(obj, pose) - def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_poses: List[Pose]) -> None: - """ - Wait until all attached objects are set to the desired poses. - param obj: The object to which the attached objects belong. - param initial_poses: The list of initial poses of the attached objects. - """ - target_poses = obj.get_target_poses_of_attached_objects() - self.multi_pose_goal_validator.register_goal_and_wait_until_achieved(list(target_poses.values()), - list(target_poses.keys()), - initial_poses) - def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: """ Get the pose of a body in the simulator. diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_functions/error_checkers.py index 2b49a5787..4087befd2 100644 --- a/src/pycram/worlds/multiverse_functions/error_checkers.py +++ b/src/pycram/worlds/multiverse_functions/error_checkers.py @@ -24,6 +24,12 @@ def __init__(self, acceptable_error: Union[float, T_Iterable[float]], is_iterabl self.tiled_acceptable_error: Optional[np.ndarray] = None self.is_iterable = is_iterable + def reset(self) -> None: + """ + Reset the error checker. + """ + self.tiled_acceptable_error = None + @property def acceptable_error(self) -> np.ndarray: return self._acceptable_error diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index d7e43adf0..8967801b0 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -58,6 +58,8 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, :param max_wait_time: The maximum time to wait. :param time_per_read: The time to wait between each read. """ + if self.goal_value is None: + return # Skip if goal value is None start_time = time() current = self.current_value while not self.goal_achieved: @@ -70,6 +72,16 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, logging.error(msg) raise TimeoutError(msg) current = self.current_value + self.reset() + + def reset(self) -> None: + """ + Reset the goal validator. + """ + self.goal_value = None + self.initial_error = None + self.current_value_getter_input = None + self.error_checker.reset() @property def _acceptable_error(self) -> np.ndarray: @@ -106,6 +118,8 @@ def register_goal(self, goal_value: Any, :param initial_value: The initial value. :param acceptable_error: The acceptable error. """ + if goal_value is None or hasattr(goal_value, '__len__') and len(goal_value) == 0: + return # Skip if goal value is None or empty self.goal_value = goal_value self.current_value_getter_input = current_value_getter_input self.update_initial_error(goal_value, initial_value=initial_value) diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index b0557f971..5e4f9876c 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -1,7 +1,6 @@ from pathlib import Path from bullet_world_testcase import BulletWorldTestCase -from pycram.datastructures.enums import ObjectType from pycram.object_descriptors.urdf import ObjectDescription as URDFObject import pathlib From 2dd5bdee42f5bb656d2bd48c1a6efd4ef1c1ab2b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 18:18:00 +0200 Subject: [PATCH 14/21] [Multiverse] made the validation into decorators. --- src/pycram/datastructures/world.py | 56 +++++++++---- src/pycram/world_concepts/world_object.py | 25 +++--- src/pycram/worlds/bullet_world.py | 4 +- src/pycram/worlds/multiverse.py | 52 +++--------- .../multiverse_functions/goal_validator.py | 82 +++++++++++++++++-- test/test_multiverse.py | 5 +- 6 files changed, 140 insertions(+), 84 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index aa526839b..6eabee2ee 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -11,8 +11,7 @@ import rospy from geometry_msgs.msg import Point from tf.transformations import euler_from_quaternion -from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING -from typing_extensions import Union +from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union from ..cache_manager import CacheManager from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, @@ -27,9 +26,9 @@ from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event -from ..worlds.multiverse_functions.goal_validator import GoalValidator, MultiPoseGoalValidator, \ +from ..worlds.multiverse_functions.goal_validator import MultiPoseGoalValidator, \ MultiPositionGoalValidator, MultiOrientationGoalValidator, PoseGoalValidator, PositionGoalValidator, \ - OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator + OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, GoalValidator if TYPE_CHECKING: from ..world_concepts.world_object import Object @@ -169,6 +168,11 @@ class World(StateEntity, ABC): The acceptable error for the position and orientation of an object/link. """ + raise_goal_validator_error: Optional[bool] = False + """ + Whether to raise an error if the goals are not achieved. + """ + def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): """ Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the @@ -185,6 +189,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.let_pycram_handle_spawning = True self.let_pycram_handle_world_sync = True self.update_poses_from_sim_on_get = True + GoalValidator.raise_error = World.raise_goal_validator_error if World.current_world is None: World.current_world = self @@ -261,10 +266,17 @@ def _init_goal_validators(self): # Goal validators for the joints of an object self.joint_position_goal_validator = JointPositionGoalValidator( - self.get_joint_position, acceptable_percentage_of_goal_achieved= self.acceptable_percentage_of_goal) + self.get_joint_position, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), - acceptable_percentage_of_goal_achieved= self.acceptable_percentage_of_goal) + acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) + + def check_object_exists(self, obj: Object): + """ + Checks if the object exists in the simulator and issues a warning if it does not. + :param obj: The object to check. + """ + return obj not in self.objects @abstractmethod def _init_world(self, mode: WorldMode): @@ -643,7 +655,7 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_joint_positions(goal, validate=False) + self.robot.set_joint_positions(goal) def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ @@ -754,12 +766,11 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: pass @abstractmethod - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], - validate: Optional[bool] = True) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: """ Set the positions of multiple joints of an articulated object. :param joint_positions: A dictionary with joint objects as keys and joint positions as values. - :param validate: If the joint position goals should be validated. + :return: True if the set was successful, False otherwise. """ pass @@ -771,13 +782,14 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: pass @abstractmethod - def reset_object_base_pose(self, obj: Object, pose: Pose): + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. :param obj: The object. :param pose: The new pose as a Pose object. + :return: True if the reset was successful, False otherwise. """ pass @@ -1235,7 +1247,8 @@ def create_box_visual_shape(self, shape_data: BoxVisualShape) -> int: """ Creates a box visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the box visual shape to be created, uses the BoxVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the box visual shape to be created, uses the BoxVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1244,7 +1257,8 @@ def create_cylinder_visual_shape(self, shape_data: CylinderVisualShape) -> int: """ Creates a cylinder visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the cylinder visual shape to be created, uses the CylinderVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the cylinder visual shape to be created, uses the + CylinderVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1253,7 +1267,8 @@ def create_sphere_visual_shape(self, shape_data: SphereVisualShape) -> int: """ Creates a sphere visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the sphere visual shape to be created, uses the SphereVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the sphere visual shape to be created, uses the SphereVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1262,7 +1277,8 @@ def create_capsule_visual_shape(self, shape_data: CapsuleVisualShape) -> int: """ Creates a capsule visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the capsule visual shape to be created, uses the CapsuleVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the capsule visual shape to be created, uses the + CapsuleVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1271,7 +1287,8 @@ def create_plane_visual_shape(self, shape_data: PlaneVisualShape) -> int: """ Creates a plane visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the plane visual shape to be created, uses the PlaneVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the plane visual shape to be created, uses the PlaneVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1294,10 +1311,13 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ :param text: The text to be added. :param position: The position of the text in the world. - :param orientation: By default, debug text will always face the camera, automatically rotation. By specifying a text orientation (quaternion), the orientation will be fixed in world space or local space (when parent is specified). + :param orientation: By default, debug text will always face the camera, automatically rotation. By specifying a + text orientation (quaternion), the orientation will be fixed in world space or local space + (when parent is specified). :param size: The size of the text. :param color: The color of the text. - :param life_time: The lifetime in seconds of the text to remain in the world, if 0 the text will remain in the world until it is removed manually. + :param life_time: The lifetime in seconds of the text to remain in the world, if 0 the text will remain in the + world until it is removed manually. :param parent_object_id: The id of the object to which the text should be attached. :param parent_link_id: The id of the link to which the text should be attached. :return: The id of the added text. diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 9dccfdd8f..d2016db43 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -61,8 +61,10 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if pose is None: pose = Pose() if name in [obj.name for obj in self.world.objects]: - rospy.logerr(f"An object with the name {name} already exists in the world.") - raise ValueError(f"An object with the name {name} already exists in the world.") + msg = f"An object with the name {name} already exists in the world,"\ + f" is_prospection_world: {self.world.is_prospection_world}" + rospy.logerr(msg) + raise ValueError(msg) self.name: str = name self.obj_type: ObjectType = obj_type @@ -203,7 +205,7 @@ def _add_virtual_move_base_joints(self): child_link = self.root_link_name axes = virtual_joints.get_axes() for joint_name, joint_type in virtual_joints.get_types().items(): - self.description.add_joint(joint_name, child_link, joint_type, axes[joint_name]) + self.description.add_joint(joint_name, child_link, joint_type, axes[joint_name], is_virtual=True) def _init_joint_name_and_id_map(self) -> None: """ @@ -590,8 +592,8 @@ def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Op self._set_attached_objects_poses() def reset_base_pose(self, pose: Pose): - self.world.reset_object_base_pose(self, pose) - self.update_pose() + if self.world.reset_object_base_pose(self, pose): + self.update_pose() def update_pose(self): """ @@ -908,21 +910,20 @@ def reset_all_joints_positions(self) -> None: joint_positions = [0] * len(joint_names) self.set_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_positions: Dict[str, float], validate: Optional[bool] = True) -> None: + def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. :param joint_positions: A dictionary with the joint names as keys and the target positions as values. - :param validate: If True the joint position goals will be validated. """ joint_positions = {self.joints[joint_name]: joint_position for joint_name, joint_position in joint_positions.items()} - self.world.set_multiple_joint_positions(joint_positions, validate) - self.update_pose() - self._update_all_links_poses() - self.update_link_transforms() - self._set_attached_objects_poses() + if self.world.set_multiple_joint_positions(joint_positions): + self.update_pose() + self._update_all_links_poses() + self.update_link_transforms() + self._set_attached_objects_poses() def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index e809aff2a..d9554daf0 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -182,10 +182,10 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], - validate: Optional[bool] = True) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) + return True def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: return {joint.name: self.get_joint_position(joint) for joint in joints} diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 1e978e56d..d5a8f6d95 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -3,10 +3,12 @@ from time import sleep, time import numpy as np +import rospy from tf.transformations import quaternion_matrix from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager +from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType @@ -198,19 +200,12 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: {self.get_joint_position_name(joint): [joint_position]}) self.joint_position_goal_validator.wait_until_goal_is_achieved() - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], - validate: Optional[bool] = True) -> None: + @validate_multiple_joint_positions + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: data = {joint.name: {self.get_joint_position_name(joint): [position]} for joint, position in joint_positions.items()} - - if validate: - self.multi_joint_position_goal_validator.register_goal(list(joint_positions.values()), - [joint.type for joint in joint_positions.keys()], - list(joint_positions.keys())) self.writer.send_multiple_body_data_to_server(data) - - if validate: - self.multi_joint_position_goal_validator.wait_until_goal_is_achieved() + return True def get_joint_position(self, joint: Joint) -> Optional[float]: data = self.reader.get_body_property(joint.name, self.get_joint_position_name(joint)) @@ -218,22 +213,19 @@ def get_joint_position(self, joint: Joint) -> Optional[float]: return data[0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: - self.check_object_exists_and_issue_warning_if_not(joints[0].object) joint_names = [joint.name for joint in joints] data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] for joint in joints}) if data is not None: return {joint_name: list(data[joint_name].values())[0][0] for joint_name in joint_names} - def get_link_pose(self, link: Link) -> Pose: - self.check_object_exists_and_issue_warning_if_not(link.object) + def get_link_pose(self, link: Link) -> Optional[Pose]: return self._get_body_pose(link.name) def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: return self._get_multiple_body_poses([link.name for link in links]) def get_object_pose(self, obj: Object) -> Pose: - self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return Pose() return self._get_body_pose(obj.name) @@ -241,17 +233,10 @@ def get_object_pose(self, obj: Object) -> Pose: def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: return self._get_multiple_body_poses([obj.name for obj in objects]) - def reset_object_base_pose(self, obj: Object, pose: Pose): - - self.check_object_exists_and_issue_warning_if_not(obj) - + @validate_object_pose + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: if obj.has_type_environment(): - return - - self.pose_goal_validator.register_goal(pose, obj) - attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) - self.multi_pose_goal_validator.register_goal(list(attachments_pose_goal.values()), - list(attachments_pose_goal.keys())) + return False if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): @@ -259,8 +244,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): else: self._set_body_pose(obj.name, pose) - self.pose_goal_validator.wait_until_goal_is_achieved() - self.multi_pose_goal_validator.wait_until_goal_is_achieved() + return True def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: """ @@ -339,9 +323,6 @@ def add_constraint(self, constraint: Constraint) -> int: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError - self.check_object_exists_and_issue_warning_if_not(constraint.parent_link.object) - self.check_object_exists_and_issue_warning_if_not(constraint.child_link.object) - if not self.let_pycram_move_attached_objects: self.api_requester.attach(constraint) @@ -368,7 +349,6 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - self.check_object_exists_and_issue_warning_if_not(obj) multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) body_link = None @@ -411,8 +391,6 @@ def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: L return contact_force_array.flatten().tolist()[2] def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: - self.check_object_exists_and_issue_warning_if_not(obj1) - self.check_object_exists_and_issue_warning_if_not(obj2) obj1_contact_points = self.get_object_contact_points(obj1) return obj1_contact_points.get_points_of_object(obj2) @@ -450,26 +428,21 @@ def restore_physics_simulator_state(self, state_id: int) -> None: raise NotImplementedError def set_link_color(self, link: Link, rgba_color: Color): - self.check_object_exists_and_issue_warning_if_not(link.object) logging.warning("set_link_color is not implemented in Multiverse") def get_link_color(self, link: Link) -> Color: - self.check_object_exists_and_issue_warning_if_not(link.object) logging.warning("get_link_color is not implemented in Multiverse") return Color() def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: - self.check_object_exists_and_issue_warning_if_not(obj) logging.warning("get_colors_of_object_links is not implemented in Multiverse") return {} def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: - self.check_object_exists_and_issue_warning_if_not(obj) logging.error("get_object_axis_aligned_bounding_box is not implemented in Multiverse") raise NotImplementedError def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: - self.check_object_exists_and_issue_warning_if_not(link.object) logging.error("get_link_axis_aligned_bounding_box is not implemented in Multiverse") raise NotImplementedError @@ -479,11 +452,6 @@ def set_realtime(self, real_time: bool) -> None: def set_gravity(self, gravity_vector: List[float]) -> None: logging.warning("set_gravity is not implemented in Multiverse") - def check_object_exists_and_issue_warning_if_not(self, obj: Object): - if obj not in self.objects: - msg = f"Object {obj} does not exist in the simulator" - logging.warning(msg) - def check_object_exists_in_multiverse(self, obj: Object) -> bool: return self.api_requester.check_object_exists(obj) diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 8967801b0..e2c269f12 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,13 +1,22 @@ -import logging from time import sleep, time import numpy as np -from typing_extensions import Any, Callable, Optional, Union, Iterable +import rospy +from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING from pycram.datastructures.enums import JointType from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, SingleValueErrorChecker +if TYPE_CHECKING: + from pycram.datastructures.world import World + from pycram.world_concepts.world_object import Object + from pycram.datastructures.pose import Pose + from pycram.description import ObjectDescription + + Joint = ObjectDescription.Joint + Link = ObjectDescription.Link + OptionalArgCallable = Union[Callable[[], Any], Callable[[Any], Any]] @@ -16,6 +25,11 @@ class GoalValidator: A class to validate the goal by tracking the goal achievement progress. """ + raise_error: Optional[bool] = False + """ + Whether to raise an error if the goal is not achieved. + """ + def __init__(self, error_checker: ErrorChecker, current_value_getter: OptionalArgCallable, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ @@ -37,7 +51,7 @@ def register_goal_and_wait_until_achieved(self, goal_value: Any, current_value_getter_input: Optional[Any] = None, initial_value: Optional[Any] = None, acceptable_error: Optional[Union[float, Iterable[float]]] = None, - max_wait_time: Optional[float] = 2, + max_wait_time: Optional[float] = 1, time_per_read: Optional[float] = 0.01) -> None: """ Register the goal value and wait until the target is reached. @@ -51,7 +65,7 @@ def register_goal_and_wait_until_achieved(self, goal_value: Any, self.register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) self.wait_until_goal_is_achieved(max_wait_time, time_per_read) - def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, + def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 1, time_per_read: Optional[float] = 0.01) -> None: """ Wait until the target is reached. @@ -69,8 +83,12 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, f" goal {self.goal_value} within {max_wait_time}" \ f" seconds, the current value is {current}, error is {self.current_error}, percentage" \ f" of goal achieved is {self.percentage_of_goal_achieved}" - logging.error(msg) - raise TimeoutError(msg) + if self.raise_error: + rospy.logerr(msg) + raise TimeoutError(msg) + else: + rospy.logwarn(msg) + break current = self.current_value self.reset() @@ -250,7 +268,7 @@ class MultiPoseGoalValidator(PoseGoalValidator): """ def __init__(self, current_poses_getter: OptionalArgCallable = None, - acceptable_error: Union[float, Iterable[float]] = (1e-2, 5*np.pi / 180), + acceptable_error: Union[float, Iterable[float]] = (1e-2, 5 * np.pi / 180), acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the multi-pose goal validator. @@ -406,3 +424,53 @@ def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], self.error_checker.acceptable_error = [np.pi / 180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_type] super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + + +def validate_object_pose(pose_setter_func): + """ + A decorator to validate the object pose. + :param pose_setter_func: The function to set the pose of the object. + """ + + def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): + attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) + if len(attachments_pose_goal) == 0: + world.pose_goal_validator.register_goal(pose, obj) + else: + world.multi_pose_goal_validator.register_goal([pose, *list(attachments_pose_goal.values())], + [obj, *list(attachments_pose_goal.keys())]) + if not pose_setter_func(world, obj, pose): + world.pose_goal_validator.reset() + world.multi_pose_goal_validator.reset() + return False + + world.pose_goal_validator.wait_until_goal_is_achieved() + world.multi_pose_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper + + +def validate_multiple_joint_positions(position_setter_func): + """ + A decorator to validate the joint positions, this function does not validate the virtual joints, + as in multiverse the virtual joints take command velocities and not positions, so after their goals + are set, they are zeroed thus can't be validated. (They are actually validated by the robot pose in case + of virtual move base joints) + :param position_setter_func: The function to set the joint positions. + """ + + def wrapper(world: 'World', joint_positions: Dict['Joint', float]): + joint_positions_to_validate = {joint: position for joint, position in joint_positions.items() + if not joint.is_virtual} + joint_types = [joint.type for joint in joint_positions_to_validate.keys()] + world.multi_joint_position_goal_validator.register_goal(list(joint_positions_to_validate.values()), joint_types, + list(joint_positions_to_validate.keys())) + if not position_setter_func(world, joint_positions): + world.multi_joint_position_goal_validator.reset() + return False + + world.multi_joint_position_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 942bcc3d4..4edd5e431 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -200,11 +200,10 @@ def test_attach_with_robot(self): robot = self.spawn_robot() ee_link = self.multiverse.get_arm_tool_frame_link(Arms.RIGHT) # Get position of milk relative to robot end effector - robot.attach(milk, ee_link.name, coincide_the_objects=True) + robot.attach(milk, ee_link.name, coincide_the_objects=False) self.assertTrue(robot in milk.attachments) milk_initial_pose = milk.root_link.get_pose_wrt_link(ee_link) - robot_position = robot.get_joint_position("arm_right_2_joint") - robot_position += 1.57 + robot_position = 1.57 robot.set_joint_position("arm_right_2_joint", robot_position) milk_pose = milk.root_link.get_pose_wrt_link(ee_link) self.assert_poses_are_equal(milk_initial_pose, milk_pose) From c1eda3b600ab90b6abc6481b50d74beef4400a5e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 18:26:38 +0200 Subject: [PATCH 15/21] [Multiverse] better angle calculation for mobile robot pose goal. --- src/pycram/datastructures/world.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 6eabee2ee..c023ae8f3 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -26,6 +26,8 @@ from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event +from ..worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ + calculate_quaternion_difference from ..worlds.multiverse_functions.goal_validator import MultiPoseGoalValidator, \ MultiPositionGoalValidator, MultiOrientationGoalValidator, PoseGoalValidator, PositionGoalValidator, \ OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, GoalValidator @@ -698,9 +700,8 @@ def get_z_angle_diff(current_quaternion: List[float], target_quaternion: List[fl param target_quaternion: The target quaternion. return: The difference between the z angles of the two quaternions in euler angles. """ - current_angle = euler_from_quaternion(current_quaternion)[2] - target_angle = euler_from_quaternion(target_quaternion)[2] - return target_angle - current_angle + quat_diff = calculate_quaternion_difference(current_quaternion, target_quaternion) + return euler_from_quaternion(quat_diff)[2] @abstractmethod def perform_collision_detection(self) -> None: From 09fca2cc0aba2c69ae2e7f25d1dcd60124064773 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jul 2024 12:37:47 +0200 Subject: [PATCH 16/21] [Multiverse] corrected mobile robot orientation target, tests are working. --- src/pycram/datastructures/world.py | 20 ++++++------ src/pycram/world_concepts/world_object.py | 2 +- .../multiverse_communication/clients.py | 18 ++++------- .../multiverse_functions/goal_validator.py | 19 +++++++++-- test/test_multiverse.py | 32 ++++++++++++++----- 5 files changed, 58 insertions(+), 33 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index c023ae8f3..b4d0834c5 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -268,9 +268,13 @@ def _init_goal_validators(self): # Goal validators for the joints of an object self.joint_position_goal_validator = JointPositionGoalValidator( - self.get_joint_position, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) + self.get_joint_position, acceptable_orientation_error=self.acceptable_orientation_error, + acceptable_position_error=self.acceptable_position_error, + acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), + acceptable_orientation_error=self.acceptable_orientation_error, + acceptable_position_error=self.acceptable_position_error, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) def check_object_exists(self, obj: Object): @@ -666,12 +670,12 @@ def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: return: The goal for the move base joints. """ position_diff = self.get_position_diff(self.robot.get_position_as_list(), pose.position_as_list())[:2] - angle_diff = self.get_z_angle_diff(self.robot.get_orientation_as_list(), pose.orientation_as_list()) + target_angle = self.get_z_angle(pose.orientation_as_list()) # Get the joints of the base link move_base_joints = self.get_move_base_joints() return {move_base_joints.translation_x: position_diff[0], move_base_joints.translation_y: position_diff[1], - move_base_joints.angular_z: angle_diff} + move_base_joints.angular_z: target_angle} @staticmethod def get_move_base_joints() -> VirtualMoveBaseJoints: @@ -693,15 +697,13 @@ def get_position_diff(current_position: List[float], target_position: List[float return [target_position[i] - current_position[i] for i in range(3)] @staticmethod - def get_z_angle_diff(current_quaternion: List[float], target_quaternion: List[float]) -> float: + def get_z_angle(target_quaternion: List[float]) -> float: """ - Get the difference between the z angles of two quaternions. - param current_quaternion: The current quaternion. + Get the z angle from a quaternion by converting it to euler angles. param target_quaternion: The target quaternion. - return: The difference between the z angles of the two quaternions in euler angles. + return: The z angle. """ - quat_diff = calculate_quaternion_difference(current_quaternion, target_quaternion) - return euler_from_quaternion(quat_diff)[2] + return euler_from_quaternion(target_quaternion)[2] @abstractmethod def perform_collision_detection(self) -> None: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index d2016db43..44e7b0f02 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -904,7 +904,7 @@ def reset_all_joints_positions(self) -> None: """ Sets the current position of all joints to 0. This is useful if the joints should be reset to their default """ - joint_names = [joint.name for joint in self.joints.values() if not joint.is_virtual] + joint_names = [joint.name for joint in self.joints.values()] # if not joint.is_virtual] if len(joint_names) == 0: return joint_positions = [0] * len(joint_names) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 718fd3ad3..470250e3f 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -262,15 +262,6 @@ def join(self): class MultiverseWriter(MultiverseClient): - time_for_sim_update: Optional[float] = 0.0 # 0.02 - """ - Wait time for the sent data to be applied in the simulator. - """ - time_for_setting_body_data: Optional[float] = 0.0 # 0.01 - """ - Wait time for setting body data. - """ - def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ @@ -365,7 +356,6 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[ for value in data] self.send_data = [time() - self.time_start, *flattened_data] self.send_and_receive_data() - sleep(self.time_for_setting_body_data * self.simulation_wait_time_factor) return self.response_meta_data def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: @@ -397,7 +387,6 @@ def send_data_to_server(self, data: List, self.send_and_receive_meta_data() self.send_data = data self.send_and_receive_data() - sleep(self.time_for_sim_update * self.simulation_wait_time_factor) return self.response_meta_data @@ -411,6 +400,7 @@ class MultiverseAPI(MultiverseClient): """ The wait time for the API request in seconds. """ + APIs_THAT_NEED_WAIT_TIME: List[API] = [API.ATTACH] def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, simulation_wait_time_factor: Optional[float] = 1.0): @@ -426,12 +416,14 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: """ super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation + self.wait: Optional[bool] = False # Whether to wait after sending the API request. def attach(self, constraint: Constraint) -> None: """ Request to attach the child link to the parent link. param constraint: The constraint. """ + self.wait = True parent_link_name, child_link_name = self.get_constraint_link_names(constraint) attachment_pose = self._get_attachment_pose_as_string(constraint) self._attach(child_link_name, parent_link_name, attachment_pose) @@ -630,7 +622,9 @@ def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[s self._add_api_request(api_name.value, *params) self._send_api_request() responses = self._get_all_apis_responses() - sleep(self.API_REQUEST_WAIT_TIME * self.simulation_wait_time_factor) + if self.wait: + sleep(self.API_REQUEST_WAIT_TIME * self.simulation_wait_time_factor) + self.wait = False return responses def _get_all_apis_responses(self) -> Dict[API, List[str]]: diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index e2c269f12..b59e70168 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -368,6 +368,8 @@ class JointPositionGoalValidator(GoalValidator): def __init__(self, current_position_getter: OptionalArgCallable = None, acceptable_error: Optional[float] = None, + acceptable_orientation_error: Optional[Iterable[float]] = np.pi / 180, + acceptable_position_error: Optional[Iterable[float]] = 1e-3, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, is_iterable: Optional[bool] = False): """ @@ -375,11 +377,15 @@ def __init__(self, current_position_getter: OptionalArgCallable = None, :param current_position_getter: The current position getter function which takes an optional input and returns the current position. :param acceptable_error: The acceptable error. + :param acceptable_orientation_error: The acceptable orientation error. + :param acceptable_position_error: The acceptable position error. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. :param is_iterable: Whether it is a sequence of joint positions. """ super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + self.acceptable_orientation_error = acceptable_orientation_error + self.acceptable_position_error = acceptable_position_error def register_goal(self, goal_value: Any, joint_type: JointType, current_value_getter_input: Optional[Any] = None, @@ -394,7 +400,8 @@ def register_goal(self, goal_value: Any, joint_type: JointType, :param acceptable_error: The acceptable error. """ if acceptable_error is None: - self.error_checker.acceptable_error = np.pi / 180 if joint_type == JointType.REVOLUTE else 1e-3 + self.error_checker.acceptable_error = self.acceptable_orientation_error if joint_type == JointType.REVOLUTE\ + else self.acceptable_position_error super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) @@ -405,24 +412,30 @@ class MultiJointPositionGoalValidator(GoalValidator): def __init__(self, current_positions_getter: OptionalArgCallable = None, acceptable_error: Optional[Iterable[float]] = None, + acceptable_orientation_error: Optional[Iterable[float]] = np.pi / 180, + acceptable_position_error: Optional[Iterable[float]] = 1e-3, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the multi-joint position goal validator. :param current_positions_getter: The current positions getter function which takes an optional input and returns the current positions. :param acceptable_error: The acceptable error. + :param acceptable_orientation_error: The acceptable orientation error. + :param acceptable_position_error: The acceptable position error. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. """ super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=True), current_positions_getter, acceptable_percentage_of_goal_achieved) + self.acceptable_orientation_error = acceptable_orientation_error + self.acceptable_position_error = acceptable_position_error def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], current_value_getter_input: Optional[Any] = None, initial_value: Optional[Any] = None, acceptable_error: Optional[Iterable[float]] = None): if acceptable_error is None: - self.error_checker.acceptable_error = [np.pi / 180 if jt == JointType.REVOLUTE else 1e-3 for jt in - joint_type] + self.error_checker.acceptable_error = [self.acceptable_orientation_error if jt == JointType.REVOLUTE + else self.acceptable_position_error for jt in joint_type] super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4edd5e431..44c6c7084 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -2,7 +2,10 @@ import os import unittest +import numpy as np import psutil +from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply, quaternion_inverse, \ + quaternion_conjugate from typing_extensions import Optional, List from pycram.datastructures.dataclasses import Color, ContactPointsList, ContactPoint @@ -11,6 +14,8 @@ from pycram.designators.object_designator import BelieveObject from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object +from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ + calculate_quaternion_difference multiverse_installed = True try: @@ -136,7 +141,6 @@ def test_spawn_robot(self): self.assertTrue(robot in self.multiverse.objects) self.assertTrue(self.multiverse.robot.name == robot.name) - # @unittest.skip("Not implemented feature yet.") def test_destroy_robot(self): if self.multiverse.robot is None: self.spawn_robot() @@ -152,14 +156,27 @@ def test_respawn_robot(self): self.spawn_robot() self.assertTrue(self.multiverse.robot in self.multiverse.objects) - # @unittest.skip("This will cause respawning of the robot.") def test_set_robot_position(self): + for i in range(3): + self.spawn_robot() + new_position = [-3, -3, 0.001] + self.multiverse.robot.set_position(new_position) + robot_position = self.multiverse.robot.get_position_as_list() + self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) + self.tearDown() + + def test_set_robot_orientation(self): self.spawn_robot() - new_position = [-3, -3, 0.001] - # self.multiverse.writer.send_multiple_body_data_to_server({"odom_vel_lin_x_joint": {"joint_tvalue": [-4]}}) - self.multiverse.robot.set_position(new_position) - robot_position = self.multiverse.robot.get_position_as_list() - self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) + for i in range(3): + current_quaternion = self.multiverse.robot.get_orientation_as_list() + # rotate by 45 degrees without using euler angles + rotation_quaternion = quaternion_from_euler(0, 0, np.pi / 4) + new_quaternion = quaternion_multiply(current_quaternion, rotation_quaternion) + self.multiverse.robot.set_orientation(new_quaternion) + robot_orientation = self.multiverse.robot.get_orientation_as_list() + quaternion_difference = calculate_angle_between_quaternions(new_quaternion, robot_orientation) + self.assertAlmostEqual(quaternion_difference, 0, delta=0.01) + # self.tearDown() def test_attach_object(self): milk = self.spawn_milk([1, 0.1, 0.1]) @@ -175,7 +192,6 @@ def test_attach_object(self): new_cup_position = cup.get_position_as_list() self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2]) - # @unittest.skip("Not implemented feature yet.") def test_detach_object(self): for i in range(2): milk = self.spawn_milk([1, 0, 0.1]) From 784c33ca64602c28ec5b3fa40f52ff0ecd55f09a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jul 2024 17:23:58 +0200 Subject: [PATCH 17/21] [Multiverse] defined enums for multiverse properties, and did some cleaning. set single joint position test is not passing because joints currently does not retain their positions, will be fixed soon. --- src/pycram/datastructures/world.py | 60 ++++----------- .../location/giskard_location.py | 2 +- src/pycram/external_interfaces/ik.py | 2 +- src/pycram/pose_generator_and_validator.py | 6 +- .../process_modules/boxy_process_modules.py | 14 ++-- .../process_modules/donbot_process_modules.py | 12 +-- .../process_modules/hsrb_process_modules.py | 6 +- .../process_modules/pr2_process_modules.py | 6 +- .../stretch_process_modules.py | 2 +- src/pycram/utils.py | 2 +- src/pycram/world_concepts/world_object.py | 25 +++---- src/pycram/worlds/bullet_world.py | 12 +-- src/pycram/worlds/multiverse.py | 68 +++++++++-------- .../multiverse_communication/clients.py | 75 ++++++++++++------- .../worlds/multiverse_datastructures/enums.py | 25 +++++++ test/test_goal_validator.py | 4 +- 16 files changed, 164 insertions(+), 157 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index b4d0834c5..e1ca2f471 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -228,49 +228,17 @@ def _init_goal_validators(self): """ Initializes the goal validators for the World objects' poses, positions, and orientations. """ + + # Goal validators for an object + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, + self.acceptable_percentage_of_goal) + # Goal validators for multiple objects self.multi_pose_goal_validator = MultiPoseGoalValidator( lambda x: list(self.get_multiple_object_poses(x).values()), self.acceptable_pose_error, self.acceptable_percentage_of_goal) - self.multi_position_goal_validator = MultiPositionGoalValidator( - lambda x: list(self.get_multiple_object_positions(x).values()), - self.acceptable_position_error, self.acceptable_percentage_of_goal) - self.multi_orientation_goal_validator = MultiOrientationGoalValidator( - lambda x: list(self.get_multiple_object_orientations(x).values()), - self.acceptable_orientation_error, self.acceptable_percentage_of_goal) - # Goal validators for an object - self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, - self.acceptable_percentage_of_goal) - self.position_goal_validator = PositionGoalValidator(self.get_object_position, self.acceptable_position_error, - self.acceptable_percentage_of_goal) - self.orientation_goal_validator = OrientationGoalValidator(self.get_object_orientation, - self.acceptable_orientation_error, - self.acceptable_percentage_of_goal) - - # Goal validators for the links of an object - self.link_pose_goal_validator = PoseGoalValidator(self.get_link_pose, self.acceptable_pose_error) - self.link_position_goal_validator = PositionGoalValidator(self.get_link_position, - self.acceptable_position_error) - self.link_orientation_goal_validator = OrientationGoalValidator(self.get_link_orientation, - self.acceptable_orientation_error, - self.acceptable_percentage_of_goal) - - self.multi_link_pose_goal_validator = MultiPoseGoalValidator( - lambda x: list(self.get_multiple_link_poses(x).values()), self.acceptable_pose_error, - self.acceptable_percentage_of_goal) - self.multi_link_position_goal_validator = MultiPositionGoalValidator( - lambda x: list(self.get_multiple_link_positions(x).values()), self.acceptable_position_error, - self.acceptable_percentage_of_goal) - self.multi_link_orientation_goal_validator = MultiOrientationGoalValidator( - lambda x: list(self.get_multiple_link_orientations(x).values()), self.acceptable_orientation_error, - self.acceptable_percentage_of_goal) - - # Goal validators for the joints of an object - self.joint_position_goal_validator = JointPositionGoalValidator( - self.get_joint_position, acceptable_orientation_error=self.acceptable_orientation_error, - acceptable_position_error=self.acceptable_position_error, - acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) + # Goal validator for the joints of an object self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), acceptable_orientation_error=self.acceptable_orientation_error, @@ -661,7 +629,7 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_joint_positions(goal) + self.robot.set_multiple_joint_positions(goal) def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ @@ -758,15 +726,15 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object, """ raise NotImplementedError - @abstractmethod - def reset_joint_position(self, joint: Joint, joint_position: float) -> None: + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation :param joint: The joint to reset the position for. :param joint_position: The new joint pose. + :return: True if the reset was successful, False otherwise """ - pass + return self.set_multiple_joint_positions({joint: joint_position}) @abstractmethod def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: @@ -784,7 +752,6 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ pass - @abstractmethod def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, @@ -794,15 +761,16 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: :param pose: The new pose as a Pose object. :return: True if the reset was successful, False otherwise. """ - pass + return self.reset_multiple_objects_base_poses({obj: pose}) @abstractmethod - def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool: """ Reset the world position and orientation of the base of multiple objects instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. :param objects: A dictionary with objects as keys and poses as values. + :return: True if the reset was successful, False otherwise. """ pass @@ -951,7 +919,7 @@ def reset_current_world(self) -> None: """ for obj in self.objects: obj.set_pose(obj.original_pose) - obj.set_joint_positions(dict(zip(list(obj.joint_names), [0] * len(obj.joint_names)))) + obj.set_multiple_joint_positions(dict(zip(list(obj.joint_names), [0] * len(obj.joint_names)))) def reset_robot(self) -> None: """ diff --git a/src/pycram/designators/specialized_designators/location/giskard_location.py b/src/pycram/designators/specialized_designators/location/giskard_location.py index 1400a8e63..de0d0a6e8 100644 --- a/src/pycram/designators/specialized_designators/location/giskard_location.py +++ b/src/pycram/designators/specialized_designators/location/giskard_location.py @@ -53,7 +53,7 @@ def __iter__(self) -> CostmapLocation.Location: prospection_robot = World.current_world.get_prospection_object_for_object(World.robot) with UseProspectionWorld(): - prospection_robot.set_joint_positions(robot_joint_states) + prospection_robot.set_multiple_joint_positions(robot_joint_states) prospection_robot.set_pose(pose) gripper_pose = prospection_robot.get_link_pose(chain.get_tool_frame()) diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 17ceca769..8f59db07a 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -234,7 +234,7 @@ def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[ robot_joint_states[joint_name] = state with UseProspectionWorld(): - prospection_robot.set_joint_positions(robot_joint_states) + prospection_robot.set_multiple_joint_positions(robot_joint_states) prospection_robot.set_pose(pose) tip_pose = prospection_robot.get_link_pose(gripper) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 170362783..bb19fa0ff 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -203,14 +203,14 @@ def reachability_validator(pose: Pose, # test the possible solution and apply it to the robot pose, joint_states = request_ik(target, robot, joints, tool_frame) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) # _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: # only check for retract pose if pose worked pose, joint_states = request_ik(retract_target_pose, robot, joints, tool_frame) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) # _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: @@ -218,7 +218,7 @@ def reachability_validator(pose: Pose, except IKError: pass finally: - robot.set_joint_positions(joint_state_before_ik) + robot.set_multiple_joint_positions(joint_state_before_ik) if arms: res = True return res, arms diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 5dc25d8f9..d2fcccf8d 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -89,13 +89,13 @@ def _execute(self, desig): pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link")) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "front")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "front")) if pose_in_shoulder.position.y >= 0 and pose_in_shoulder.position.y >= abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_right")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_right")) if pose_in_shoulder.position.x <= 0 and abs(pose_in_shoulder.position.x) > abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "back")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "back")) if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_left")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_left")) pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link")) @@ -115,7 +115,7 @@ def _execute(self, desig): robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_joint_positions(RobotDescription.current_robot_description.kinematic_chains[gripper].get_static_gripper_state(motion)) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.kinematic_chains[gripper].get_static_gripper_state(motion)) class BoxyDetecting(ProcessModule): @@ -160,9 +160,9 @@ def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) + robot.set_multiple_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class BoxyWorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index e9ff45edc..ae9fb70b9 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -69,13 +69,13 @@ def _execute(self, desig): pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link")) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "front")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "front")) if pose_in_shoulder.position.y >= 0 and pose_in_shoulder.position.y >= abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_right")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_right")) if pose_in_shoulder.position.x <= 0 and abs(pose_in_shoulder.position.x) > abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "back")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "back")) if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_left")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_left")) pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link")) @@ -94,7 +94,7 @@ def _execute(self, desig): robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_joint_positions(RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion)) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion)) class DonbotMoveTCP(ProcessModule): @@ -118,7 +118,7 @@ class DonbotMoveJoints(ProcessModule): def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class DonbotWorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/hsrb_process_modules.py b/src/pycram/process_modules/hsrb_process_modules.py index 5b66d6ec2..cdab22128 100644 --- a/src/pycram/process_modules/hsrb_process_modules.py +++ b/src/pycram/process_modules/hsrb_process_modules.py @@ -152,9 +152,9 @@ def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) + robot.set_multiple_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class HSRBMoveJoints(ProcessModule): @@ -164,7 +164,7 @@ class HSRBMoveJoints(ProcessModule): def _execute(self, desig: MoveJointsMotion): robot = World.robot - robot.set_joint_positions(dict(zip(desig.names, desig.positions))) + robot.set_multiple_joint_positions(dict(zip(desig.names, desig.positions))) class HSRBWorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 0ac65ba8d..62112ec5e 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -121,9 +121,9 @@ def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) + robot.set_multiple_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class PR2MoveJoints(ProcessModule): @@ -133,7 +133,7 @@ class PR2MoveJoints(ProcessModule): def _execute(self, desig: MoveJointsMotion): robot = World.robot - robot.set_joint_positions(dict(zip(desig.names, desig.positions))) + robot.set_multiple_joint_positions(dict(zip(desig.names, desig.positions))) class Pr2WorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py index 77b33596a..6ac3d45df 100644 --- a/src/pycram/process_modules/stretch_process_modules.py +++ b/src/pycram/process_modules/stretch_process_modules.py @@ -132,7 +132,7 @@ def _move_arm_tcp(target: Pose, robot: Object, arm: Arms) -> None: # inv = request_ik(target, robot, joints, gripper) pose, joint_states = request_giskard_ik(target, robot, gripper) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) ########################################################### diff --git a/src/pycram/utils.py b/src/pycram/utils.py index 28b5109ac..c4cb7710e 100644 --- a/src/pycram/utils.py +++ b/src/pycram/utils.py @@ -46,7 +46,7 @@ def _apply_ik(robot: 'pycram.world_concepts.WorldObject', pose_and_joint_poses: """ pose, joint_states = pose_and_joint_poses robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) class GeneratorList: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 44e7b0f02..ece5eda96 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -908,9 +908,18 @@ def reset_all_joints_positions(self) -> None: if len(joint_names) == 0: return joint_positions = [0] * len(joint_names) - self.set_joint_positions(dict(zip(joint_names, joint_positions))) + self.set_multiple_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: + def set_joint_position(self, joint_name: str, joint_position: float) -> None: + """ + Sets the position of the given joint to the given joint pose and updates the poses of all attached objects. + + :param joint_name: The name of the joint + :param joint_position: The target pose for this joint + """ + self.world.reset_joint_position(self.joints[joint_name], joint_position) + + def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. @@ -925,18 +934,6 @@ def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: self.update_link_transforms() self._set_attached_objects_poses() - def set_joint_position(self, joint_name: str, joint_position: float) -> None: - """ - Sets the position of the given joint to the given joint pose and updates the poses of all attached objects. - - :param joint_name: The name of the joint - :param joint_position: The target pose for this joint - """ - self.joints[joint_name].position = joint_position - self._update_all_links_poses() - self.update_link_transforms() - self._set_attached_objects_poses() - def get_joint_position(self, joint_name: str) -> float: """ :param joint_name: The name of the joint diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index d9554daf0..253776796 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -184,22 +184,16 @@ def parse_points_list_to_args(self, point: List) -> Dict: def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): - self.reset_joint_position(joint, joint_position) + p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) return True def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: return {joint.name: self.get_joint_position(joint) for joint in joints} - def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: float) -> None: - p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) - - def reset_object_base_pose(self, obj: Object, pose: Pose) -> None: - p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), - physicsClientId=self.id) - def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: for obj, pose in objects.items(): - self.reset_object_base_pose(obj, pose) + p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), + physicsClientId=self.id) def step(self): p.stepSimulation(physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index d5a8f6d95..854a080fe 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,15 +1,13 @@ import logging import os -from time import sleep, time import numpy as np -import rospy from tf.transformations import quaternion_matrix from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager +from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions - from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -25,9 +23,9 @@ class Multiverse(World): This class implements an interface between Multiverse and PyCRAM. """ - _joint_type_to_position_name: Dict[JointType, str] = { - JointType.REVOLUTE: "joint_rvalue", - JointType.PRISMATIC: "joint_tvalue", + _joint_type_to_position_name: Dict[JointType, MultiverseJointProperty] = { + JointType.REVOLUTE: MultiverseJointProperty.REVOLUTE_JOINT_POSITION, + JointType.PRISMATIC: MultiverseJointProperty.PRISMATIC_JOINT_POSITION, } """ A dictionary to map JointType to the corresponding multiverse attribute name. @@ -140,10 +138,9 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def get_joint_position_name(self, joint: Joint) -> str: + def get_joint_position_name(self, joint: Joint) -> MultiverseJointProperty: if joint.type not in self._joint_type_to_position_name: - logging.warning(f"Invalid joint type: {joint.type}") - return "joint_rvalue" + raise ValueError(f"Joint type {joint.type} is not supported in Multiverse") return self._joint_type_to_position_name[joint.type] def load_object_and_get_id(self, name: Optional[str] = None, @@ -194,12 +191,6 @@ def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: return self.reader.get_multiple_body_orientations([link.name for link in links]) - def reset_joint_position(self, joint: Joint, joint_position: float) -> None: - self.joint_position_goal_validator.register_goal(joint_position, joint.type, joint) - self.writer.send_body_data_to_server(joint.name, - {self.get_joint_position_name(joint): [joint_position]}) - self.joint_position_goal_validator.wait_until_goal_is_achieved() - @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: data = {joint.name: {self.get_joint_position_name(joint): [position]} @@ -208,16 +199,16 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b return True def get_joint_position(self, joint: Joint) -> Optional[float]: - data = self.reader.get_body_property(joint.name, self.get_joint_position_name(joint)) - if data is not None and len(data) > 0: - return data[0] + data = self.get_multiple_joint_positions([joint]) + if data is not None: + return data[joint.name] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: joint_names = [joint.name for joint in joints] data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] for joint in joints}) if data is not None: - return {joint_name: list(data[joint_name].values())[0][0] for joint_name in joint_names} + return {name: list(value.values())[0][0] for name, value in data.items()} def get_link_pose(self, link: Link) -> Optional[Pose]: return self._get_body_pose(link.name) @@ -260,10 +251,29 @@ def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: return False def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: - # TODO: Implement a more efficient way to reset multiple objects' poses by sending all the poses at once, - # instead of sending them one by one, this can be done constructing the metadata and data dictionaries. - for obj, pose in objects.items(): - self.reset_object_base_pose(obj, pose) + """ + Reset the poses of multiple objects in the simulator. + param objects: The dictionary of objects and poses. + """ + self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()}) + + def _set_body_pose(self, body_name: str, pose: Pose) -> None: + """ + Reset the pose of a body (object, link, or joint) in the simulator. + param body_name: The name of the body. + param pose: The pose of the body. + """ + self._set_multiple_body_poses({body_name: pose}) + + def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: + """ + Reset the poses of multiple bodies in the simulator. + param body_poses: The dictionary of body names and poses. + """ + self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), + MultiverseBodyProperty.ORIENTATION: + self.xyzw_to_wxyz(pose.orientation_as_list())} + for name, pose in body_poses.items()}) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: """ @@ -292,15 +302,9 @@ def get_object_orientation(self, obj: Object) -> List[float]: def multiverse_reset_world(self): self.writer.reset_world() - def _set_body_pose(self, body_name: str, pose: Pose) -> None: - """ - Reset the pose of a body (object, link, or joint) in the simulator. - param body_name: The name of the body. - param pose: The pose of the body. - """ - xyzw = pose.orientation_as_list() - wxyz = [xyzw[3], *xyzw[:3]] - self.writer.set_body_pose(body_name, pose.position_as_list(), wxyz) + @staticmethod + def xyzw_to_wxyz(xyzw: List[float]) -> List[float]: + return [xyzw[3], *xyzw[:3]] def disconnect_from_physics_server(self) -> None: MultiverseClientManager.stop_all_clients() diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 470250e3f..51635addd 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -5,7 +5,8 @@ from typing_extensions import List, Dict, Tuple, Optional from .socket import MultiverseSocket, MultiverseMetaData, SocketAddress -from ..multiverse_datastructures.enums import MultiverseAPIName as API +from ..multiverse_datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, + MultiverseProperty as Property) from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ...datastructures.pose import Pose from ...datastructures.world import World @@ -77,10 +78,14 @@ def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = param wait: Whether to wait for the data. return: The positions and orientations of the bodies as a dictionary. """ - data = self.get_multiple_body_data(body_names, {name: ["position", "quaternion"] for name in body_names}, + data = self.get_multiple_body_data(body_names, + {name: [BodyProperty.POSITION, BodyProperty.ORIENTATION] + for name in body_names + }, wait=wait) if data is not None: - return {name: Pose(data[name]["position"], self.wxyz_to_xyzw(data[name]["quaternion"])) + return {name: Pose(data[name][BodyProperty.POSITION.value], + self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value])) for name in body_names} def get_body_position(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: @@ -100,7 +105,7 @@ def get_multiple_body_positions(self, body_names: List[str], param wait: Whether to wait for the data. return: The positions of the bodies as a dictionary. """ - return self.get_multiple_body_properties(body_names, ["position"], wait=wait) + return self.get_multiple_body_properties(body_names, [BodyProperty.POSITION], wait=wait) def get_body_orientation(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: """ @@ -119,23 +124,23 @@ def get_multiple_body_orientations(self, body_names: List[str], param wait: Whether to wait for the data. return: The orientations of the bodies as a dictionary. """ - data = self.get_multiple_body_properties(body_names, ["quaternion"], wait=wait) + data = self.get_multiple_body_properties(body_names, [BodyProperty.ORIENTATION], wait=wait) if data is not None: - return {name: self.wxyz_to_xyzw(data[name]["quaternion"]) for name in body_names} + return {name: self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value]) for name in body_names} - def get_body_property(self, name: str, property_name: str) -> Optional[List[float]]: + def get_body_property(self, name: str, property_: Property) -> Optional[List[float]]: """ Get the body property from the multiverse server. param name: The name of the body. - param property_name: The name of the property. + param property_: The property of the body as a Property. param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_multiple_body_properties([name], [property_name]) + data = self.get_multiple_body_properties([name], [property_]) if data is not None: - return data[name][property_name] + return data[name][property_.value] - def get_multiple_body_properties(self, body_names: List[str], properties: List[str], + def get_multiple_body_properties(self, body_names: List[str], properties: List[Property], wait: Optional[bool] = False) -> Optional[Dict[str, Dict[str, List[float]]]]: """ Get the body properties from the multiverse server for multiple bodies. @@ -169,7 +174,8 @@ def get_body_data(self, name: str, if data is not None: return data[name] - def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[str]]] = None, + def get_multiple_body_data(self, body_names: List[str], + properties: Optional[Dict[str, List[Property]]] = None, wait: Optional[bool] = False) -> Optional[Dict]: """ Get the body data from the multiverse server for multiple bodies. @@ -187,7 +193,7 @@ def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dic return {name: data[name] for name in body_names} def wait_for_multiple_body_data(self, body_names: List[str], - properties: Optional[Dict[str, List[str]]] = None) -> Dict: + properties: Optional[Dict[str, List[Property]]] = None) -> Dict: """ Wait for the body data from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -208,7 +214,7 @@ def wait_for_multiple_body_data(self, body_names: List[str], raise ValueError(msg) def check_multiple_body_data(self, body_names: List[str], data: Dict, - properties: Optional[Dict[str, List[str]]] = None) -> bool: + properties: Optional[Dict[str, List[Property]]] = None) -> bool: """ Check if the body data is received from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -222,7 +228,7 @@ def check_multiple_body_data(self, body_names: List[str], data: Dict, return all([self.check_for_body_data(name, data, properties[name]) for name in body_names]) @staticmethod - def check_for_body_data(name: str, data: Dict, properties: Optional[List[str]] = None) -> bool: + def check_for_body_data(name: str, data: Dict, properties: Optional[List[Property]] = None) -> bool: """ Check if the body data is received from the multiverse server. param name: The name of the body. @@ -233,7 +239,8 @@ def check_for_body_data(name: str, data: Dict, properties: Optional[List[str]] = if properties is None: return name in data else: - return name in data and all([prop in data[name] and None not in data[name][prop] for prop in properties]) + return name in data and all([prop.value in data[name] and None not in data[name][prop.value] + for prop in properties]) def get_received_data(self): """ @@ -296,10 +303,15 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List param position: The position of the body. param orientation: The orientation of the body. """ - self.send_body_data_to_server(body_name, - {"position": position, - "quaternion": orientation} - ) + self.set_multiple_body_poses({body_name: {BodyProperty.POSITION: position, + BodyProperty.ORIENTATION: orientation}}) + + def set_multiple_body_poses(self, body_data: Dict[str, Dict[BodyProperty, List[float]]]) -> None: + """ + Set the body poses in the simulation for multiple bodies. + param body_data: The data to be sent for multiple bodies. + """ + self.send_multiple_body_data_to_server(body_data) def set_body_position(self, body_name: str, position: List[float]) -> None: """ @@ -307,7 +319,7 @@ def set_body_position(self, body_name: str, position: List[float]) -> None: param body_name: The name of the body. param position: The position of the body. """ - self.send_body_data_to_server(body_name, {"position": position}) + self.send_body_data_to_server(body_name, {BodyProperty.POSITION: position}) def set_body_orientation(self, body_name: str, orientation: List[float]) -> None: """ @@ -315,7 +327,7 @@ def set_body_orientation(self, body_name: str, orientation: List[float]) -> None param body_name: The name of the body. param orientation: The orientation of the body. """ - self.send_body_data_to_server(body_name, {"quaternion": orientation}) + self.send_body_data_to_server(body_name, {BodyProperty.ORIENTATION: orientation}) def remove_body(self, body_name: str) -> None: """ @@ -332,24 +344,22 @@ def reset_world(self) -> None: """ self.send_data_to_server([0]) - def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]) -> Dict: + def send_body_data_to_server(self, body_name: str, data: Dict[Property, List[float]]) -> Dict: """ Send data to the multiverse server. param body_name: The name of the body. param data: The data to be sent. return: The response from the server. """ - flattened_data = [value for values in data.values() for value in values] - return self.send_data_to_server([time() - self.time_start, *flattened_data], - send_meta_data={body_name: list(data.keys())}) + return self.send_multiple_body_data_to_server({body_name: data}) - def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[float]]]) -> Dict: + def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, List[float]]]) -> Dict: """ Send data to the multiverse server for multiple bodies. param body_data: The data to be sent for multiple bodies. return: The response from the server. """ - send_meta_data = {body_name: list(data.keys()) for body_name, data in body_data.items()} + send_meta_data = {body_name: list(map(str, data.keys())) for body_name, data in body_data.items()} response_meta_data = self.send_meta_data_and_get_response(send_meta_data) body_names = list(response_meta_data["send"].keys()) flattened_data = [value for body_name in body_names for data in body_data[body_name].values() @@ -358,6 +368,15 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[ self.send_and_receive_data() return self.response_meta_data + @staticmethod + def get_actuator_name(body_name: str) -> str: + """ + Get the actuator name from the body name. + param body_name: The name of the body. + return: The actuator name. + """ + return body_name.replace("_joint", "_actuator") + def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: """ Send metadata to the multiverse server and get the response. diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index 7ffd8c8aa..cb001a789 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -1,3 +1,4 @@ +from abc import ABC from enum import Enum @@ -11,3 +12,27 @@ class MultiverseAPIName(Enum): DETACH = "detach" GET_RAYS = "get_rays" EXIST = "exist" + + +class MultiverseProperty(Enum): + def __str__(self): + return self.value + + +class MultiverseBodyProperty(MultiverseProperty): + """ + Enum for the different properties of a body the Multiverse. + """ + POSITION = "position" + ORIENTATION = "quaternion" + RELATIVE_VELOCITY = "relative_velocity" + + +class MultiverseJointProperty(MultiverseProperty): + """ + Enum for the different properties of a joint the Multiverse. + """ + REVOLUTE_JOINT_POSITION = "joint_rvalue" + PRISMATIC_JOINT_POSITION = "joint_tvalue" + REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" + PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index c183cd488..6ebfe04ff 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -164,7 +164,7 @@ def validate_multi_joint_goal(self, goal_validator, joint_types: Optional[List[J for percent in [0.5, 1]: current_joint_positions = goal_joint_positions * percent - self.robot.set_joint_positions(dict(zip(joint_names, current_joint_positions.tolist()))) + self.robot.set_multiple_joint_positions(dict(zip(joint_names, current_joint_positions.tolist()))) self.assertTrue(np.allclose(self.robot.get_joint_position('torso_lift_joint'), current_joint_positions[0], atol=0.001)) self.assertTrue( @@ -307,7 +307,7 @@ def validate_list_of_revolute_joint_positions_goal(self, goal_validator, for percent in [0.5, 1]: current_joint_position = goal_joint_positions * percent - self.robot.set_joint_positions(dict(zip(joint_names, current_joint_position))) + self.robot.set_multiple_joint_positions(dict(zip(joint_names, current_joint_position))) self.assertTrue(np.allclose(list(self.robot.get_multiple_joint_positions(joint_names).values()), current_joint_position, atol=0.001)) if percent == 1: From 08bdb4a60eb7e64454700ab5933e90972e315c5f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jul 2024 19:53:33 +0200 Subject: [PATCH 18/21] [Multiverse] Faster and all tests are running, applied concept of single responsibility. --- src/pycram/datastructures/world.py | 6 +- src/pycram/worlds/bullet_world.py | 14 ++- src/pycram/worlds/multiverse.py | 19 +++- .../multiverse_communication/clients.py | 87 +++++++++++++------ test/test_orm.py | 1 + 5 files changed, 92 insertions(+), 35 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index e1ca2f471..f9433780a 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -726,6 +726,7 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object, """ raise NotImplementedError + @abstractmethod def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation @@ -734,7 +735,7 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: :param joint_position: The new joint pose. :return: True if the reset was successful, False otherwise """ - return self.set_multiple_joint_positions({joint: joint_position}) + pass @abstractmethod def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: @@ -752,6 +753,7 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ pass + @abstractmethod def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, @@ -761,7 +763,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: :param pose: The new pose as a Pose object. :return: True if the reset was successful, False otherwise. """ - return self.reset_multiple_objects_base_poses({obj: pose}) + pass @abstractmethod def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool: diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 253776796..171a34445 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -184,7 +184,11 @@ def parse_points_list_to_args(self, point: List) -> Dict: def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): - p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) + self.reset_joint_position(joint, joint_position) + return True + + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) return True def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: @@ -192,8 +196,12 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: for obj, pose in objects.items(): - p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), - physicsClientId=self.id) + self.reset_object_base_pose(obj, pose) + + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: + p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), + physicsClientId=self.id) + return True def step(self): p.stepSimulation(physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 854a080fe..55d67d6dc 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -191,6 +191,10 @@ def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: return self.reader.get_multiple_body_orientations([link.name for link in links]) + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + self.writer.set_body_property(joint.name, self.get_joint_position_name(joint), [joint_position]) + return True + @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: data = {joint.name: {self.get_joint_position_name(joint): [position]} @@ -199,9 +203,10 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b return True def get_joint_position(self, joint: Joint) -> Optional[float]: - data = self.get_multiple_joint_positions([joint]) + joint_position_name = self.get_joint_position_name(joint) + data = self.reader.get_body_data(joint.name, [joint_position_name]) if data is not None: - return data[joint.name] + return data[joint_position_name.value][0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: joint_names = [joint.name for joint in joints] @@ -282,7 +287,15 @@ def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optiona :param wait: Whether to wait until the pose is received. return: The pose of the body. """ - return self.reader.get_body_pose(body_name, wait) + data = self.reader.get_body_pose(body_name, wait) + return Pose(data[MultiverseBodyProperty.POSITION.value], + self.wxyz_to_xyzw(data[MultiverseBodyProperty.ORIENTATION.value])) + + def wxyz_to_xyzw(self, wxyz: List[float]) -> List[float]: + """ + Convert a quaternion from WXYZ to XYZW format. + """ + return [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: return self.reader.get_multiple_body_poses(body_names) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 51635addd..792c977ae 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -2,12 +2,12 @@ import threading from time import time, sleep -from typing_extensions import List, Dict, Tuple, Optional +from typing_extensions import List, Dict, Tuple, Optional, Callable, Union from .socket import MultiverseSocket, MultiverseMetaData, SocketAddress +from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ..multiverse_datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) -from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ...datastructures.pose import Pose from ...datastructures.world import World from ...world_concepts.constraints import Constraint @@ -35,7 +35,6 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = class MultiverseReader(MultiverseClient): - MAX_WAIT_TIME_FOR_DATA: Optional[float] = 2 """ The maximum wait time for the data in seconds. @@ -60,16 +59,14 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = self.thread.start() - def get_body_pose(self, name: str, wait: Optional[bool] = False) -> Optional[Pose]: + def get_body_pose(self, name: str, wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: """ Get the body pose from the multiverse server. param name: The name of the body. param wait: Whether to wait for the data. return: The position and orientation of the body. """ - data = self.get_multiple_body_poses([name], wait=wait) - if data is not None: - return data[name] + return self.get_body_data(name, [BodyProperty.POSITION, BodyProperty.ORIENTATION], wait=wait) def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, Pose]]: """ @@ -95,7 +92,7 @@ def get_body_position(self, name: str, wait: Optional[bool] = False) -> Optional param wait: Whether to wait for the data. return: The position of the body. """ - return self.get_multiple_body_positions([name], wait=wait)[name] + return self.get_body_property(name, BodyProperty.POSITION, wait=wait) def get_multiple_body_positions(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: @@ -114,7 +111,7 @@ def get_body_orientation(self, name: str, wait: Optional[bool] = False) -> Optio param wait: Whether to wait for the data. return: The orientation of the body. """ - return self.get_multiple_body_orientations([name], wait=wait)[name] + return self.get_body_property(name, BodyProperty.ORIENTATION, wait=wait) def get_multiple_body_orientations(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: @@ -128,7 +125,7 @@ def get_multiple_body_orientations(self, body_names: List[str], if data is not None: return {name: self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value]) for name in body_names} - def get_body_property(self, name: str, property_: Property) -> Optional[List[float]]: + def get_body_property(self, name: str, property_: Property, wait: Optional[bool] = False) -> Optional[List[float]]: """ Get the body property from the multiverse server. param name: The name of the body. @@ -136,9 +133,9 @@ def get_body_property(self, name: str, property_: Property) -> Optional[List[flo param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_multiple_body_properties([name], [property_]) + data = self.get_body_data(name, [property_], wait=wait) if data is not None: - return data[name][property_.value] + return data[property_.value] def get_multiple_body_properties(self, body_names: List[str], properties: List[Property], wait: Optional[bool] = False) -> Optional[Dict[str, Dict[str, List[float]]]]: @@ -161,7 +158,8 @@ def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: return quaternion[1:] + [quaternion[0]] def get_body_data(self, name: str, - properties: Optional[List[str]] = None) -> Optional[Dict]: + properties: Optional[List[Property]] = None, + wait: Optional[bool] = False) -> Optional[Dict]: """ Get the body data from the multiverse server. param name: The name of the body. @@ -169,9 +167,11 @@ def get_body_data(self, name: str, param wait: Whether to wait for the data. return: The body data as a dictionary. """ - properties = {name: properties} if properties is not None else None - data = self.get_multiple_body_data([name], properties) - if data is not None: + if wait: + return self.wait_for_body_data(name, properties) + + data = self.get_received_data() + if self.check_for_body_data(name, data, properties): return data[name] def get_multiple_body_data(self, body_names: List[str], @@ -192,6 +192,15 @@ def get_multiple_body_data(self, body_names: List[str], if self.check_multiple_body_data(body_names, data, properties): return {name: data[name] for name in body_names} + def wait_for_body_data(self, name: str, properties: Optional[List[Property]] = None) -> Dict: + """ + Wait for the body data from the multiverse server. + param name: The name of the body. + param properties: The properties of the body. + return: The body data as a dictionary. + """ + return self._wait_for_body_data_template(name, self.check_for_body_data, properties)[name] + def wait_for_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[Property]]] = None) -> Dict: """ @@ -200,16 +209,29 @@ def wait_for_multiple_body_data(self, body_names: List[str], param properties: The properties of the bodies. return: The body data as a dictionary. """ + return self._wait_for_body_data_template(body_names, self.check_multiple_body_data, properties) + + def _wait_for_body_data_template(self, body_names: Union[str, List[str]], + check_func: Callable[[Union[str, List[str]], Dict, Union[Dict, List]], bool], + properties: Optional[Union[Dict, List]] = None) -> Dict: + """ + Wait for the body data from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param properties: The properties of the bodies. + param check_func: The function to check if the data is received. + param return_func: The function to return the data. + return: The body data as a dictionary. + """ start = time() data_received_flag = False while time() - start < self.MAX_WAIT_TIME_FOR_DATA: received_data = self.get_received_data() - data_received_flag = self.check_multiple_body_data(body_names, received_data, properties) + data_received_flag = check_func(body_names, received_data, properties) if data_received_flag: - return {name: received_data[name] for name in body_names} + return received_data if not data_received_flag: properties_str = "Data" if properties is None else f"Properties {properties}" - msg = f"{properties_str} for bodies {body_names} not received within {self.MAX_WAIT_TIME_FOR_DATA} seconds" + msg = f"{properties_str} for {body_names} not received within {self.MAX_WAIT_TIME_FOR_DATA} seconds" logging.error(msg) raise ValueError(msg) @@ -303,8 +325,9 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List param position: The position of the body. param orientation: The orientation of the body. """ - self.set_multiple_body_poses({body_name: {BodyProperty.POSITION: position, - BodyProperty.ORIENTATION: orientation}}) + self.send_body_data_to_server(body_name, + {BodyProperty.POSITION: position, + BodyProperty.ORIENTATION: orientation}) def set_multiple_body_poses(self, body_data: Dict[str, Dict[BodyProperty, List[float]]]) -> None: """ @@ -319,7 +342,7 @@ def set_body_position(self, body_name: str, position: List[float]) -> None: param body_name: The name of the body. param position: The position of the body. """ - self.send_body_data_to_server(body_name, {BodyProperty.POSITION: position}) + self.set_body_property(body_name, BodyProperty.POSITION, position) def set_body_orientation(self, body_name: str, orientation: List[float]) -> None: """ @@ -327,7 +350,16 @@ def set_body_orientation(self, body_name: str, orientation: List[float]) -> None param body_name: The name of the body. param orientation: The orientation of the body. """ - self.send_body_data_to_server(body_name, {BodyProperty.ORIENTATION: orientation}) + self.set_body_property(body_name, BodyProperty.ORIENTATION, orientation) + + def set_body_property(self, body_name: str, property_: Property, value: List[float]) -> None: + """ + Set the body property in the simulation. + param body_name: The name of the body. + param property_: The property of the body. + param value: The value of the property. + """ + self.send_body_data_to_server(body_name, {property_: value}) def remove_body(self, body_name: str) -> None: """ @@ -344,14 +376,16 @@ def reset_world(self) -> None: """ self.send_data_to_server([0]) - def send_body_data_to_server(self, body_name: str, data: Dict[Property, List[float]]) -> Dict: + def send_body_data_to_server(self, body_name: str, body_data: Dict[Property, List[float]]) -> Dict: """ Send data to the multiverse server. param body_name: The name of the body. - param data: The data to be sent. + param body_data: The data to be sent. return: The response from the server. """ - return self.send_multiple_body_data_to_server({body_name: data}) + send_meta_data = {body_name: list(map(str, body_data.keys()))} + flattened_data = [value for data in body_data.values() for value in data] + return self.send_data_to_server([time() - self.time_start, *flattened_data], send_meta_data) def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, List[float]]]) -> Dict: """ @@ -410,7 +444,6 @@ def send_data_to_server(self, data: List, class MultiverseAPI(MultiverseClient): - BASE_NAME: str = "api_requester" """ The base name of the Multiverse reader. diff --git a/test/test_orm.py b/test/test_orm.py index 482ce14e3..7f33c65e8 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -1,4 +1,5 @@ import os +import time import unittest from sqlalchemy import select import sqlalchemy.orm From f1c40574ee4f9d2fcdd2e579cd1470930623f4db Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 26 Jul 2024 11:49:29 +0200 Subject: [PATCH 19/21] [Multiverse] added single joint validator. --- src/pycram/datastructures/world.py | 11 +++++---- src/pycram/worlds/multiverse.py | 4 +++- .../multiverse_functions/goal_validator.py | 23 +++++++++++++++++++ test/test_multiverse.py | 2 +- 4 files changed, 34 insertions(+), 6 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index f9433780a..113682211 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -229,16 +229,19 @@ def _init_goal_validators(self): Initializes the goal validators for the World objects' poses, positions, and orientations. """ - # Goal validators for an object + # Objects Pose goal validators self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, self.acceptable_percentage_of_goal) - - # Goal validators for multiple objects self.multi_pose_goal_validator = MultiPoseGoalValidator( lambda x: list(self.get_multiple_object_poses(x).values()), self.acceptable_pose_error, self.acceptable_percentage_of_goal) - # Goal validator for the joints of an object + # Joint Goal validators + self.joint_position_goal_validator = JointPositionGoalValidator( + self.get_joint_position, + acceptable_orientation_error=self.acceptable_orientation_error, + acceptable_position_error=self.acceptable_position_error, + acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), acceptable_orientation_error=self.acceptable_orientation_error, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 55d67d6dc..a8bad45d2 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -7,7 +7,8 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty -from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions +from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ + validate_joint_position from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -191,6 +192,7 @@ def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: return self.reader.get_multiple_body_orientations([link.name for link in links]) + @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: self.writer.set_body_property(joint.name, self.get_joint_position_name(joint), [joint_position]) return True diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index b59e70168..980faae9b 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -446,12 +446,14 @@ def validate_object_pose(pose_setter_func): """ def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): + attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) if len(attachments_pose_goal) == 0: world.pose_goal_validator.register_goal(pose, obj) else: world.multi_pose_goal_validator.register_goal([pose, *list(attachments_pose_goal.values())], [obj, *list(attachments_pose_goal.keys())]) + if not pose_setter_func(world, obj, pose): world.pose_goal_validator.reset() world.multi_pose_goal_validator.reset() @@ -464,6 +466,27 @@ def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): return wrapper +def validate_joint_position(position_setter_func): + """ + A decorator to validate the joint position. + :param position_setter_func: The function to set the joint position. + """ + + def wrapper(world: 'World', joint: 'Joint', position: float): + + joint_type = joint.type + world.joint_position_goal_validator.register_goal(position, joint_type, joint) + + if not position_setter_func(world, joint, position): + world.joint_position_goal_validator.reset() + return False + + world.joint_position_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper + + def validate_multiple_joint_positions(position_setter_func): """ A decorator to validate the joint positions, this function does not validate the virtual joints, diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 44c6c7084..91bdc52d2 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -175,7 +175,7 @@ def test_set_robot_orientation(self): self.multiverse.robot.set_orientation(new_quaternion) robot_orientation = self.multiverse.robot.get_orientation_as_list() quaternion_difference = calculate_angle_between_quaternions(new_quaternion, robot_orientation) - self.assertAlmostEqual(quaternion_difference, 0, delta=0.01) + self.assertAlmostEqual(quaternion_difference, 0, delta=self.multiverse.acceptable_orientation_error) # self.tearDown() def test_attach_object(self): From 81d78546e64e1690d863b9f43189c1cec4435154 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 13:00:36 +0200 Subject: [PATCH 20/21] [AbstractWorld] Moved joint type mapping for multiverse to the enum class definition, and defined a new exception for unsupported joint types, and defined a multiverse class variable as a tuple providing the supported joint types. --- src/pycram/worlds/multiverse.py | 17 ++++++----------- .../worlds/multiverse_datastructures/enums.py | 13 ++++++++++++- .../worlds/multiverse_functions/exceptions.py | 6 ++++++ test/test_multiverse.py | 8 +++++--- 4 files changed, 29 insertions(+), 15 deletions(-) create mode 100644 src/pycram/worlds/multiverse_functions/exceptions.py diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a8bad45d2..e663684a3 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -24,12 +24,9 @@ class Multiverse(World): This class implements an interface between Multiverse and PyCRAM. """ - _joint_type_to_position_name: Dict[JointType, MultiverseJointProperty] = { - JointType.REVOLUTE: MultiverseJointProperty.REVOLUTE_JOINT_POSITION, - JointType.PRISMATIC: MultiverseJointProperty.PRISMATIC_JOINT_POSITION, - } + supported_joint_types = (JointType.REVOLUTE, JointType.PRISMATIC) """ - A dictionary to map JointType to the corresponding multiverse attribute name. + A Tuple for the supported pycram joint types in Multiverse. """ added_multiverse_resources: bool = False @@ -139,10 +136,9 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def get_joint_position_name(self, joint: Joint) -> MultiverseJointProperty: - if joint.type not in self._joint_type_to_position_name: - raise ValueError(f"Joint type {joint.type} is not supported in Multiverse") - return self._joint_type_to_position_name[joint.type] + @staticmethod + def get_joint_position_name(joint: Joint) -> MultiverseJointProperty: + return MultiverseJointProperty.from_pycram_joint_type(joint.type) def load_object_and_get_id(self, name: Optional[str] = None, pose: Optional[Pose] = None, @@ -174,8 +170,7 @@ def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: return self.last_object_id def get_object_joint_names(self, obj: Object) -> List[str]: - return [joint.name for joint in obj.description.joints - if joint.type in self._joint_type_to_position_name.keys()] + return [joint.name for joint in obj.description.joints if joint.type in self.supported_joint_types] def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index cb001a789..12a943637 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -1,6 +1,8 @@ -from abc import ABC from enum import Enum +from pycram.datastructures.enums import JointType +from ..multiverse_functions.exceptions import UnsupportedJointType + class MultiverseAPIName(Enum): """ @@ -36,3 +38,12 @@ class MultiverseJointProperty(MultiverseProperty): PRISMATIC_JOINT_POSITION = "joint_tvalue" REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" + + @classmethod + def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointProperty': + if joint_type == JointType.REVOLUTE: + return MultiverseJointProperty.REVOLUTE_JOINT_POSITION + elif joint_type == JointType.PRISMATIC: + return MultiverseJointProperty.PRISMATIC_JOINT_POSITION + else: + raise UnsupportedJointType(joint_type) \ No newline at end of file diff --git a/src/pycram/worlds/multiverse_functions/exceptions.py b/src/pycram/worlds/multiverse_functions/exceptions.py new file mode 100644 index 000000000..bca03b173 --- /dev/null +++ b/src/pycram/worlds/multiverse_functions/exceptions.py @@ -0,0 +1,6 @@ +from pycram.datastructures.enums import JointType + + +class UnsupportedJointType(Exception): + def __init__(self, joint_type: 'JointType'): + super().__init__(f"Unsupported joint type: {joint_type}") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 91bdc52d2..c5509bd8f 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import os +import time import unittest import numpy as np @@ -130,7 +131,7 @@ def test_set_joint_position(self): original_joint_position = robot.get_joint_position(joint) robot.set_joint_position(joint, original_joint_position + step) joint_position = robot.get_joint_position(joint) - self.assertAlmostEqual(joint_position, original_joint_position + step, delta=0.01) + self.assertAlmostEqual(joint_position, original_joint_position + step, delta=0.05) def test_spawn_robot(self): if self.multiverse.robot is not None: @@ -164,6 +165,7 @@ def test_set_robot_position(self): robot_position = self.multiverse.robot.get_position_as_list() self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) self.tearDown() + time.sleep(0.5) def test_set_robot_orientation(self): self.spawn_robot() @@ -207,8 +209,8 @@ def test_detach_object(self): milk.set_position(milk_position) new_milk_position = milk.get_position_as_list() new_cup_position = cup.get_position_as_list() - self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.005) - self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.002) + self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.01) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.02) self.tearDown() def test_attach_with_robot(self): From 53237ac5ed442d4b994ede55b18ca69c18faf405 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 17:38:24 +0200 Subject: [PATCH 21/21] [MultiverseGoalValidation] Made the goal validator available globally not just for multiverse.py Added the use of validators to pybullet. --- src/pycram/datastructures/world.py | 28 ++++++++++++------- src/pycram/validation/__init__.py | 0 .../error_checkers.py | 0 .../goal_validator.py | 16 +++++------ src/pycram/world_concepts/world_object.py | 2 ++ src/pycram/worlds/bullet_world.py | 4 +++ src/pycram/worlds/multiverse.py | 2 +- test/test_error_checkers.py | 2 +- test/test_goal_validator.py | 4 +-- test/test_multiverse.py | 3 +- 10 files changed, 37 insertions(+), 24 deletions(-) create mode 100644 src/pycram/validation/__init__.py rename src/pycram/{worlds/multiverse_functions => validation}/error_checkers.py (100%) rename src/pycram/{worlds/multiverse_functions => validation}/goal_validator.py (97%) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index b6ac4a53c..1a4e9a540 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -27,9 +27,11 @@ from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event from ..config import world_conf as conf -from ..worlds.multiverse_functions.goal_validator import (MultiPoseGoalValidator, - PoseGoalValidator, JointPositionGoalValidator, - MultiJointPositionGoalValidator, GoalValidator) +from pycram.validation.goal_validator import (MultiPoseGoalValidator, + PoseGoalValidator, JointPositionGoalValidator, + MultiJointPositionGoalValidator, GoalValidator, + validate_joint_position, validate_multiple_joint_positions, + validate_object_pose) if TYPE_CHECKING: from ..world_concepts.world_object import Object @@ -354,10 +356,8 @@ def get_object_by_name(self, name: str) -> Optional[Object]: :return: The object with the given name, if there is one. """ - object = list(filter(lambda obj: obj.name == name, self.objects)) - if len(object) > 0: - return object[0] - return None + matching_objects = list(filter(lambda obj: obj.name == name, self.objects)) + return matching_objects[0] if len(matching_objects) > 0 else None def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: """ @@ -726,21 +726,26 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object, """ raise NotImplementedError + @validate_joint_position @abstractmethod def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation - + NOTE: It is recommended to use the validate_joint_position decorator to validate the joint position for + the implementation of this method. :param joint: The joint to reset the position for. :param joint_position: The new joint pose. :return: True if the reset was successful, False otherwise """ pass + @validate_multiple_joint_positions @abstractmethod def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: """ Set the positions of multiple joints of an articulated object. + NOTE: It is recommended to use the validate_multiple_joint_positions decorator to validate the + joint positions for the implementation of this method. :param joint_positions: A dictionary with joint objects as keys and joint positions as values. :return: True if the set was successful, False otherwise. """ @@ -753,12 +758,14 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ pass + @validate_object_pose @abstractmethod def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. - + NOTE: It is recommended to use the validate_object_pose decorator to validate the object pose for the + implementation of this method. :param obj: The object. :param pose: The new pose as a Pose object. :return: True if the reset was successful, False otherwise. @@ -1365,7 +1372,8 @@ def __init__(self): self.prev_world: Optional[World] = None # The previous world is saved to restore it after the with block is exited. - def sync_worlds(self): + @staticmethod + def sync_worlds(): """ Synchronizes the state of the prospection world with the main world. """ diff --git a/src/pycram/validation/__init__.py b/src/pycram/validation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/validation/error_checkers.py similarity index 100% rename from src/pycram/worlds/multiverse_functions/error_checkers.py rename to src/pycram/validation/error_checkers.py diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/validation/goal_validator.py similarity index 97% rename from src/pycram/worlds/multiverse_functions/goal_validator.py rename to src/pycram/validation/goal_validator.py index 980faae9b..437ea1a50 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -5,7 +5,7 @@ from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING from pycram.datastructures.enums import JointType -from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ +from pycram.validation.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, SingleValueErrorChecker if TYPE_CHECKING: @@ -368,10 +368,10 @@ class JointPositionGoalValidator(GoalValidator): def __init__(self, current_position_getter: OptionalArgCallable = None, acceptable_error: Optional[float] = None, - acceptable_orientation_error: Optional[Iterable[float]] = np.pi / 180, - acceptable_position_error: Optional[Iterable[float]] = 1e-3, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, - is_iterable: Optional[bool] = False): + acceptable_orientation_error: float = np.pi / 180, + acceptable_position_error: float = 1e-3, + acceptable_percentage_of_goal_achieved: float = 0.8, + is_iterable: bool = False): """ Initialize the joint position goal validator. :param current_position_getter: The current position getter function which takes an optional input and returns @@ -412,9 +412,9 @@ class MultiJointPositionGoalValidator(GoalValidator): def __init__(self, current_positions_getter: OptionalArgCallable = None, acceptable_error: Optional[Iterable[float]] = None, - acceptable_orientation_error: Optional[Iterable[float]] = np.pi / 180, - acceptable_position_error: Optional[Iterable[float]] = 1e-3, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + acceptable_orientation_error: float = np.pi / 180, + acceptable_position_error: float = 1e-3, + acceptable_percentage_of_goal_achieved: float = 0.8): """ Initialize the multi-joint position goal validator. :param current_positions_getter: The current positions getter function which takes an optional input and diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index d35e4242a..2b29b96a9 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -903,6 +903,7 @@ def reset_all_joints_positions(self) -> None: joint_positions = [0] * len(joint_names) self.set_multiple_joint_positions(dict(zip(joint_names, joint_positions))) + def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ Set the position of the given joint to the given joint pose and updates the poses of all attached objects. @@ -912,6 +913,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ self.world.reset_joint_position(self.joints[joint_name], joint_position) + def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 171a34445..7059b2850 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -17,6 +17,7 @@ from ..datastructures.pose import Pose from ..object_descriptors.urdf import ObjectDescription from ..datastructures.world import World +from ..validation.goal_validator import validate_multiple_joint_positions, validate_joint_position, validate_object_pose from ..world_concepts.constraints import Constraint from ..world_concepts.world_object import Object @@ -182,11 +183,13 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} + @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) return True + @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) return True @@ -198,6 +201,7 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None for obj, pose in objects.items(): self.reset_object_base_pose(obj, pose) + @validate_object_pose def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index e663684a3..f89427293 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -7,7 +7,7 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty -from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ +from pycram.validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType diff --git a/test/test_error_checkers.py b/test/test_error_checkers.py index b7a459ff9..63bf06416 100644 --- a/test/test_error_checkers.py +++ b/test/test_error_checkers.py @@ -4,7 +4,7 @@ from tf.transformations import quaternion_from_euler from pycram.datastructures.enums import JointType -from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ +from pycram.validation.error_checkers import calculate_angle_between_quaternions, \ PoseErrorChecker, PositionErrorChecker, OrientationErrorChecker, RevoluteJointPositionErrorChecker, \ PrismaticJointPositionErrorChecker, MultiJointPositionErrorChecker diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index 6ebfe04ff..9b79cc114 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -6,10 +6,10 @@ from pycram.datastructures.enums import JointType from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescription -from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ +from pycram.validation.error_checkers import PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ MultiJointPositionErrorChecker -from pycram.worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ +from pycram.validation.goal_validator import GoalValidator, PoseGoalValidator, \ PositionGoalValidator, OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, \ MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 9ea99398a..f01483a0a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import time import unittest import numpy as np @@ -13,7 +12,7 @@ from pycram.designators.object_designator import BelieveObject from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object -from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions +from pycram.validation.error_checkers import calculate_angle_between_quaternions multiverse_installed = True try: