From 4c950bcdd6c30453b4dc30d23dc1113ca2700215 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 29 Nov 2023 19:59:50 +0100 Subject: [PATCH 01/69] [RemovingPybulletFromObjectClass] removed all usages of pybullet from the object class; needs tests to verify functionality. --- src/pycram/world.py | 1866 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1866 insertions(+) create mode 100644 src/pycram/world.py diff --git a/src/pycram/world.py b/src/pycram/world.py new file mode 100644 index 000000000..095604298 --- /dev/null +++ b/src/pycram/world.py @@ -0,0 +1,1866 @@ +# used for delayed evaluation of typing until python 3.11 becomes mainstream +from __future__ import annotations + +import logging +import os +import pathlib +import re +import threading +import time +import xml.etree.ElementTree +from queue import Queue +import tf +from typing import List, Optional, Dict, Tuple, Callable, Iterable +from typing import Union + +import numpy as np +import pybullet as p +import rospkg +import rospy +import rosgraph + +import urdf_parser_py.urdf +from geometry_msgs.msg import Quaternion, Point +from urdf_parser_py.urdf import URDF + +from .event import Event +from .robot_descriptions import robot_description +from .enums import JointType, ObjectType +from .local_transformer import LocalTransformer +from sensor_msgs.msg import JointState + +from .pose import Pose, Transform + + +class BulletWorld: + """ + The BulletWorld Class represents the physics Simulation and belief state. + """ + + current_bullet_world: BulletWorld = None + """ + Global reference to the currently used BulletWorld, usually this is the + graphical one. However, if you are inside a Use_shadow_world() environment the current_bullet_world points to the + shadow world. In this way you can comfortably use the current_bullet_world, which should point towards the BulletWorld + used at the moment. + """ + robot: Object = None + """ + Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the + URDF with the name of the URDF on the parameter server. + """ + + # Check is for sphinx autoAPI to be able to work in a CI workflow + if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): + rospy.init_node('pycram') + + def __init__(self, type: str = "GUI", is_shadow_world: bool = False): + """ + Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the + background. There can only be one rendered simulation. + The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. + + :param type: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" + :param is_shadow_world: For internal usage, decides if this BulletWorld should be used as a shadow world. + """ + self.objects: List[Object] = [] + self.client_id: int = -1 + self.detachment_event: Event = Event() + self.attachment_event: Event = Event() + self.manipulation_event: Event = Event() + self.type: str = type + self._gui_thread: Gui = Gui(self, type) + self._gui_thread.start() + # This disables file caching from PyBullet, since this would also cache + # files that can not be loaded + p.setPhysicsEngineParameter(enableFileCaching=0) + # Needed to let the other thread start the simulation, before Objects are spawned. + time.sleep(0.1) + if BulletWorld.current_bullet_world == None: + BulletWorld.current_bullet_world = self + self.vis_axis: Object = [] + self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} + self.data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] + self.shadow_world: BulletWorld = BulletWorld("DIRECT", True) if not is_shadow_world else None + self.world_sync: WorldSync = WorldSync(self, self.shadow_world) if not is_shadow_world else None + self.is_shadow_world: bool = is_shadow_world + self.local_transformer = LocalTransformer() + if not is_shadow_world: + self.world_sync.start() + self.local_transformer.bullet_world = self + self.local_transformer.shadow_world = self.shadow_world + + # Some default settings + self.set_gravity([0, 0, -9.8]) + if not is_shadow_world: + plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) + # atexit.register(self.exit) + + def get_objects_by_name(self, name: str) -> List[Object]: + """ + Returns a list of all Objects in this BulletWorld with the same name as the given one. + + :param name: The name of the returned Objects. + :return: A list of all Objects with the name 'name'. + """ + return list(filter(lambda obj: obj.name == name, self.objects)) + + def get_objects_by_type(self, obj_type: str) -> List[Object]: + """ + Returns a list of all Objects which have the type 'obj_type'. + + :param obj_type: The type of the returned Objects. + :return: A list of all Objects that have the type 'obj_type'. + """ + return list(filter(lambda obj: obj.type == obj_type, self.objects)) + + def get_object_by_id(self, id: int) -> Object: + """ + Returns the single Object that has the unique id. + + :param id: The unique id for which the Object should be returned. + :return: The Object with the id 'id'. + """ + return list(filter(lambda obj: obj.id == id, self.objects))[0] + + def remove_object(self, obj_id: int) -> None: + """ + Remove an object by its ID. + + :param obj_id: The unique id of the object to be removed. + """ + + p.removeBody(obj_id, self.client_id) + + def attach(self, + parent_obj: Object, + child_obj: Object, + parent_link: Optional[str] = None, + loose: Optional[bool] = False) -> None: + """ + Attaches another object to this object. This is done by + saving the transformation between the given link, if there is one, and + the base pose of the other object. Additionally, the name of the link, to + which the object is attached, will be saved. + Furthermore, a constraint of pybullet will be created so the attachment + also works while simulation. + Loose attachments means that the attachment will only be one-directional. + For example, if the parent object moves, the child object will also move, but not the other way around. + + :param parent_obj: The parent object (jf loose, then this would not move when child moves) + :param child_obj: The child object + :param parent_link: The link of the parent object to which the child object should be attached + :param loose: If the attachment should be a loose attachment. + """ + link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 + link_to_object = parent_obj._calculate_transform(child_obj, parent_link) + parent_obj.attachments[child_obj] = [link_to_object, parent_link, loose] + child_obj.attachments[parent_obj] = [link_to_object.invert(), None, False] + + cid = p.createConstraint(parent_obj.id, link_id, child_obj.id, -1, p.JOINT_FIXED, + [0, 1, 0], link_to_object.translation_as_list(), [0, 0, 0], + link_to_object.rotation_as_list(), + physicsClientId=self.client_id) + parent_obj.cids[child_obj] = cid + child_obj.cids[parent_obj] = cid + self.attachment_event(parent_obj, [parent_obj, child_obj]) + + def detach(self, obj1: Object, obj2: Object) -> None: + """ + Detaches obj2 from obj1. This is done by + deleting the attachment from the attachments dictionary of both objects + and deleting the constraint of pybullet. + Afterward the detachment event of the corresponding BulletWorld will be fired. + + :param obj1: The object from which an object should be detached. + :param obj2: The object which should be detached. + """ + del obj1.attachments[obj2] + del obj2.attachments[obj1] + + p.removeConstraint(obj1.cids[obj2], physicsClientId=self.client_id) + + del obj1.cids[obj2] + del obj2.cids[obj1] + obj1.world.detachment_event(obj1, [obj1, obj2]) + + def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: + """ + Updates the positions of all attached objects. This is done + by calculating the new pose in world coordinate frame and setting the + base pose of the attached objects to this new pose. + After this the _set_attached_objects method of all attached objects + will be called. + + :param prev_object: A list of Objects that were already moved, + these will be excluded to prevent loops in the update. + """ + for att_obj in obj.attachments: + if att_obj in prev_object: + continue + if obj.attachments[att_obj][2]: + # Updates the attachment transformation and constraint if the + # attachment is loose, instead of updating the position of all attached objects + link_to_object = obj._calculate_transform(att_obj, obj.attachments[att_obj][1]) + link_id = obj.get_link_id(obj.attachments[att_obj][1]) if obj.attachments[att_obj][1] else -1 + obj.attachments[att_obj][0] = link_to_object + att_obj.attachments[obj][0] = link_to_object.invert() + p.removeConstraint(obj.cids[att_obj], physicsClientId=self.client_id) + cid = p.createConstraint(obj.id, link_id, att_obj.id, -1, p.JOINT_FIXED, [0, 0, 0], + link_to_object.translation_as_list(), + [0, 0, 0], link_to_object.rotation_as_list(), + physicsClientId=self.client_id) + obj.cids[att_obj] = cid + att_obj.cids[obj] = cid + else: + link_to_object = obj.attachments[att_obj][0] + + world_to_object = obj.local_transformer.transform_pose(link_to_object.to_pose(), "map") + p.resetBasePositionAndOrientation(att_obj.id, world_to_object.position_as_list(), + world_to_object.orientation_as_list(), + physicsClientId=self.client_id) + att_obj._current_pose = world_to_object + self._set_attached_objects(att_obj, prev_object + [obj]) + + def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: + """ + Get the joint limits of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + :return: A tuple containing the upper and the lower limits of the joint. + """ + up_lim, low_lim = p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8:10] + return up_lim, low_lim + + def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: + """ + Returns the axis along which a joint is moving. The given joint_name has to be part of this object. + + :param obj: The object + :param joint_name: Name of the joint for which the axis should be returned. + :return: The axis a vector of xyz + """ + return p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[13] + + def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: + """ + Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. + + :param obj: The object + :param joint_name: Joint name for which the type should be returned + :return: The type of the joint + """ + joint_type = p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[2] + return JointType(joint_type) + + def get_object_joint_position(self, obj: Object, joint_name: str) -> float: + """ + Get the state of a joint of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + """ + return p.getJointState(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[0] + + def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: + """ + Get the pose of a link of an articulated object with respect to the world frame. + The pose is given as a tuple of position and orientation. + + :param obj: The object + :param link_name: The name of the link + """ + return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] + + def get_object_contact_points(self, obj: Object) -> List: + """ + Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned + list please look at `PyBullet Doc `_ + + :param obj: The object. + :return: A list of all contact points with other objects + """ + return p.getContactPoints(obj.id) + + def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: + """ + Get the ID of a joint in an articulated object. + + :param obj: The object + :param joint_idx: The index of the joint (would indicate order). + """ + return p.getJointInfo(obj.id, joint_idx, self.client_id)[0] + + def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: + """ + Get the name of a joint in an articulated object. + + :param obj: The object + :param joint_idx: The index of the joint (would indicate order). + """ + return p.getJointInfo(obj.id, joint_idx, self.client_id)[1].decode('utf-8') + + def get_object_link_name(self, obj: Object, link_idx: int) -> str: + """ + Get the name of a link in an articulated object. + + :param obj: The object + :param link_idx: The index of the link (would indicate order). + """ + return p.getJointInfo(obj.id, link_idx, self.client_id)[12].decode('utf-8') + + def get_object_number_of_joints(self, obj: Object) -> int: + """ + Get the number of joints of an articulated object + + :param obj: The object + """ + return p.getNumJoints(obj.id, self.client_id) + + def reset_joint_state(self, obj: Object, joint_name: str, joint_pose: float) -> None: + """ + Reset the joint state instantly without physics simulation + + :param obj: The object + :param joint_name: The name of the joint + :param joint_pose: The new joint pose + """ + p.resetJointState(obj.id, obj.joints[joint_name], joint_pose, physicsClientId=self.client_id) + + def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): + """ + 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 position: The new position of the object as a vector of x,y,z + :param orientation: The new orientation of the object as a quaternion of x,y,z,w + """ + p.resetBasePositionAndOrientation(obj.id, position, orientation, self.client_id) + + def step(self): + """ + Step the world simulation using forward dynamics + """ + p.stepSimulation(self.client_id) + + def set_object_color(self, obj: Object, color: List[float], link: Optional[str] = ""): + """ + Changes the color of this object, the color has to be given as a list + of RGBA values. Optionally a link name can can be provided, if no link + name is provided all links of this object will be colored. + + :param obj: The object which should be colored + :param color: The color as RGBA values between 0 and 1 + :param link: The link name of the link which should be colored + """ + if link == "": + # Check if there is only one link, this is the case for primitive + # forms or if loaded from an .stl or .obj file + if obj.links != {}: + for link_id in obj.links.values(): + p.changeVisualShape(obj.id, link_id, rgbaColor=color, physicsClientId=self.client_id) + else: + p.changeVisualShape(obj.id, -1, rgbaColor=color, physicsClientId=self.client_id) + else: + p.changeVisualShape(obj.id, obj.links[link], rgbaColor=color, physicsClientId=self.client_id) + + def get_object_color(self, + obj: Object, + link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: + """ + This method returns the color of this object or a link of this object. If no link is given then the + return is either: + + 1. A list with the color as RGBA values, this is the case if the object only has one link (this + happens for example if the object is spawned from a .obj or .stl file) + 2. A dict with the link name as key and the color as value. The color is given as RGBA values. + Please keep in mind that not every link may have a color. This is dependent on the URDF from which the + object is spawned. + + If a link is specified then the return is a list with RGBA values representing the color of this link. + It may be that this link has no color, in this case the return is None as well as an error message. + + :param obj: The object for which the color should be returned. + :param link: the link name for which the color should be returned. + :return: The color of the object or link, or a dictionary containing every colored link with its color + """ + visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) + swap = {v: k for k, v in obj.links.items()} + links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) + colors = list(map(lambda x: x[7], visual_data)) + link_to_color = dict(zip(links, colors)) + + if link: + if link in link_to_color.keys(): + return link_to_color[link] + elif link not in obj.links.keys(): + rospy.logerr(f"The link '{link}' is not part of this obejct") + return None + else: + rospy.logerr(f"The link '{link}' has no color") + return None + + if len(visual_data) == 1: + return list(visual_data[0][7]) + else: + return link_to_color + + def get_object_AABB(self, obj: Object, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case + the axis aligned bounding box of the link will be returned. The return of this method are two points in + world coordinate frame which define a bounding box. + + :param obj: The object for which the bounding box should be returned. + :param link_name: The Optional name of a link of this object. + :return: Two lists of x,y,z which define the bounding box. + """ + if link_name: + return p.getAABB(obj.id, obj.links[link_name], self.client_id) + else: + return p.getAABB(obj.id, physicsClientId=self.client_id) + + def get_attachment_event(self) -> Event: + """ + Returns the event reference that is fired if an attachment occurs. + + :return: The reference to the attachment event + """ + return self.attachment_event + + def get_detachment_event(self) -> Event: + """ + Returns the event reference that is fired if a detachment occurs. + + :return: The event reference for the detachment event. + """ + return self.detachment_event + + def get_manipulation_event(self) -> Event: + """ + Returns the event reference that is fired if any manipulation occurs. + + :return: The event reference for the manipulation event. + """ + return self.manipulation_event + + def set_realtime(self, real_time: bool) -> None: + """ + Enables the real time simulation of Physic in the BulletWorld. By default this is disabled and Physic is only + simulated to reason about it. + + :param real_time: Whether the BulletWorld should simulate Physic in real time. + """ + p.setRealTimeSimulation(1 if real_time else 0, self.client_id) + + def set_gravity(self, velocity: List[float]) -> None: + """ + Sets the gravity that is used in the BullteWorld, by default the is the gravity on earth ([0, 0, -9.8]). Gravity + is given as a vector in x,y,z. Gravity is only applied while simulating Physic. + + :param velocity: The gravity vector that should be used in the BulletWorld. + """ + p.setGravity(velocity[0], velocity[1], velocity[2], physicsClientId=self.client_id) + + def set_robot(self, robot: Object) -> None: + """ + Sets the global variable for the robot Object. This should be set on spawning the robot. + + :param robot: The Object reference to the Object representing the robot. + """ + BulletWorld.robot = robot + + def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: + """ + Simulates Physic in the BulletWorld for a given amount of seconds. Usually this simulation is faster than real + time, meaning you can simulate for example 10 seconds of Physic in the BulletWorld in 1 second real time. By + setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real + time. + + :param seconds: The amount of seconds that should be simulated. + :param real_time: If the simulation should happen in real time or faster. + """ + for i in range(0, int(seconds * 240)): + p.stepSimulation(self.client_id) + for objects, callback in self.coll_callbacks.items(): + contact_points = p.getContactPoints(objects[0].id, objects[1].id, physicsClientId=self.client_id) + # contact_points = p.getClosestPoints(objects[0].id, objects[1].id, 0.02) + # print(contact_points[0][5]) + if contact_points != (): + callback[0]() + elif callback[1] != None: # Call no collision callback + callback[1]() + if real_time: + # Simulation runs at 240 Hz + time.sleep(0.004167) + + def exit(self) -> None: + """ + Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the + preferred method to close the BulletWorld. + """ + # True if this is NOT the shadow world since it has a reference to the + # Shadow world + time.sleep(0.1) + if self.shadow_world: + self.world_sync.terminate = True + self.world_sync.join() + self.shadow_world.exit() + p.disconnect(self.client_id) + if self._gui_thread: + self._gui_thread.join() + if BulletWorld.current_bullet_world == self: + BulletWorld.current_bullet_world = None + BulletWorld.robot = None + + def save_state(self) -> Tuple[int, Dict]: + """ + Returns the id of the saved state of the BulletWorld. The saved state contains the position, orientation and joint + position of every Object in the BulletWorld. + + :return: A unique id of the state + """ + objects2attached = {} + # ToDo find out what this is for and where it is used + for o in self.objects: + objects2attached[o] = (o.attachments.copy(), o.cids.copy()) + return p.saveState(self.client_id), objects2attached + + def restore_state(self, state, objects2attached: Dict = {}) -> None: + """ + Restores the state of the BulletWorld according to the given state id. This includes position, orientation and + joint states. However, restore can not respawn objects if there are objects that were deleted between creation of + the state and restoring they will be skiped. + + :param state: The unique id representing the state, as returned by :func:`~save_state` + :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~bullet_world.Object.attachments` + """ + p.restoreState(state, physicsClientId=self.client_id) + for obj in self.objects: + try: + obj.attachments, obj.cids = objects2attached[obj] + except KeyError: + continue + + def copy(self) -> BulletWorld: + """ + Copies this Bullet World into another and returns it. The other BulletWorld + will be in Direct mode. The shadow world should always be preferred instead of creating a new BulletWorld. + This method should only be used if necessary since there can be unforeseen problems. + + :return: The reference to the new BulletWorld + """ + world = BulletWorld("DIRECT") + for obj in self.objects: + o = Object(obj.name, obj.type, obj.path, obj.get_position(), obj.get_orientation(), + world, obj.color) + for joint in obj.joints: + o.set_joint_state(joint, obj.get_joint_state(joint)) + return world + + def add_vis_axis(self, pose: Pose, + length: Optional[float] = 0.2) -> None: + """ + Creates a Visual object which represents the coordinate frame at the given + position and orientation. There can be an unlimited amount of vis axis objects. + + :param pose: The pose at which the axis should be spawned + :param length: Optional parameter to configure the length of the axes + """ + + position, orientation = pose.to_list() + + vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], + rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) + vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], + rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) + vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], + rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) + + obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], + basePosition=position, baseOrientation=orientation, + linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], + linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], + linkParentIndices=[0, 0, 0], + linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], + linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], + linkCollisionShapeIndices=[-1, -1, -1]) + + self.vis_axis.append(obj) + + def remove_vis_axis(self) -> None: + """ + Removes all spawned vis axis objects that are currently in this BulletWorld. + """ + for id in self.vis_axis: + p.removeBody(id) + self.vis_axis = [] + + def register_collision_callback(self, objectA: Object, objectB: Object, + callback_collision: Callable, + callback_no_collision: Optional[Callable] = None) -> None: + """ + Registers callback methods for contact between two Objects. There can be a callback for when the two Objects + get in contact and, optionally, for when they are not in contact anymore. + + :param objectA: An object in the BulletWorld + :param objectB: Another object in the BulletWorld + :param callback_collision: A function that should be called if the objects are in contact + :param callback_no_collision: A function that should be called if the objects are not in contact + """ + self.coll_callbacks[(objectA, objectB)] = (callback_collision, callback_no_collision) + + def add_additional_resource_path(self, path: str) -> None: + """ + Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an + Object is spawned only with a filename. + + :param path: A path in the filesystem in which to search for files. + """ + self.data_directory.append(path) + + def get_shadow_object(self, object: Object) -> Object: + """ + Returns the corresponding object from the shadow world for the given object. If the given Object is already in + the shadow world it is returned. + + :param object: The object for which the shadow worlds object should be returned. + :return: The corresponding object in the shadow world. + """ + try: + return self.world_sync.object_mapping[object] + except KeyError: + shadow_world = self if self.is_shadow_world else self.shadow_world + if object in shadow_world.objects: + return object + else: + raise ValueError( + f"There is no shadow object for the given object: {object}, this could be the case if the object isn't anymore in the main (graphical) BulletWorld or if the given object is already a shadow object. ") + + def get_bullet_object_for_shadow(self, object: Object) -> Object: + """ + Returns the corresponding object from the main Bullet World for a given + object in the shadow world. If the given object is not in the shadow + world an error will be raised. + + :param object: The object for which the corresponding object in the main Bullet World should be found + :return: The object in the main Bullet World + """ + map = self.world_sync.object_mapping + try: + return list(map.keys())[list(map.values()).index(object)] + except ValueError: + raise ValueError("The given object is not in the shadow world.") + + def reset_bullet_world(self) -> None: + """ + Resets the BulletWorld to the state it was first spawned in. + All attached objects will be detached, all joints will be set to the + default position of 0 and all objects will be set to the position and + orientation in which they were spawned. + """ + for obj in self.objects: + if obj.attachments: + attached_objects = list(obj.attachments.keys()) + for att_obj in attached_objects: + obj.detach(att_obj) + joint_names = list(obj.joints.keys()) + joint_poses = [0 for j in joint_names] + obj.set_joint_states(dict(zip(joint_names, joint_poses))) + obj.set_pose(obj.original_pose) + + +class Use_shadow_world(): + """ + An environment for using the shadow world, while in this environment the :py:attr:`~BulletWorld.current_bullet_world` + variable will point to the shadow world. + + Example: + with Use_shadow_world(): + NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() + """ + + def __init__(self): + self.prev_world: BulletWorld = None + + def __enter__(self): + if not BulletWorld.current_bullet_world.is_shadow_world: + time.sleep(20 / 240) + # blocks until the adding queue is ready + BulletWorld.current_bullet_world.world_sync.add_obj_queue.join() + # **This is currently not used since the sleep(20/240) seems to be enough, but on weaker hardware this might + # not be a feasible solution** + # while not BulletWorld.current_bullet_world.world_sync.equal_states: + # time.sleep(0.1) + + self.prev_world = BulletWorld.current_bullet_world + BulletWorld.current_bullet_world.world_sync.pause_sync = True + BulletWorld.current_bullet_world = BulletWorld.current_bullet_world.shadow_world + + def __exit__(self, *args): + if not self.prev_world == None: + BulletWorld.current_bullet_world = self.prev_world + BulletWorld.current_bullet_world.world_sync.pause_sync = False + + +class WorldSync(threading.Thread): + """ + Synchronizes the state between the BulletWorld and its shadow world. + Meaning the cartesian and joint position of everything in the shadow world will be + synchronized with the main BulletWorld. + Adding and removing objects is done via queues, such that loading times of objects + in the shadow world does not affect the BulletWorld. + The class provides the possibility to pause the synchronization, this can be used + if reasoning should be done in the shadow world. + """ + + def __init__(self, world: BulletWorld, shadow_world: BulletWorld): + threading.Thread.__init__(self) + self.world: BulletWorld = world + self.shadow_world: BulletWorld = shadow_world + self.shadow_world.world_sync: WorldSync = self + + self.terminate: bool = False + self.add_obj_queue: Queue = Queue() + self.remove_obj_queue: Queue = Queue() + self.pause_sync: bool = False + # Maps bullet to shadow world objects + self.object_mapping: Dict[Object, Object] = {} + self.equal_states = False + + def run(self): + """ + Main method of the synchronization, this thread runs in a loop until the + terminate flag is set. + While this loop runs it continuously checks the cartesian and joint position of + every object in the BulletWorld and updates the corresponding object in the + shadow world. When there are entries in the adding or removing queue the corresponding objects will be added + or removed in the same iteration. + """ + while not self.terminate: + self.check_for_pause() + # self.equal_states = False + for i in range(self.add_obj_queue.qsize()): + obj = self.add_obj_queue.get() + # [name, type, path, position, orientation, self.world.shadow_world, color, bulletworld object] + o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) + # Maps the BulletWorld object to the shadow world object + self.object_mapping[obj[7]] = o + self.add_obj_queue.task_done() + for i in range(self.remove_obj_queue.qsize()): + obj = self.remove_obj_queue.get() + # Get shadow world object reference from object mapping + shadow_obj = self.object_mapping[obj] + shadow_obj.remove() + del self.object_mapping[obj] + self.remove_obj_queue.task_done() + + for bulletworld_obj, shadow_obj in self.object_mapping.items(): + b_pose = bulletworld_obj.get_pose() + s_pose = shadow_obj.get_pose() + if b_pose.dist(s_pose) != 0.0: + shadow_obj.set_pose(bulletworld_obj.get_pose()) + + # Manage joint positions + if len(bulletworld_obj.joints) > 2: + for joint_name in bulletworld_obj.joints.keys(): + if shadow_obj.get_joint_state(joint_name) != bulletworld_obj.get_joint_state(joint_name): + shadow_obj.set_joint_states(bulletworld_obj.get_complete_joint_state()) + break + + self.check_for_pause() + # self.check_for_equal() + time.sleep(1 / 240) + + self.add_obj_queue.join() + self.remove_obj_queue.join() + + def check_for_pause(self) -> None: + """ + Checks if :py:attr:`~self.pause_sync` is true and sleeps this thread until it isn't anymore. + """ + while self.pause_sync: + time.sleep(0.1) + + def check_for_equal(self) -> None: + """ + Checks if both BulletWorlds have the same state, meaning all objects are in the same position. + This is currently not used, but might be used in the future if synchronization issues worsen. + """ + eql = True + for obj, shadow_obj in self.object_mapping.items(): + eql = eql and obj.get_pose() == shadow_obj.get_pose() + self.equal_states = eql + + +class Gui(threading.Thread): + """ + For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~BulletWorld.exit` + Also contains the code for controlling the camera. + """ + + def __init__(self, world, type): + threading.Thread.__init__(self) + self.world: BulletWorld = world + self.type: str = type + + def run(self): + """ + Initializes the new simulation and checks in an endless loop + if it is still active. If it is the thread will be suspended for 1/80 seconds, if it is not the method and + thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. + """ + if self.type != "GUI": + self.world.client_id = p.connect(p.DIRECT) + else: + self.world.client_id = p.connect(p.GUI) + + # Disable the side windows of the GUI + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + # Change the init camera pose + p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, + cameraTargetPosition=[-2, 0, 1]) + + # Get the initial camera target location + cameraTargetPosition = p.getDebugVisualizerCamera()[11] + + sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1]) + + # Create a sphere with a radius of 0.05 and a mass of 0 + sphereUid = p.createMultiBody(baseMass=0.0, + baseInertialFramePosition=[0, 0, 0], + baseVisualShapeIndex=sphereVisualId, + basePosition=cameraTargetPosition) + + # Define the maxSpeed, used in calculations + maxSpeed = 16 + + # Set initial Camera Rotation + cameraYaw = 50 + cameraPitch = -35 + + # Keep track of the mouse state + mouseState = [0, 0, 0] + oldMouseX, oldMouseY = 0, 0 + + # Determines if the sphere at cameraTargetPosition is visible + visible = 1 + + # Loop to update the camera position based on keyboard events + while p.isConnected(self.world.client_id): + # Monitor user input + keys = p.getKeyboardEvents() + mouse = p.getMouseEvents() + + # Get infos about the camera + width, height, dist = p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], \ + p.getDebugVisualizerCamera()[10] + cameraTargetPosition = p.getDebugVisualizerCamera()[11] + + # Get vectors used for movement on x,y,z Vector + xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] + yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] + zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] + + # Check the mouse state + if mouse: + for m in mouse: + + mouseX = m[2] + mouseY = m[1] + + # update mouseState + # Left Mouse button + if m[0] == 2 and m[3] == 0: + mouseState[0] = m[4] + # Middle mouse butto (scroll wheel) + if m[0] == 2 and m[3] == 1: + mouseState[1] = m[4] + # right mouse button + if m[0] == 2 and m[3] == 2: + mouseState[2] = m[4] + + # change visibility by clicking the mousewheel + if m[4] == 6 and m[3] == 1 and visible == 1: + visible = 0 + elif m[4] == 6 and visible == 0: + visible = 1 + + # camera movement when the left mouse button is pressed + if mouseState[0] == 3: + speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < maxSpeed else maxSpeed + speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < maxSpeed else maxSpeed + + # max angle of 89.5 and -89.5 to make sure the camera does not flip (is annoying) + if mouseX < oldMouseX: + if (cameraPitch + speedX) < 89.5: + cameraPitch += (speedX / 4) + 1 + elif mouseX > oldMouseX: + if (cameraPitch - speedX) > -89.5: + cameraPitch -= (speedX / 4) + 1 + + if mouseY < oldMouseY: + cameraYaw += (speedY / 4) + 1 + elif mouseY > oldMouseY: + cameraYaw -= (speedY / 4) + 1 + + if mouseState[1] == 3: + speedX = abs(oldMouseX - mouseX) + factor = 0.05 + + if mouseX < oldMouseX: + dist = dist - speedX * factor + elif mouseX > oldMouseX: + dist = dist + speedX * factor + dist = max(dist, 0.1) + + # camera movement when the right mouse button is pressed + if mouseState[2] == 3: + speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < 5 else 5 + speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < 5 else 5 + factor = 0.05 + + if mouseX < oldMouseX: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(zVec, factor), speedX)) + elif mouseX > oldMouseX: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(zVec, factor), speedX)) + + if mouseY < oldMouseY: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(xVec, factor), speedY)) + elif mouseY > oldMouseY: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(xVec, factor), speedY)) + # update oldMouse values + oldMouseY, oldMouseX = mouseY, mouseX + + # check the keyboard state + if keys: + # if shift is pressed, double the speed + if p.B3G_SHIFT in keys: + speedMult = 5 + else: + speedMult = 2.5 + + # if control is pressed, the movements caused by the arrowkeys, the '+' as well as the '-' key + # change + if p.B3G_CONTROL in keys: + + # the up and down arrowkeys cause the targetPos to move along the z axis of the map + if p.B3G_DOWN_ARROW in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(zVec, 0.03), speedMult)) + elif p.B3G_UP_ARROW in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(zVec, 0.03), speedMult)) + + # left and right arrowkeys cause the targetPos to move horizontally relative to the camera + if p.B3G_LEFT_ARROW in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(xVec, 0.03), speedMult)) + elif p.B3G_RIGHT_ARROW in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(xVec, 0.03), speedMult)) + + # the '+' and '-' keys cause the targetpos to move forwards and backwards relative to the camera + # while the camera stays at a constant distance. SHIFT + '=' is for US layout + if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(yVec, 0.03), speedMult)) + elif ord("-") in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(yVec, 0.03), speedMult)) + + # standard bindings for thearrowkeys, the '+' as well as the '-' key + else: + + # left and right arrowkeys cause the camera to rotate around the yaw axis + if p.B3G_RIGHT_ARROW in keys: + cameraYaw += (360 / width) * speedMult + elif p.B3G_LEFT_ARROW in keys: + cameraYaw -= (360 / width) * speedMult + + # the up and down arrowkeys cause the camera to rotate around the pitch axis + if p.B3G_DOWN_ARROW in keys: + if (cameraPitch + (360 / height) * speedMult) < 89.5: + cameraPitch += (360 / height) * speedMult + elif p.B3G_UP_ARROW in keys: + if (cameraPitch - (360 / height) * speedMult) > -89.5: + cameraPitch -= (360 / height) * speedMult + + # the '+' and '-' keys cause the camera to zoom towards and away from the targetPos without + # moving it. SHIFT + '=' is for US layout since the events can't handle shift plus something + if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: + if (dist - (dist * 0.02) * speedMult) > 0.1: + dist -= dist * 0.02 * speedMult + elif ord("-") in keys: + dist += dist * 0.02 * speedMult + + p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, + cameraTargetPosition=cameraTargetPosition) + if visible == 0: + cameraTargetPosition = (0.0, -50, 50) + p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) + time.sleep(1. / 80.) + + +class Object: + """ + Represents a spawned Object in the BulletWorld. + """ + + def __init__(self, name: str, type: Union[str, ObjectType], path: str, + pose: Pose = None, + world: BulletWorld = None, + color: Optional[List[float]] = [1, 1, 1, 1], + ignoreCachedFiles: Optional[bool] = False): + """ + The constructor loads the urdf file into the given BulletWorld, if no BulletWorld is specified the + :py:attr:`~BulletWorld.current_bullet_world` will be used. It is also possible to load .obj and .stl file into the BulletWorld. + The color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. + + :param name: The name of the object + :param type: The type of the object + :param path: The path to the source file, if only a filename is provided then the resourcer directories will be searched + :param pose: The pose at which the Object should be spawned + :param world: The BulletWorld in which the object should be spawned, if no world is specified the :py:attr:`~BulletWorld.current_bullet_world` will be used + :param color: The color with which the object should be spawned. + :param ignoreCachedFiles: If true the file will be spawned while ignoring cached files. + """ + if not pose: + pose = Pose() + self.world: BulletWorld = world if world is not None else BulletWorld.current_bullet_world + self.local_transformer = LocalTransformer() + self.name: str = name + self.type: str = type + self.color: List[float] = color + pose_in_map = self.local_transformer.transform_pose(pose, "map") + position, orientation = pose_in_map.to_list() + self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignoreCachedFiles) + self.joints: Dict[str, int] = self._joint_or_link_name_to_id("joint") + self.links: Dict[str, int] = self._joint_or_link_name_to_id("link") + self.attachments: Dict[Object, List] = {} + self.cids: Dict[Object, int] = {} + self.original_pose = pose_in_map + + self.tf_frame = ("shadow/" if self.world.is_shadow_world else "") + self.name + "_" + str(self.id) + + # This means "world" is not the shadow world since it has a reference to a shadow world + if self.world.shadow_world != None: + self.world.world_sync.add_obj_queue.put( + [name, type, path, position, orientation, self.world.shadow_world, color, self]) + + with open(self.path) as f: + self.urdf_object = URDF.from_xml_string(f.read()) + if self.urdf_object.name == robot_description.name and not BulletWorld.robot: + BulletWorld.robot = self + + self.links[self.urdf_object.get_root()] = -1 + + self._current_pose = pose_in_map + self._current_link_poses = {} + self._current_link_transforms = {} + self._current_joint_states = {} + self._init_current_joint_states() + self._update_link_poses() + + self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) + self.local_transformer.update_transforms_for_object(self) + self.link_to_geometry = self._get_geometry_for_link() + + self.world.objects.append(self) + + def __repr__(self): + skip_attr = ["links", "joints", "urdf_object", "attachments", "cids", "_current_link_poses", + "_current_link_transforms", "link_to_geometry"] + return self.__class__.__qualname__ + f"(" + ', \n'.join( + [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" + + def remove(self) -> None: + """ + Removes this object from the BulletWorld it currently resides in. + For the object to be removed it has to be detached from all objects it + is currently attached to. After this is done a call to PyBullet is done + to remove this Object from the simulation. + """ + for obj in self.attachments.keys(): + self.world.detach(self, obj) + self.world.objects.remove(self) + # This means the current world of the object is not the shadow world, since it + # has a reference to the shadow world + if self.world.shadow_world is not None: + self.world.world_sync.remove_obj_queue.put(self) + self.world.world_sync.remove_obj_queue.join() + self.world.remove_object(self.id) + if BulletWorld.robot == self: + BulletWorld.robot = None + + def detach_all(self) -> None: + """ + Detach all objects attached to this object. + """ + attachments = self.attachments.copy() + for att in attachments.keys(): + self.world.detach(self, att) + + def get_position(self) -> Pose: + """ + Returns the position of this Object as a list of xyz. + + :return: The current position of this object + """ + return self.get_pose().position + + def get_orientation(self) -> Quaternion: + """ + Returns the orientation of this object as a list of xyzw, representing a quaternion. + + :return: A list of xyzw + """ + return self.get_pose().orientation + + def get_pose(self) -> Pose: + """ + Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. + + :return: The current pose of this object + """ + return self._current_pose + + def set_pose(self, pose: Pose, base: bool = False) -> None: + """ + Sets the Pose of the object. + + :param pose: New Pose for the object + :param base: If True places the object base instead of origin at the specified position and orientation + """ + pose_in_map = self.local_transformer.transform_pose(pose, "map") + position, orientation = pose_in_map.to_list() + if base: + position = np.array(position) + self.base_origin_shift + self.world.reset_object_base_pose(self, position, orientation) + self._current_pose = pose_in_map + self._update_link_poses() + self.world._set_attached_objects(self, [self]) + + @property + def pose(self) -> Pose: + """ + Property that returns the current position of this Object. + + :return: The position as a list of xyz + """ + return self.get_pose() + + @pose.setter + def pose(self, value: Pose) -> None: + """ + Sets the Pose of the Object to the given value. Function for attribute use. + + :param value: New Pose of the Object + """ + self.set_pose(value) + + def move_base_to_origin_pos(self) -> None: + """ + Move the object such that its base will be at the current origin position. + This is useful when placing objects on surfaces where you want the object base in contact with the surface. + """ + self.set_pose(self.get_pose(), base=True) + + def _calculate_transform(self, obj: Object, link: str = None) -> Transform: + """ + Calculates the transformation between another object and the given + link of this object. If no link is provided then the base position will be used. + + :param obj: The other object for which the transformation should be calculated + :param link: The optional link name + :return: The transformation from the link (or base position) to the other objects base position + """ + transform = self.local_transformer.transform_to_object_frame(obj.pose, self, link) + + return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) + + def set_position(self, position: Union[Pose, Point], base=False) -> None: + """ + Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position + instead of the origin in the center of the Object. The given position can either be a Pose, + in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. + + :param position: Target position as xyz. + :param base: If the bottom of the Object should be placed or the origin in the center. + """ + pose = Pose() + if isinstance(position, Pose): + target_position = position.position + pose.frame = position.frame + else: + target_position = position + + pose.pose.position = target_position + pose.pose.orientation = self.get_orientation() + self.set_pose(pose, base=base) + + def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: + """ + Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only + the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. + + :param orientation: Target orientation given as a list of xyzw. + """ + pose = Pose() + if isinstance(orientation, Pose): + target_orientation = orientation.orientation + pose.frame = orientation.frame + else: + target_orientation = orientation + + pose.pose.position = self.get_position() + pose.pose.orientation = target_orientation + self.set_pose(pose) + + def _joint_or_link_name_to_id(self, name_type: str) -> Dict[str, int]: + """ + Creates a dictionary which maps the link or joint name to the unique ids used by pybullet. + + :param name_type: Determines if the dictionary should be for joints or links + :return: A dictionary that maps joint or link names to unique ids + """ + n_joints = self.world.get_object_number_of_joints(self) + joint_name_to_id = {} + for i in range(0, n_joints): + _id = self.world.get_object_joint_id(self, i) + if name_type == "joint": + _name = self.world.get_object_joint_name(self, i) + else: + _name = self.world.get_object_link_name(self, i) + joint_name_to_id[_name] = _id + return joint_name_to_id + + def get_joint_id(self, name: str) -> int: + """ + Returns the unique id for a joint name. As used by PyBullet. + + :param name: The joint name + :return: The unique id + """ + return self.joints[name] + + def get_link_id(self, name: str) -> int: + """ + Returns a unique id for a link name. As used by PyBullet. + + :param name: The link name + :return: The unique id + """ + return self.links[name] + + def get_link_by_id(self, id: int) -> str: + """ + Returns the name of a link for a given unique PyBullet id + + :param id: PyBullet id for link + :return: The link name + """ + return dict(zip(self.links.values(), self.links.keys()))[id] + + def get_joint_by_id(self, joint_id: int) -> str: + """ + Returns the joint name for a unique PyBullet id + + :param joint_id: The Pybullet id of for joint + :return: The joint name + """ + return dict(zip(self.joints.values(), self.joints.keys()))[joint_id] + + def get_link_relative_to_other_link(self, source_frame: str, target_frame: str) -> Pose: + """ + Calculates the position of a link in the coordinate frame of another link. + + :param source_frame: The name of the source frame + :param target_frame: The name of the target frame + :return: The pose of the source frame in the target frame + """ + source_pose = self.get_link_pose(source_frame) + return self.local_transformer.transform_to_object_frame(source_pose, self, target_frame) + + def get_link_position(self, name: str) -> Point: + """ + Returns the position of a link of this Object. Position is returned as a list of xyz. + + :param name: The link name + :return: The link position as xyz + """ + return self.get_link_pose(name).position + + def get_link_orientation(self, name: str) -> Quaternion: + """ + Returns the orientation of a link of this Object. Orientation is returned as a quaternion. + + :param name: The name of the link + :return: The orientation of the link as a quaternion + """ + return self.get_link_pose(name).orientation + + def get_link_pose(self, name: str) -> Pose: + """ + Returns a Pose of the link corresponding to the given name. The returned Pose will be in world coordinate frame. + + :param name: Link name for which a Pose should be returned + :return: The pose of the link + """ + if name in self.links.keys() and self.links[name] == -1: + return self.get_pose() + return self._current_link_poses[name] + # return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6]) + + def set_joint_state(self, joint_name: str, joint_pose: float) -> None: + """ + Sets the state of the given joint to the given joint pose. If the pose is outside the joint limits, as stated + in the URDF, an error will be printed. However, the joint will be set either way. + + :param joint_name: The name of the joint + :param joint_pose: The target pose for this joint + """ + # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly + up_lim, low_lim = self.world.get_object_joint_limits(self, joint_name) + if low_lim > up_lim: + low_lim, up_lim = up_lim, low_lim + if not low_lim <= joint_pose <= up_lim: + logging.error( + f"The joint position has to be within the limits of the joint. The joint limits for {joint_name} are {low_lim} and {up_lim}") + logging.error(f"The given joint position was: {joint_pose}") + # Temporarily disabled because kdl outputs values exciting joint limits + # return + self.world.reset_joint_state(self, joint_name, joint_pose) + self._current_joint_states[joint_name] = joint_pose + # self.local_transformer.update_transforms_for_object(self) + self._update_link_poses() + self.world._set_attached_objects(self, [self]) + + def set_joint_states(self, joint_poses: dict) -> None: + """ + Sets the current state of multiple joints at once, this method should be preferred when setting multiple joints + at once instead of running :func:`~Object.set_joint_state` in a loop. + + :param joint_poses: + :return: + """ + for joint_name, joint_pose in joint_poses.items(): + self.world.reset_joint_state(self, joint_name, joint_pose) + self._current_joint_states[joint_name] = joint_pose + self._update_link_poses() + self.world._set_attached_objects(self, [self]) + + def get_joint_state(self, joint_name: str) -> float: + """ + Returns the joint state for the given joint name. + + :param joint_name: The name of the joint + :return: The current pose of the joint + """ + return self._current_joint_states[joint_name] + + def contact_points(self) -> List: + """ + Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned + list please look at `PyBullet Doc `_ + + :return: A list of all contact points with other objects + """ + return self.world.get_object_contact_points(self) + + def contact_points_simulated(self) -> List: + """ + Returns a list of all contact points between this Object and other Objects after stepping the simulation once. + For a more detailed explanation of the returned + list please look at `PyBullet Doc `_ + + :return: A list of contact points between this Object and other Objects + """ + s = self.world.save_state() + self.world.step() + contact_points = self.contact_points() + self.world.restore_state(*s) + return contact_points + + def update_joints_from_topic(self, topic_name: str) -> None: + """ + Updates the joints of this object with positions obtained from a topic with the message type JointState. + Joint names on the topic have to correspond to the joints of this object otherwise an error message will be logged. + + :param topic_name: Name of the topic with the joint states + """ + msg = rospy.wait_for_message(topic_name, JointState) + joint_names = msg.name + joint_positions = msg.position + if set(joint_names).issubset(self.joints.keys()): + for i in range(len(joint_names)): + self.set_joint_state(joint_names[i], joint_positions[i]) + else: + add_joints = set(joint_names) - set(self.joints.keys()) + rospy.logerr(f"There are joints in the published joint state which are not in this model: /n \ + The following joint{'s' if len(add_joints) != 1 else ''}: {add_joints}") + + def update_pose_from_tf(self, frame: str) -> None: + """ + Updates the pose of this object from a TF message. + + :param frame: Name of the TF frame from which the position should be taken + """ + tf_listener = tf.TransformListener() + time.sleep(0.5) + position, orientation = tf_listener.lookupTransform(frame, "map", rospy.Time(0)) + position = [position[0][0] * -1, + position[0][1] * -1, + position[0][2]] + self.set_position(Pose(position, orientation)) + + def set_color(self, color: List[float], link: Optional[str] = "") -> None: + """ + Changes the color of this object, the color has to be given as a list + of RGBA values. Optionally a link name can can be provided, if no link + name is provided all links of this object will be colored. + + :param color: The color as RGBA values between 0 and 1 + :param link: The link name of the link which should be colored + """ + self.world.set_object_color(self, color, link) + + def get_color(self, link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: + """ + This method returns the color of this object or a link of this object. If no link is given then the + return is either: + + 1. A list with the color as RGBA values, this is the case if the object only has one link (this + happens for example if the object is spawned from a .obj or .stl file) + 2. A dict with the link name as key and the color as value. The color is given as RGBA values. + Please keep in mind that not every link may have a color. This is dependent on the URDF from which the + object is spawned. + + If a link is specified then the return is a list with RGBA values representing the color of this link. + It may be that this link has no color, in this case the return is None as well as an error message. + + :param link: the link name for which the color should be returned. + :return: The color of the object or link, or a dictionary containing every colored link with its color + """ + return self.world.get_object_color(self, link) + + def get_AABB(self, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case + the axis aligned bounding box of the link will be returned. The return of this method are two points in + world coordinate frame which define a bounding box. + + :param link_name: The Optional name of a link of this object. + :return: Two lists of x,y,z which define the bounding box. + """ + return self.world.get_object_AABB(self, link_name) + + def get_base_origin(self, link_name: Optional[str] = None) -> Pose: + """ + Returns the origin of the base/bottom of an object/link + + :param link_name: The link name for which the bottom position should be returned + :return: The position of the bottom of this Object or link + """ + aabb = self.get_AABB(link_name=link_name) + base_width = np.absolute(aabb[0][0] - aabb[1][0]) + base_length = np.absolute(aabb[0][1] - aabb[1][1]) + return Pose([aabb[0][0] + base_width / 2, aabb[0][1] + base_length / 2, aabb[0][2]], + self.get_pose().orientation_as_list()) + + def get_joint_limits(self, joint: str) -> Tuple[float, float]: + """ + Returns the lower and upper limit of a joint, if the lower limit is higher + than the upper they are swapped to ensure the lower limit is always the smaller one. + + :param joint: The name of the joint for which the limits should be found. + :return: The lower and upper limit of the joint. + """ + if joint not in self.joints.keys(): + raise KeyError(f"The given Joint: {joint} is not part of this object") + lower, upper = self.world.get_object_joint_limits(self, joint) + if lower > upper: + lower, upper = upper, lower + return lower, upper + + def get_joint_axis(self, joint_name: str) -> Tuple[float]: + """ + Returns the axis along which a joint is moving. The given joint_name has to be part of this object. + + :param joint_name: Name of the joint for which the axis should be returned. + :return: The axis a vector of xyz + """ + return self.world.get_object_joint_axis(self, joint_name) + + def get_joint_type(self, joint_name: str) -> JointType: + """ + Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. + + :param joint_name: Joint name for which the type should be returned + :return: The type of the joint + """ + return self.world.get_object_joint_type(self, joint_name) + + def find_joint_above(self, link_name: str, joint_type: JointType) -> str: + """ + Traverses the chain from 'link_name' to the URDF origin and returns the first joint that is of type 'joint_type'. + + :param link_name: Link name above which the joint should be found + :param joint_type: Joint type that should be searched for + :return: Name of the first joint which has the given type + """ + chain = self.urdf_object.get_chain(self.urdf_object.get_root(), link_name) + reversed_chain = reversed(chain) + container_joint = None + for element in reversed_chain: + if element in self.joints and self.get_joint_type(element) == joint_type: + container_joint = element + break + if not container_joint: + rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") + return container_joint + + def get_complete_joint_state(self) -> Dict[str: float]: + """ + Returns the complete joint state of the object as a dictionary of joint names and joint values. + + :return: A dictionary with the complete joint state + """ + return self._current_joint_states + + def get_link_tf_frame(self, link_name: str) -> str: + """ + Returns the name of the tf frame for the given link name. This method does not check if the given name is + actually a link of this object. + + :param link_name: Name of a link for which the tf frame should be returned + :return: A TF frame name for a specific link + """ + return self.tf_frame + "/" + link_name + + def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]: + """ + Extracts the geometry information for each collision of each link and links them to the respective link. + + :return: A dictionary with link name as key and geometry information as value + """ + link_to_geometry = {} + for link in self.links.keys(): + link_obj = self.urdf_object.link_map[link] + if not link_obj.collision: + link_to_geometry[link] = None + else: + link_to_geometry[link] = link_obj.collision.geometry + return link_to_geometry + + def _update_link_poses(self) -> None: + """ + Updates the cached poses and transforms for each link of this Object + """ + for link_name in self.links.keys(): + if link_name == self.urdf_object.get_root(): + self._current_link_poses[link_name] = self._current_pose + self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame) + else: + self._current_link_poses[link_name] = Pose(*self.world.get_object_link_pose(self, link_name)) + self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( + self.get_link_tf_frame(link_name)) + + def _init_current_joint_states(self) -> None: + """ + Initialize the cached joint position for each joint. + """ + for joint_name in self.joints.keys(): + self._current_joint_states[joint_name] = self.world.get_object_joint_position(self, joint_name) + + +def filter_contact_points(contact_points, exclude_ids) -> List: + """ + Returns a list of contact points where Objects that are in the 'exclude_ids' list are removed. + + :param contact_points: A list of contact points + :param exclude_ids: A list of unique ids of Objects that should be removed from the list + :return: A list containing 'contact_points' without Objects that are in 'exclude_ids' + """ + return list(filter(lambda cp: cp[2] not in exclude_ids, contact_points)) + + +def get_path_from_data_dir(file_name: str, data_directory: str) -> str: + """ + Returns the full path for a given file name in the given directory. If there is no file with the given filename + this method returns None. + + :param file_name: The filename of the searched file. + :param data_directory: The directory in which to search for the file. + :return: The full path in the filesystem or None if there is no file with the filename in the directory + """ + dir = pathlib.Path(data_directory) + for file in os.listdir(data_directory): + if file == file_name: + return data_directory + f"/{file_name}" + + +def _get_robot_name_from_urdf(urdf_string: str) -> str: + """ + Extracts the robot name from the 'robot_name' tag of a URDF. + + :param urdf_string: The URDF as string. + :return: The name of the robot described by the URDF. + """ + res = re.findall(r"robot\ *name\ *=\ *\"\ *[a-zA-Z_0-9]*\ *\"", urdf_string) + if len(res) == 1: + begin = res[0].find("\"") + end = res[0][begin + 1:].find("\"") + robot = res[0][begin + 1:begin + 1 + end].lower() + return robot + + +def _load_object(name: str, + path: str, + position: List[float], + orientation: List[float], + world: BulletWorld, + color: List[float], + ignoreCachedFiles: bool) -> Tuple[int, str]: + """ + Loads an object to the given BulletWorld with the given position and orientation. The color will only be + used when an .obj or .stl file is given. + If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created + and this URDf file will be loaded instead. + When spawning a URDf file a new file will be created in the cache directory, if there exists none. + This new file will have resolved mesh file paths, meaning there will be no references + to ROS packges instead there will be absolute file paths. + + :param name: The name of the object which should be spawned + :param path: The path to the source file or the name on the ROS parameter server + :param position: The position in which the object should be spawned + :param orientation: The orientation in which the object should be spawned + :param world: The BulletWorld to which the Object should be spawned + :param color: The color of the object, only used when .obj or .stl file is given + :param ignoreCachedFiles: Whether to ignore files in the cache directory. + :return: The unique id of the object and the path to the file used for spawning + """ + pa = pathlib.Path(path) + extension = pa.suffix + world, world_id = _world_and_id(world) + if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): + for dir in world.data_directory: + path = get_path_from_data_dir(path, dir) + if path: break + + if not path: + raise FileNotFoundError( + f"File {pa.name} could not be found in the resource directory {world.data_directory}") + # rospack = rospkg.RosPack() + # cach_dir = rospack.get_path('pycram') + '/resources/cached/' + cach_dir = world.data_directory[0] + '/cached/' + if not pathlib.Path(cach_dir).exists(): + os.mkdir(cach_dir) + + # if file is not yet cached corrcet the urdf and save if in the cache directory + if not _is_cached(path, name, cach_dir) or ignoreCachedFiles: + if extension == ".obj" or extension == ".stl": + path = _generate_urdf_file(name, path, color, cach_dir) + elif extension == ".urdf": + with open(path, mode="r") as f: + urdf_string = fix_missing_inertial(f.read()) + urdf_string = remove_error_tags(urdf_string) + urdf_string = fix_link_attributes(urdf_string) + try: + urdf_string = _correct_urdf_string(urdf_string) + except rospkg.ResourceNotFound as e: + rospy.logerr(f"Could not find resource package linked in this URDF") + raise e + path = cach_dir + pa.name + with open(path, mode="w") as f: + f.write(urdf_string) + else: # Using the urdf from the parameter server + urdf_string = rospy.get_param(path) + path = cach_dir + name + ".urdf" + with open(path, mode="w") as f: + f.write(_correct_urdf_string(urdf_string)) + # save correct path in case the file is already in the cache directory + elif extension == ".obj" or extension == ".stl": + path = cach_dir + pa.stem + ".urdf" + elif extension == ".urdf": + path = cach_dir + pa.name + else: + path = cach_dir + name + ".urdf" + + try: + obj = p.loadURDF(path, basePosition=position, baseOrientation=orientation, physicsClientId=world_id) + return obj, path + except p.error as e: + logging.error( + "The File could not be loaded. Plese note that the path has to be either a URDF, stl or obj file or the name of an URDF string on the parameter server.") + os.remove(path) + raise (e) + # print(f"{bcolors.BOLD}{bcolors.WARNING}The path has to be either a path to a URDf file, stl file, obj file or the name of a URDF on the parameter server.{bcolors.ENDC}") + + +def _is_cached(path: str, name: str, cach_dir: str) -> bool: + """ + Checks if the file in the given path is already cached or if + there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from + the parameter server is used. + + :param path: The path given by the user to the source file. + :param name: The name for this object. + :param cach_dir: The absolute path the cach directory in the pycram package. + :return: True if there already exists a chached file, False in any other case. + """ + file_name = pathlib.Path(path).name + p = pathlib.Path(cach_dir + file_name) + if p.exists(): + return True + # Returns filename without the filetype, e.g. returns "test" for "test.txt" + file_stem = pathlib.Path(path).stem + p = pathlib.Path(cach_dir + file_stem + ".urdf") + if p.exists(): + return True + return False + + +def _correct_urdf_string(urdf_string: str) -> str: + """ + Changes paths for files in the URDF from ROS paths to paths in the file system. Since PyBullet can't deal with ROS + package paths. + + :param urdf_name: The name of the URDf on the parameter server + :return: The URDF string with paths in the filesystem instead of ROS packages + """ + r = rospkg.RosPack() + new_urdf_string = "" + for line in urdf_string.split('\n'): + if "package://" in line: + s = line.split('//') + s1 = s[1].split('/') + path = r.get_path(s1[0]) + line = line.replace("package://" + s1[0], path) + new_urdf_string += line + '\n' + + return fix_missing_inertial(new_urdf_string) + + +def fix_missing_inertial(urdf_string: str) -> str: + """ + Insert inertial tags for every URDF link that has no inertia. + This is used to prevent PyBullet from dumping warnings in the terminal + + :param urdf_string: The URDF description as string + :returns: The new, corrected URDF description as string. + """ + + inertia_tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.Element("inertial")) + inertia_tree.getroot().append(xml.etree.ElementTree.Element("mass", {"value": "0.1"})) + inertia_tree.getroot().append(xml.etree.ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) + inertia_tree.getroot().append(xml.etree.ElementTree.Element("inertia", {"ixx": "0.01", + "ixy": "0", + "ixz": "0", + "iyy": "0.01", + "iyz": "0", + "izz": "0.01"})) + + # create tree from string + tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) + + for link_element in tree.iter("link"): + inertial = [*link_element.iter("inertial")] + if len(inertial) == 0: + link_element.append(inertia_tree.getroot()) + + return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') + + +def remove_error_tags(urdf_string: str) -> str: + """ + Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the + URDF_parser + + :param urdf_string: String of the URDF from which the tags should be removed + :return: The URDF string with the tags removed + """ + tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) + removing_tags = ["gazebo", "transmission"] + for tag_name in removing_tags: + all_tags = tree.findall(tag_name) + for tag in all_tags: + tree.getroot().remove(tag) + + return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') + + +def fix_link_attributes(urdf_string: str) -> str: + """ + Removes the attribute 'type' from links since this is not parsable by the URDF parser. + + :param urdf_string: The string of the URDF from which the attributes should be removed + :return: The URDF string with the attributes removed + """ + tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) + + for link in tree.iter("link"): + if "type" in link.attrib.keys(): + del link.attrib["type"] + + return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') + + +def _generate_urdf_file(name: str, path: str, color: List[float], cach_dir: str) -> str: + """ + Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given color will be + used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with the name + as filename. + + :param name: The name of the object + :param path: The path to the .obj or .stl file + :param color: The color which should be used for the material tag + :param cach_dir The absolute file path to the cach directory in the pycram package + :return: The absolute path of the created file + """ + urdf_template = ' \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + ' + urdf_template = fix_missing_inertial(urdf_template) + rgb = " ".join(list(map(str, color))) + pathlib_obj = pathlib.Path(path) + path = str(pathlib_obj.resolve()) + content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) + with open(cach_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: + file.write(content) + return cach_dir + pathlib_obj.stem + ".urdf" + + +def _world_and_id(world: BulletWorld) -> Tuple[BulletWorld, int]: + """ + Selects the world to be used. If the given world is None the 'current_bullet_world' is used. + + :param world: The world which should be used or None if 'current_bullet_world' should be used + :return: The BulletWorld object and the id of this BulletWorld + """ + world = world if world is not None else BulletWorld.current_bullet_world + id = world.client_id if world is not None else BulletWorld.current_bullet_world.client_id + return world, id From 1953249a15551dd0288caa37959fe72ac9eae2b5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 30 Nov 2023 12:21:34 +0100 Subject: [PATCH 02/69] [RefactoringObjectClass] Now object class is independent of pybullet, and the tests are passing --- demos/pycram_bullet_world_demo/demo.py | 2 +- demos/pycram_ur5_demo/demo.py | 2 +- doc/source/notebooks/bullet_world.ipynb | 559 ++++- doc/source/notebooks/giskard.ipynb | 267 ++- src/pycram/bullet_world.py | 38 +- src/pycram/costmaps.py | 4 +- src/pycram/designator.py | 4 +- src/pycram/designators/action_designator.py | 2 +- src/pycram/designators/location_designator.py | 4 +- src/pycram/designators/motion_designator.py | 2 +- src/pycram/designators/object_designator.py | 2 +- src/pycram/external_interfaces/giskard.py | 2 +- src/pycram/external_interfaces/ik.py | 2 +- src/pycram/external_interfaces/robokudo.py | 2 +- src/pycram/helper.py | 4 +- src/pycram/local_transformer.py | 2 +- src/pycram/pose_generator_and_validator.py | 4 +- .../process_modules/boxy_process_modules.py | 22 +- .../process_modules/donbot_process_modules.py | 16 +- .../process_modules/hsr_process_modules.py | 14 +- .../process_modules/pr2_process_modules.py | 36 +- .../resolver/location/giskard_location.py | 2 +- src/pycram/resolver/location/jpt_location.py | 2 +- src/pycram/ros/force_torque_sensor.py | 2 +- src/pycram/ros/joint_state_publisher.py | 4 +- src/pycram/ros/robot_state_updater.py | 4 +- src/pycram/ros/tf_broadcaster.py | 2 +- src/pycram/ros/viz_marker_publisher.py | 2 +- src/pycram/task.py | 2 +- src/pycram/world.py | 1866 ----------------- ..._world_reasoning.py => world_reasoning.py} | 4 +- test/local_transformer_tests.py | 2 +- test/test_action_designator.py | 8 +- test/test_bullet_world.py | 2 +- test/test_bullet_world_reasoning.py | 2 +- test/test_database_resolver.py | 2 +- test/test_jpt_resolver.py | 2 +- test/test_location_designator.py | 4 +- 38 files changed, 929 insertions(+), 1973 deletions(-) mode change 120000 => 100644 doc/source/notebooks/bullet_world.ipynb mode change 120000 => 100644 doc/source/notebooks/giskard.ipynb delete mode 100644 src/pycram/world.py rename src/pycram/{bullet_world_reasoning.py => world_reasoning.py} (99%) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index c19ad349c..0216c5474 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -2,7 +2,7 @@ from pycram.designators.location_designator import * from pycram.designators.object_designator import * from pycram.pose import Pose -from pycram.bullet_world import BulletWorld, Object +from pycram.world import BulletWorld, Object from pycram.process_module import simulated_robot, with_simulated_robot from pycram.enums import ObjectType diff --git a/demos/pycram_ur5_demo/demo.py b/demos/pycram_ur5_demo/demo.py index 2c6d12a08..2fc87dc42 100644 --- a/demos/pycram_ur5_demo/demo.py +++ b/demos/pycram_ur5_demo/demo.py @@ -5,7 +5,7 @@ import pybullet as pb from pycram import robot_description -from pycram.bullet_world import BulletWorld, Object +from pycram.world import BulletWorld, Object from pycram.pose import Pose from pycram.ros.force_torque_sensor import ForceTorqueSensor from pycram.ros.joint_state_publisher import JointStatePublisher diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb deleted file mode 120000 index bdb627d54..000000000 --- a/doc/source/notebooks/bullet_world.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/bullet_world.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb new file mode 100644 index 000000000..cf736fd77 --- /dev/null +++ b/doc/source/notebooks/bullet_world.ipynb @@ -0,0 +1,558 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "8d2d6a94", + "metadata": {}, + "source": [ + "# Bullet World\n", + "This Notebook will show you the basics of working with the PyCRAM BulletWorld.\n", + "\n", + "First we need to import and create a BulletWorld." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "23bbba35", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:27:48.492859720Z", + "start_time": "2023-11-30T09:27:47.729848173Z" + } + }, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "pybullet build time: Nov 28 2023 23:51:11\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "/home/bassioun/cram_ws/src/pycram/src/pycram/orm/action_designator.py:10: SAWarning: Implicitly combining column Designator.dtype with column Action.dtype under attribute 'dtype'. Please configure one or more attributes for these same-named columns explicitly.\n", + " class Action(MapperArgsMixin, Designator):\n", + "/home/bassioun/cram_ws/src/pycram/src/pycram/orm/motion_designator.py:16: SAWarning: Implicitly combining column Designator.dtype with column Motion.dtype under attribute 'dtype'. Please configure one or more attributes for these same-named columns explicitly.\n", + " class Motion(MapperArgsMixin, Designator):\n", + "[WARN] [1701336468.169695]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "[WARN] [1701336468.173591]: Failed to import Giskard messages\n" + ] + } + ], + "source": [ + "from pycram.world import BulletWorld\n", + "from pycram.pose import Pose\n", + "from pycram.enums import ObjectType\n", + "\n", + "world = BulletWorld()" + ] + }, + { + "cell_type": "markdown", + "id": "dccaf6ff", + "metadata": {}, + "source": [ + "This new window is the BulletWorld, PyCRAMs internal physics simulation. You can use the mouse to move the camera around:\n", + "\n", + " * Press the left mouse button to rotate the camera\n", + " * Press the right mouse button to move the camera \n", + " * Press the middle mouse button (scroll wheel) and move the mouse up or down to zoom\n", + " \n", + "At the moment the BulletWorld only contains a floor, this is spawned by default when creating the BulletWorld. Furthermore, the gravity is set to 9.8 m^2, which is the same gravitation as the one on earth. \n", + " " + ] + }, + { + "cell_type": "markdown", + "id": "4403bf9c", + "metadata": {}, + "source": [ + "To close the BulletWorld again please use the ```exit``` method since it will also terminate threads running in the background" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "b1e6ed82", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T08:32:14.940359764Z", + "start_time": "2023-11-30T08:32:14.691411659Z" + } + }, + "outputs": [], + "source": [ + "world.exit()" + ] + }, + { + "cell_type": "markdown", + "id": "e0fc7086", + "metadata": {}, + "source": [ + "To spawn new things in the BulletWorld we need to import the Object class and create and instance of it. " + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "48a3aed2", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:27:52.209614939Z", + "start_time": "2023-11-30T09:27:52.159974972Z" + } + }, + "outputs": [], + "source": [ + "from pycram.world import Object\n", + "\n", + "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([0, 0, 1]))" + ] + }, + { + "cell_type": "markdown", + "id": "8a754f12", + "metadata": {}, + "source": [ + "As you can see this spawns a milk floating in the air. What we did here was create a new Object which has the name \"milk\" as well as the type ```ObjectType.MILK ``` , is spawned from the file \"milk.stl\" and is at the position [0, 0, 1]. \n", + "\n", + "The type of an Object can either be from the enum ObjectType or a string. However, it is recommended to use the enum since this would make for a more consistent naming of types which makes it easiert to work with types. But since the types of the enum might not fit your case you can also use strings. \n", + "\n", + "The first three of these parameters are required while the position is optional. As you can see it was sufficent to only specify the filename for PyCRAM to spawn the milk mesh. When only providing a filename PyCRAM will search in its resource directory for a matching file and use it. \n", + "\n", + "For a complete list of all parameters that can be used to crate an Object please check the documentation. \n", + "\n", + "\n", + "\n", + "Since the Object is spawned we can now interact with it. First we want to move it around and change it's orientation" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "4ae2bc42", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:27:57.498773893Z", + "start_time": "2023-11-30T09:27:57.497252820Z" + } + }, + "outputs": [], + "source": [ + "milk.set_position(Pose([1, 1, 1]))" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "4adc7b11", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:27:57.867485393Z", + "start_time": "2023-11-30T09:27:57.865626321Z" + } + }, + "outputs": [], + "source": [ + "milk.set_orientation(Pose(orientation=[1,0, 0, 1]))" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "f91d1809", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:27:58.203719277Z", + "start_time": "2023-11-30T09:27:58.201735028Z" + } + }, + "outputs": [], + "source": [ + "milk.set_pose(Pose([0, 0, 1], [0, 0, 0, 1]))" + ] + }, + { + "cell_type": "markdown", + "id": "5805be38", + "metadata": {}, + "source": [ + "In the same sense as setting the position or orientation you can also get the position and orientation." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "1db2aa78", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:27:59.859274393Z", + "start_time": "2023-11-30T09:27:59.856354972Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Position: \n", + "x: 0.0\n", + "y: 0.0\n", + "z: 1.0\n", + "Orientation: \n", + "x: 0.0\n", + "y: 0.0\n", + "z: 0.0\n", + "w: 1.0\n", + "Pose: \n", + "header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1701336478\n", + " nsecs: 200602293\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 1.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0\n" + ] + } + ], + "source": [ + "print(f\"Position: \\n{milk.get_position()}\")\n", + "\n", + "print(f\"Orientation: \\n{milk.get_orientation()}\")\n", + "\n", + "print(f\"Pose: \\n{milk.get_pose()}\")" + ] + }, + { + "cell_type": "markdown", + "id": "68c02db9", + "metadata": {}, + "source": [ + "## Attachments\n", + "You can attach Objects to each other simply by calling the attach method on one of them and providing the other as parameter. Since attachments are bi-directional it doesn't matter on which Object you call the method. \n", + "\n", + "First we neeed another Object" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "10a493b1", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:28:02.809496385Z", + "start_time": "2023-11-30T09:28:02.808590814Z" + } + }, + "outputs": [], + "source": [ + "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1, 0, 1]))" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "275372e3", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:28:03.828259221Z", + "start_time": "2023-11-30T09:28:03.825819607Z" + } + }, + "outputs": [], + "source": [ + "milk.attach(cereal)" + ] + }, + { + "cell_type": "markdown", + "id": "5577c567", + "metadata": {}, + "source": [ + "Now since they are attached to each other, if we move one of them the other will move in conjunction." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "id": "dff85834", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:28:05.435992616Z", + "start_time": "2023-11-30T09:28:05.433266300Z" + } + }, + "outputs": [], + "source": [ + "milk.set_position(Pose([1,1,1]))" + ] + }, + { + "cell_type": "markdown", + "id": "196bd705", + "metadata": {}, + "source": [ + "In the same way the Object can also be detached, just call the detach method on one of the two attached Objects." + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "b0bba145", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:28:06.817969292Z", + "start_time": "2023-11-30T09:28:06.815384873Z" + } + }, + "outputs": [], + "source": [ + "cereal.detach(milk)" + ] + }, + { + "cell_type": "markdown", + "id": "e5e44a67", + "metadata": {}, + "source": [ + "## Links and Joints\n", + "Objects spawned from mesh files do not have links or joints, but if you spawn things from a URDF like a robot they will have a lot of links and joints. Every Object has two dictionaries as attributes namley ```links``` and ```joints``` which contain every link or joint as key and a unique id, used by PyBullet, as value. \n", + "\n", + "We will see this at the example of the PR2" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "id": "edf5cb72", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:28:13.754327852Z", + "start_time": "2023-11-30T09:28:10.551209150Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "{'base_link': 0, 'base_bellow_link': 1, 'base_laser_link': 2, 'fl_caster_rotation_link': 3, 'fl_caster_l_wheel_link': 4, 'fl_caster_r_wheel_link': 5, 'fr_caster_rotation_link': 6, 'fr_caster_l_wheel_link': 7, 'fr_caster_r_wheel_link': 8, 'bl_caster_rotation_link': 9, 'bl_caster_l_wheel_link': 10, 'bl_caster_r_wheel_link': 11, 'br_caster_rotation_link': 12, 'br_caster_l_wheel_link': 13, 'br_caster_r_wheel_link': 14, 'torso_lift_link': 15, 'l_torso_lift_side_plate_link': 16, 'r_torso_lift_side_plate_link': 17, 'imu_link': 18, 'head_pan_link': 19, 'head_tilt_link': 20, 'head_plate_frame': 21, 'sensor_mount_link': 22, 'high_def_frame': 23, 'high_def_optical_frame': 24, 'double_stereo_link': 25, 'wide_stereo_link': 26, 'wide_stereo_optical_frame': 27, 'wide_stereo_l_stereo_camera_frame': 28, 'wide_stereo_l_stereo_camera_optical_frame': 29, 'wide_stereo_r_stereo_camera_frame': 30, 'wide_stereo_r_stereo_camera_optical_frame': 31, 'narrow_stereo_link': 32, 'narrow_stereo_optical_frame': 33, 'narrow_stereo_l_stereo_camera_frame': 34, 'narrow_stereo_l_stereo_camera_optical_frame': 35, 'narrow_stereo_r_stereo_camera_frame': 36, 'narrow_stereo_r_stereo_camera_optical_frame': 37, 'projector_wg6802418_frame': 38, 'projector_wg6802418_child_frame': 39, 'laser_tilt_mount_link': 40, 'laser_tilt_link': 41, 'r_shoulder_pan_link': 42, 'r_shoulder_lift_link': 43, 'r_upper_arm_roll_link': 44, 'r_upper_arm_link': 45, 'r_elbow_flex_link': 46, 'r_forearm_roll_link': 47, 'r_forearm_link': 48, 'r_wrist_flex_link': 49, 'r_wrist_roll_link': 50, 'r_gripper_palm_link': 51, 'r_gripper_led_frame': 52, 'r_gripper_motor_accelerometer_link': 53, 'r_gripper_tool_frame': 54, 'r_gripper_motor_slider_link': 55, 'r_gripper_motor_screw_link': 56, 'r_gripper_l_finger_link': 57, 'r_gripper_l_finger_tip_link': 58, 'r_gripper_r_finger_link': 59, 'r_gripper_r_finger_tip_link': 60, 'r_gripper_l_finger_tip_frame': 61, 'r_forearm_cam_frame': 62, 'r_forearm_cam_optical_frame': 63, 'l_shoulder_pan_link': 64, 'l_shoulder_lift_link': 65, 'l_upper_arm_roll_link': 66, 'l_upper_arm_link': 67, 'l_elbow_flex_link': 68, 'l_forearm_roll_link': 69, 'l_forearm_link': 70, 'l_wrist_flex_link': 71, 'l_wrist_roll_link': 72, 'l_gripper_palm_link': 73, 'l_gripper_led_frame': 74, 'l_gripper_motor_accelerometer_link': 75, 'l_gripper_tool_frame': 76, 'l_gripper_motor_slider_link': 77, 'l_gripper_motor_screw_link': 78, 'l_gripper_l_finger_link': 79, 'l_gripper_l_finger_tip_link': 80, 'l_gripper_r_finger_link': 81, 'l_gripper_r_finger_tip_link': 82, 'l_gripper_l_finger_tip_frame': 83, 'l_forearm_cam_frame': 84, 'l_forearm_cam_optical_frame': 85, 'torso_lift_motor_screw_link': 86, 'base_footprint': -1}\n" + ] + } + ], + "source": [ + "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", + "print(pr2.links)" + ] + }, + { + "cell_type": "markdown", + "id": "e157eb6f", + "metadata": {}, + "source": [ + "For links there are similar methods available as for the pose. However, you can only **get** the position and orientation of a link. " + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "id": "51b59ffd", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:28:17.138722982Z", + "start_time": "2023-11-30T09:28:17.136550048Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Position: \n", + "x: -0.05000000447034836\n", + "y: 0.0\n", + "z: 0.7906750440597534\n", + "Orientation: \n", + "x: 0.0\n", + "y: 0.0\n", + "z: 0.0\n", + "w: 1.0\n", + "Pose: \n", + "header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1701336493\n", + " nsecs: 695036649\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.05000000447034836\n", + " y: 0.0\n", + " z: 0.7906750440597534\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0\n" + ] + } + ], + "source": [ + "print(f\"Position: \\n{pr2.get_link_position('torso_lift_link')}\")\n", + "\n", + "print(f\"Orientation: \\n{pr2.get_link_orientation('torso_lift_link')}\")\n", + "\n", + "print(f\"Pose: \\n{pr2.get_link_pose('torso_lift_link')}\")" + ] + }, + { + "cell_type": "markdown", + "id": "ecd9c4a3", + "metadata": {}, + "source": [ + "Methods available for joints are:\n", + "\n", + " * ```get_joint_state```\n", + " * ```set_joint_state```\n", + " * ```get_joint_limits```\n", + " \n", + "We will see how these methods work at the example of the torso_lift_joint" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "id": "38a8aef4", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:28:19.963901990Z", + "start_time": "2023-11-30T09:28:19.914673646Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Joint limits: (0.0, 0.33)\n", + "Current Joint state: 0.0\n", + "New Joint state: 0.2\n" + ] + } + ], + "source": [ + "print(f\"Joint limits: {pr2.get_joint_limits('torso_lift_joint')}\")\n", + "\n", + "print(f\"Current Joint state: {pr2.get_joint_state('torso_lift_joint')}\")\n", + "\n", + "pr2.set_joint_state(\"torso_lift_joint\", 0.2)\n", + "\n", + "print(f\"New Joint state: {pr2.get_joint_state('torso_lift_joint')}\")" + ] + }, + { + "cell_type": "markdown", + "id": "6d4ee70a", + "metadata": {}, + "source": [ + "## Misc Methods\n", + "There are a few methods that don't fit any category but could be helpful anyways. The first two are ```get_color``` and ```set_color```, as the name implies they can be used to get or set the color for specific links or the whole Object. " + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "id": "5eee9b8a", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-30T09:28:22.289708761Z", + "start_time": "2023-11-30T09:28:22.287988559Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Pr2 forearm color: (0.699999988079071, 0.699999988079071, 0.699999988079071, 1.0)\n" + ] + } + ], + "source": [ + "print(f\"Pr2 forearm color: {pr2.get_color('r_forearm_link')}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "id": "07aa1c3f", + "metadata": {}, + "outputs": [], + "source": [ + "pr2.set_color([1, 0, 0], \"r_forearm_link\")" + ] + }, + { + "cell_type": "markdown", + "id": "ce5d910b", + "metadata": {}, + "source": [ + "Lastly, there is ```get_AABB``` AABB stands for axis aligned bounding box. This method returns two points in world coordinates which span a rectangle representing the AABB." + ] + }, + { + "cell_type": "code", + "execution_count": 19, + "id": "2a78715b", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "((-0.0015000000000000005, -0.0015000000000000005, 0.06949999999999999),\n", + " (0.0015000000000000005, 0.0015000000000000005, 0.0725))" + ] + }, + "execution_count": 19, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "pr2.get_AABB()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/doc/source/notebooks/giskard.ipynb b/doc/source/notebooks/giskard.ipynb deleted file mode 120000 index 6d8f4dcef..000000000 --- a/doc/source/notebooks/giskard.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/interface_examples/giskard.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/giskard.ipynb b/doc/source/notebooks/giskard.ipynb new file mode 100644 index 000000000..747e789cc --- /dev/null +++ b/doc/source/notebooks/giskard.ipynb @@ -0,0 +1,266 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "6577676e", + "metadata": {}, + "source": [ + "# Giskard interface in PyCRAM\n", + "This notebook should provide you with an example on how to use the Giskard interface. This includes how to use the interface, how it actually works and how to extend it. \n", + "\n", + "We start installing and starting Giskard. For the installation please follow the instructions [here](https://github.com/SemRoCo/giskardpy).\n", + "After you finish the installation you should be able to start giskard by calling \n", + "```\n", + "roslaunch giskardpy giskardpy_pr2_standalone.launch\n", + "```\n", + "\n", + "In this way you can start Giskard for any robot that is supported.\n", + "```\n", + "roslaunch giskardpy giskardpy_hsr_standalone.launch\n", + "```\n", + "\"Standalone\" means that Giskard only uses a simulated robot and does not look for a real robot. If you want to use Giskard with a real robot you have to switch \"standalone\" with \"iai\". \n", + "```\n", + "roslaunch giskardpy giskardpy_hsr_iai.launch\n", + "```\n", + "\n", + "To see what Giskard is doing you can start RViz, there should already be a MarkerArray when starting otherwise you have to add this manually." + ] + }, + { + "cell_type": "markdown", + "id": "a86d4f3b", + "metadata": {}, + "source": [ + "## How to use the Giskard interface \n", + "Everything related to the Giskard interface is located in ```pycram.external_interfaces.giskard```. \n", + "The content of the file can be roughtly divided into three parts:\n", + " 1. Methods to manage the beliefe states between PyCRAM and Giskard\n", + " 2. Motion goals that should be send to Giskard for execution \n", + " 3. Helper methods to construct ROS messages\n", + " \n", + "The most useful methods are the ones for sending and executing Motion goals. These are the ones we will mostly look at.\n", + "\n", + "We will now start by setting up PyCRAM and then try to send some simple motion goals." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "50cd3e8d", + "metadata": { + "ExecuteTime": { + "end_time": "2023-11-29T10:35:04.790103741Z", + "start_time": "2023-11-29T10:35:03.914188198Z" + } + }, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "pybullet build time: Nov 28 2023 23:51:11\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "/home/bassioun/cram_ws/src/pycram/src/pycram/orm/action_designator.py:10: SAWarning: Implicitly combining column Designator.dtype with column Action.dtype under attribute 'dtype'. Please configure one or more attributes for these same-named columns explicitly.\n", + " class Action(MapperArgsMixin, Designator):\n", + "/home/bassioun/cram_ws/src/pycram/src/pycram/orm/motion_designator.py:16: SAWarning: Implicitly combining column Designator.dtype with column Motion.dtype under attribute 'dtype'. Please configure one or more attributes for these same-named columns explicitly.\n", + " class Motion(MapperArgsMixin, Designator):\n", + "[WARN] [1701254104.411256]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "[WARN] [1701254104.415521]: Failed to import Giskard messages\n" + ] + } + ], + "source": [ + "from pycram.bullet_world import BulletWorld, Object\n", + "from pycram.enums import ObjectType\n", + "\n", + "world = BulletWorld(\"DIRECT\")\n", + "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")" + ] + }, + { + "cell_type": "markdown", + "id": "e4166109", + "metadata": {}, + "source": [ + "When you are working on the real robot you also need to initialize the RobotStateUpdater, this module updates the robot in the BulletWorld with the pose and joint state of the real robot. \n", + "\n", + "You might need to change to topic names to fit the topic names as published by your robot. " + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "outputs": [], + "source": [ + "from pycram.ros.viz_marker_publisher import VizMarkerPublisher\n", + "vis = VizMarkerPublisher()" + ], + "metadata": { + "collapsed": false, + "ExecuteTime": { + "end_time": "2023-11-29T10:36:14.233784427Z", + "start_time": "2023-11-29T10:36:14.188401451Z" + } + }, + "id": "5a3cb3f6c8d2d5b" + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a3eff6ba", + "metadata": {}, + "outputs": [], + "source": [ + "from pycram.ros.robot_state_updater import RobotStateUpdater\n", + "\n", + "r = RobotStateUpdater(\"/tf\", \"/joint_states\")" + ] + }, + { + "cell_type": "markdown", + "id": "f4c4f85d", + "metadata": {}, + "source": [ + "Now we have a PyCRAM belief state set up, belief state in this case just refeers to the BulletWorld since the BulletWorld represents what we belief the world to look like. \n", + "\n", + "The next step will be to send a simple motion goal. The motion goal we will be sending is moving the torso up. For this we just need to move one joint so we use the ```achive_joint_goal```. This method takes a dictionary with the joints that should be moved and the target value for the joint. \n", + "\n", + "Look at Rviz to see the robot move, since we call Giskard for movement the robot in the BulletWorld will not move." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "79fb8a5a", + "metadata": { + "scrolled": true + }, + "outputs": [], + "source": [ + "from pycram.external_interfaces import giskard\n", + "\n", + "giskard.achieve_joint_goal({\"torso_lift_joint\": 0.28})" + ] + }, + { + "cell_type": "markdown", + "id": "77dbded9", + "metadata": {}, + "source": [ + "For Giskard everything is connected by joints (this is called [World Tree](https://github.com/SemRoCo/giskardpy/wiki/World-Tree) by Giskard) therefore we can move the robot by using a motion goal between the map origin and the robot base. \n", + "\n", + "In the example below we use a cartesian goal, meaning we give Giskard a goal pose, a root link and a tip link and Giskard tries to move all joints between root link and tip link such that the tip link is at the goal pose.\n", + "\n", + "This sort of movement is fine for short distance but keep in mind that Giskard has no collision avoidance for longer journeys. So using MoveBase for longer distances is a better idea." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ec79b6b5", + "metadata": {}, + "outputs": [], + "source": [ + "from pycram.external_interfaces import giskard\n", + "from pycram.pose import Pose\n", + "\n", + "giskard.achieve_cartesian_goal(Pose([1, 0, 0]), \"base_link\", \"map\")" + ] + }, + { + "cell_type": "markdown", + "id": "98af5723", + "metadata": {}, + "source": [ + "Now for the last example: we will move the gripper using full body motion controll. \n", + "\n", + "We will again use the cartesian goal, but now between \"map\" and \"r_gripper_tool_frame\". This will not only move the robot (because the chain between \"map\" and \"base_link\" as used in the previous example is also part of this chain) but also move the arm of the robot such that it reaches the goal pose." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a255212e", + "metadata": {}, + "outputs": [], + "source": [ + "from pycram.external_interfaces import giskard\n", + "from pycram.pose import Pose\n", + "\n", + "giskard.achieve_cartesian_goal(Pose([1, 0.5, 0.7]), \"r_gripper_tool_frame\", \"map\")" + ] + }, + { + "cell_type": "markdown", + "id": "7dfe78ba", + "metadata": {}, + "source": [ + "That conludes this example you can now close the BulletWorld by using the \"exit\" method." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "197aa1f0", + "metadata": {}, + "outputs": [], + "source": [ + "world.exit()" + ] + }, + { + "cell_type": "markdown", + "id": "2ae027ac", + "metadata": {}, + "source": [ + "## How the Giskard interface works \n", + "The PyCRAM interface to Giskard mostly relies on the Python interface that Giskard already proviedes ([tutorial](https://github.com/SemRoCo/giskardpy/wiki/Python-Interface) and the [source code](https://github.com/SemRoCo/giskardpy/blob/master/src/giskardpy/python_interface.py)). This inteface provides methods to achive motion goals and load things into the Giskard believe state. \n", + "\n", + "What PyCRAM with this does is: Synchronize the believe state of Giskard with the one of PyCRAM by loading the environment URDF in Giskard, this is done before any motion goal is send. Furthermore, the motion goals are wrapped in methods that use PyCRAM data types. \n", + "\n", + "You can also set collisions between different groups of links. By default Giskard avoids all collisions but for things like grasping an object you want to allow collisions of the gripper. The interface also the following colliion modes:\n", + " * avoid_all_collisions\n", + " * allow_self_collision\n", + " * allow_gripper_collision\n", + "The collision mode can be set by calling the respective method, after calling the method the collision mode is valid for the next motion goal afterwards it default back to avoid_all_collisions.\n", + "\n", + "There is a ```init_giskard_interface``` method which can be used as a decorator. This decorator should be used on all methods that access the giskard_wrapper, since it assures that the interface is working and checks if Giskard died or the imports for the giskard_msgs failed. " + ] + }, + { + "cell_type": "markdown", + "id": "6908a9ab", + "metadata": {}, + "source": [ + "## Extend the Giskard interface\n", + "At the moment the PyCRAM Giskard interface is mostly a wrapper around the Python interface provided by Giskard. If you want to extend the interface there are two ways:\n", + " * Wrap more motion goals which are provided by the Python interface \n", + " * Design new Higher-Level motion goals by combining the motion goals already provided" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 23c40d591..19671d8c3 100644 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -261,7 +261,7 @@ def copy(self) -> BulletWorld: o = Object(obj.name, obj.type, obj.path, obj.get_position(), obj.get_orientation(), world, obj.color) for joint in obj.joints: - o.set_joint_state(joint, obj.get_joint_state(joint)) + o.set_joint_position(joint, obj.get_joint_position(joint)) return world def add_vis_axis(self, pose: Pose, @@ -374,7 +374,7 @@ def reset_bullet_world(self) -> None: obj.detach(att_obj) joint_names = list(obj.joints.keys()) joint_poses = [0 for j in joint_names] - obj.set_joint_states(dict(zip(joint_names, joint_poses))) + obj.set_joint_positions(dict(zip(joint_names, joint_poses))) obj.set_pose(obj.original_pose) @@ -472,8 +472,8 @@ def run(self): # Manage joint positions if len(bulletworld_obj.joints) > 2: for joint_name in bulletworld_obj.joints.keys(): - if shadow_obj.get_joint_state(joint_name) != bulletworld_obj.get_joint_state(joint_name): - shadow_obj.set_joint_states(bulletworld_obj.get_complete_joint_state()) + if shadow_obj.get_joint_position(joint_name) != bulletworld_obj.get_joint_position(joint_name): + shadow_obj.set_joint_positions(bulletworld_obj.get_positions_of_all_joints()) break self.check_for_pause() @@ -770,8 +770,8 @@ def __init__(self, name: str, type: Union[str, ObjectType], path: str, self._current_pose = pose_in_map self._current_link_poses = {} self._current_link_transforms = {} - self._current_joint_states = {} - self._init_current_joint_states() + self._current_joints_positions = {} + self._init_current_positions_of_joints() self._update_link_poses() self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) @@ -1104,7 +1104,7 @@ def get_link_pose(self, name: str) -> Pose: return self._current_link_poses[name] # return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6]) - def set_joint_state(self, joint_name: str, joint_pose: float) -> None: + def set_joint_position(self, joint_name: str, joint_pose: float) -> None: """ Sets the state of the given joint to the given joint pose. If the pose is outside the joint limits, as stated in the URDF, an error will be printed. However, the joint will be set either way. @@ -1124,12 +1124,12 @@ def set_joint_state(self, joint_name: str, joint_pose: float) -> None: # Temporarily disabled because kdl outputs values exciting joint limits # return p.resetJointState(self.id, self.joints[joint_name], joint_pose, physicsClientId=self.world.client_id) - self._current_joint_states[joint_name] = joint_pose + self._current_joints_positions[joint_name] = joint_pose # self.local_transformer.update_transforms_for_object(self) self._update_link_poses() self._set_attached_objects([self]) - def set_joint_states(self, joint_poses: dict) -> None: + def set_joint_positions(self, joint_poses: dict) -> None: """ Sets the current state of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_state` in a loop. @@ -1139,18 +1139,18 @@ def set_joint_states(self, joint_poses: dict) -> None: """ for joint_name, joint_pose in joint_poses.items(): p.resetJointState(self.id, self.joints[joint_name], joint_pose, physicsClientId=self.world.client_id) - self._current_joint_states[joint_name] = joint_pose + self._current_joints_positions[joint_name] = joint_pose self._update_link_poses() self._set_attached_objects([self]) - def get_joint_state(self, joint_name: str) -> float: + def get_joint_position(self, joint_name: str) -> float: """ Returns the joint state for the given joint name. :param joint_name: The name of the joint :return: The current pose of the joint """ - return self._current_joint_states[joint_name] + return self._current_joints_positions[joint_name] def contact_points(self) -> List: """ @@ -1187,7 +1187,7 @@ def update_joints_from_topic(self, topic_name: str) -> None: joint_positions = msg.position if set(joint_names).issubset(self.joints.keys()): for i in range(len(joint_names)): - self.set_joint_state(joint_names[i], joint_positions[i]) + self.set_joint_position(joint_names[i], joint_positions[i]) else: add_joints = set(joint_names) - set(self.joints.keys()) rospy.logerr(f"There are joints in the published joint state which are not in this model: /n \ @@ -1345,13 +1345,13 @@ def find_joint_above(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_complete_joint_state(self) -> Dict[str: float]: + def get_positions_of_all_joints(self) -> Dict[str: float]: """ - Returns the complete joint state of the object as a dictionary of joint names and joint values. + Returns the positions of all joints of the object as a dictionary of joint names and joint positions. - :return: A dictionary with the complete joint state + :return: A dictionary with all joints positions'. """ - return self._current_joint_states + return self._current_joints_positions def get_link_tf_frame(self, link_name: str) -> str: """ @@ -1392,12 +1392,12 @@ def _update_link_poses(self) -> None: self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( self.get_link_tf_frame(link_name)) - def _init_current_joint_states(self) -> None: + def _init_current_positions_of_joints(self) -> None: """ Initialize the cached joint position for each joint. """ for joint_name in self.joints.keys(): - self._current_joint_states[joint_name] = \ + self._current_joints_positions[joint_name] = \ p.getJointState(self.id, self.joints[joint_name], physicsClientId=self.world.client_id)[0] diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 5aee3d24a..67d4c6fc9 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -8,8 +8,8 @@ from matplotlib import colors import psutil import time -from .bullet_world import BulletWorld, Use_shadow_world, Object -from .bullet_world_reasoning import _get_images_for_target +from .world import BulletWorld, Use_shadow_world, Object +from .world_reasoning import _get_images_for_target from nav_msgs.msg import OccupancyGrid, MapMetaData from typing import Tuple, List, Union, Optional diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 20f1beb48..38ce79ab7 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -9,7 +9,7 @@ from sqlalchemy.orm.session import Session import rospy -from .bullet_world import (Object as BulletWorldObject, BulletWorld) +from .world import (Object as BulletWorldObject, BulletWorld) from .helper import GeneratorList, bcolors from threading import Lock from time import time @@ -486,7 +486,7 @@ class Action: def __post_init__(self): self.robot_position = BulletWorld.robot.get_pose() - self.robot_torso_height = BulletWorld.robot.get_joint_state(robot_description.torso_joint) + self.robot_torso_height = BulletWorld.robot.get_joint_position(robot_description.torso_joint) self.robot_type = BulletWorld.robot.type @with_tree diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index ddc9b24ab..354aed83e 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -23,7 +23,7 @@ from ..task import with_tree from ..enums import Arms from ..designator import ActionDesignatorDescription -from ..bullet_world import BulletWorld +from ..world import BulletWorld from ..pose import Pose from ..helper import multiply_quaternions diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 1edb9d17d..7b62fb27c 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -3,8 +3,8 @@ from typing import List, Tuple, Union, Iterable, Optional, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart -from ..bullet_world import Object, BulletWorld, Use_shadow_world -from ..bullet_world_reasoning import link_pose_for_joint_config +from ..world import Object, BulletWorld, Use_shadow_world +from ..world_reasoning import link_pose_for_joint_config from ..designator import Designator, DesignatorError, LocationDesignatorDescription from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap from ..robot_descriptions import robot_description diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index c6535a3a6..8d618ed20 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -2,7 +2,7 @@ from sqlalchemy.orm import Session from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject -from ..bullet_world import Object, BulletWorld +from ..world import Object, BulletWorld from ..designator import DesignatorError from ..plan_failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 02a58ef41..0b063ac9f 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,7 +1,7 @@ import dataclasses from typing import List, Union, Optional, Callable, Tuple, Iterable import sqlalchemy.orm -from ..bullet_world import BulletWorld, Object as BulletWorldObject +from ..world import BulletWorld, Object as BulletWorldObject from ..designator import DesignatorDescription, ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 3742d1d16..98f15d55a 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -6,7 +6,7 @@ from ..pose import Pose from ..robot_descriptions import robot_description -from ..bullet_world import BulletWorld, Object +from ..world import BulletWorld, Object from ..robot_description import ManipulatorDescription from typing import List, Tuple, Dict, Callable diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index d84e9a027..a42acd13a 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -7,7 +7,7 @@ from moveit_msgs.srv import GetPositionIK from sensor_msgs.msg import JointState -from ..bullet_world import Object +from ..world import Object from ..helper import calculate_wrist_tool_offset from ..local_transformer import LocalTransformer from ..pose import Pose, Transform diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index e332c3e07..3a41c2074 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -9,7 +9,7 @@ from ..designator import ObjectDesignatorDescription from ..pose import Pose from ..local_transformer import LocalTransformer -from ..bullet_world import BulletWorld +from ..world import BulletWorld from ..enums import ObjectType try: diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 64acc9cad..aad79a9d1 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -16,7 +16,7 @@ from pytransform3d.transformations import transform_from_pq, transform_from, pq_from_transform from macropy.core.quotes import ast_literal, q -from .bullet_world import Object as BulletWorldObject +from .world import Object as BulletWorldObject from .local_transformer import LocalTransformer from .pose import Transform, Pose from .robot_descriptions import robot_description @@ -54,7 +54,7 @@ def _apply_ik(robot: BulletWorldObject, joint_poses: List[float], joints: List[s # arm ="left" if gripper == robot_description.get_tool_frame("left") else "right" # ik_joints = [robot_description.torso_joint] + robot_description._safely_access_chains(arm).joints # ik_joints = robot_description._safely_access_chains(arm).joints - robot.set_joint_states(dict(zip(joints, joint_poses))) + robot.set_positions_of_all_joints(dict(zip(joints, joint_poses))) # for i in range(0, len(joints)): # robot.set_joint_state(joints[i], joint_poses[i]) diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 5ef4d5c85..c68945182 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -53,7 +53,7 @@ def __init__(self): self.tf_stampeds: List[TransformStamped] = [] self.static_tf_stampeds: List[TransformStamped] = [] - # Since this file can't import bullet_world.py this holds the reference to the current_bullet_world + # Since this file can't import world.py this holds the reference to the current_bullet_world self.bullet_world = None self.shadow_world = None diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index cc45ad372..a836034d1 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -3,8 +3,8 @@ import rospy import pybullet as p -from .bullet_world import Object, BulletWorld, Use_shadow_world -from .bullet_world_reasoning import contact +from .world import Object, BulletWorld, Use_shadow_world +from .world_reasoning import contact from .costmaps import Costmap from .pose import Pose, Transform from .robot_descriptions import robot_description diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 7879654ca..f8c3e5d17 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -3,9 +3,9 @@ import pybullet as p -import pycram.bullet_world_reasoning as btr +import pycram.world_reasoning as btr import pycram.helper as helper -from ..bullet_world import BulletWorld +from ..world import BulletWorld from ..external_interfaces.ik import request_ik from ..local_transformer import LocalTransformer as local_tf from ..process_module import ProcessModule, ProcessModuleManager @@ -22,10 +22,10 @@ def _park_arms(arm): robot = BulletWorld.robot if arm == "right": for joint, pose in robot_description.get_static_joint_chain("right", "park").items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) if arm == "left": for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) class BoxyNavigation(ProcessModule): @@ -39,7 +39,7 @@ def _execute(self, desig): robot = BulletWorld.robot # Reset odom joints to zero for joint_name in robot_description.odom_joints: - robot.set_joint_state(joint_name, 0.0) + robot.set_joint_position(joint_name, 0.0) # Set actual goal pose robot.set_position_and_orientation(solution['target'], solution['orientation']) time.sleep(0.5) @@ -110,7 +110,7 @@ def _execute(self, desig): drawer_handle = solution['drawer_handle'] drawer_joint = solution['drawer_joint'] dis = solution['distance'] - robot.set_joint_state(robot_description.torso_joint, -0.1) + robot.set_joint_position(robot_description.torso_joint, -0.1) arm = "left" if solution['gripper'] == robot_description.get_tool_frame("left") else "right" joints = robot_description._safely_access_chains(arm).joints #inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), kitchen.get_link_position(drawer_handle)) @@ -126,7 +126,7 @@ def _execute(self, desig): new_p = helper._transform_to_torso(new_p, robot) inv = request_ik(robot_description.base_frame, gripper, new_p, robot, joints) helper._apply_ik(robot, inv, gripper) - kitchen.set_joint_state(drawer_joint, 0.3) + kitchen.set_joint_position(drawer_joint, 0.3) time.sleep(0.5) @@ -190,7 +190,7 @@ def _execute(self, desig): else: conf = "behind_up" for joint, state in robot_description.get_static_joint_chain("neck", conf).items(): - robot.set_joint_state(joint, state) + robot.set_joint_position(joint, state) class BoxyMoveGripper(ProcessModule): @@ -207,7 +207,7 @@ def _execute(self, desig): motion = solution['motion'] for joint, state in robot_description.get_static_gripper_chain(gripper, motion).items(): # TODO: Test this, add gripper-opening/-closing to the demo.py - robot.set_joint_state(joint, state) + robot.set_joint_position(joint, state) time.sleep(0.5) @@ -261,13 +261,13 @@ def _execute(self, desig): left_arm_poses = solution['left_arm_poses'] if type(right_arm_poses) == dict: for joint, pose in right_arm_poses.items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) elif type(right_arm_poses) == str and right_arm_poses == "park": _park_arms("right") if type(left_arm_poses) == dict: for joint, pose in left_arm_poses.items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) elif type(right_arm_poses) == str and left_arm_poses == "park": _park_arms("left") diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 98d45f430..44db2771f 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -3,9 +3,9 @@ import pybullet as p -import pycram.bullet_world_reasoning as btr +import pycram.world_reasoning as btr import pycram.helper as helper -from ..bullet_world import BulletWorld +from ..world import BulletWorld from ..local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description @@ -21,7 +21,7 @@ def _park_arms(arm): robot = BulletWorld.robot if arm == "left": for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) class DonbotNavigation(ProcessModule): @@ -35,7 +35,7 @@ def _execute(self, desig): robot = BulletWorld.robot # Reset odom joints to zero for joint_name in robot_description.odom_joints: - robot.set_joint_state(joint_name, 0.0) + robot.set_joint_position(joint_name, 0.0) # Set actual goal pose robot.set_position_and_orientation(solution['target'], solution['orientation']) time.sleep(0.5) @@ -102,7 +102,7 @@ def _execute(self, desig): new_p = [han_pose[0] - dis, han_pose[1], han_pose[2]] inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), new_p) helper._apply_ik(robot, inv) - kitchen.set_joint_state(drawer_joint, 0.3) + kitchen.set_joint_position(drawer_joint, 0.3) time.sleep(0.5) @@ -153,7 +153,7 @@ def _execute(self, desig): else: conf = "right" for joint, state in robot_description.get_static_joint_chain("neck", conf).items(): - robot.set_joint_state(joint, state) + robot.set_joint_position(joint, state) class DonbotMoveGripper(ProcessModule): @@ -170,7 +170,7 @@ def _execute(self, desig): motion = solution['motion'] for joint, state in robot_description.get_static_gripper_chain(gripper, motion).items(): # TODO: Test this, add gripper-opening/-closing to the demo.py - robot.set_joint_state(joint, state) + robot.set_joint_position(joint, state) time.sleep(0.5) @@ -224,7 +224,7 @@ def _execute(self, desig): if type(left_arm_poses) == dict: for joint, pose in left_arm_poses.items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) elif type(left_arm_poses) == str and left_arm_poses == "park": _park_arms("left") diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index e3df9dba1..594f355f1 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -2,9 +2,9 @@ from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..bullet_world import BulletWorld +from ..world import BulletWorld from ..helper import _apply_ik -import pycram.bullet_world_reasoning as btr +import pycram.world_reasoning as btr import pybullet as p import logging import time @@ -20,7 +20,7 @@ def _park_arms(arm): robot = BulletWorld.robot if arm == "left": for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) class HSRNavigation(ProcessModule): @@ -96,7 +96,7 @@ def _execute(self, desig): new_p = [han_pose[0] - dis, han_pose[1], han_pose[2]] inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), new_p) _apply_ik(robot, inv) - kitchen.set_joint_state(drawer_joint, 0.3) + kitchen.set_joint_position(drawer_joint, 0.3) time.sleep(0.5) @@ -125,7 +125,7 @@ def _execute(self, desig): if target == 'forward' or target == 'down': robot = BulletWorld.robot for joint, state in robot_description.get_static_joint_chain("neck", target).items(): - robot.set_joint_state(joint, state) + robot.set_joint_position(joint, state) else: logging.error("There is no target position defined with the target %s.", target) @@ -143,7 +143,7 @@ def _execute(self, desig): gripper = solution['gripper'] motion = solution['motion'] for joint, state in robot_description.get_static_gripper_chain(gripper, motion).items(): - robot.set_joint_state(joint, state) + robot.set_joint_position(joint, state) time.sleep(0.5) @@ -197,7 +197,7 @@ def _execute(self, desig): if type(left_arm_poses) == dict: for joint, pose in left_arm_poses.items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) elif type(left_arm_poses) == str and left_arm_poses == "park": _park_arms("left") diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index f93544d70..07800af68 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -4,7 +4,7 @@ import actionlib -import pycram.bullet_world_reasoning as btr +import pycram.world_reasoning as btr import numpy as np import time import rospy @@ -13,7 +13,7 @@ from ..plan_failures import EnvironmentManipulationImpossible from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..bullet_world import BulletWorld, Object +from ..world import BulletWorld, Object from ..helper import transform from ..external_interfaces.ik import request_ik, IKError from ..helper import _transform_to_torso, _apply_ik, calculate_wrist_tool_offset, inverseTimes @@ -39,10 +39,10 @@ def _park_arms(arm): robot = BulletWorld.robot if arm == "right": for joint, pose in robot_description.get_static_joint_chain("right", "park").items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) if arm == "left": for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) class Pr2Navigation(ProcessModule): @@ -121,11 +121,11 @@ def _execute(self, desig: LookingMotion.Motion): new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 - current_pan = robot.get_joint_state("head_pan_joint") - current_tilt = robot.get_joint_state("head_tilt_joint") + current_pan = robot.get_joint_position("head_pan_joint") + current_tilt = robot.get_joint_position("head_tilt_joint") - robot.set_joint_state("head_pan_joint", new_pan + current_pan) - robot.set_joint_state("head_tilt_joint", new_tilt + current_tilt) + robot.set_joint_position("head_pan_joint", new_pan + current_pan) + robot.set_joint_position("head_tilt_joint", new_tilt + current_tilt) class Pr2MoveGripper(ProcessModule): @@ -139,7 +139,7 @@ def _execute(self, desig: MoveGripperMotion.Motion): gripper = desig.gripper motion = desig.motion for joint, state in robot_description.get_static_gripper_chain(gripper, motion).items(): - robot.set_joint_state(joint, state) + robot.set_joint_position(joint, state) class Pr2Detecting(ProcessModule): @@ -184,9 +184,9 @@ def _execute(self, desig: MoveArmJointsMotion.Motion): robot = BulletWorld.robot if desig.right_arm_poses: - robot.set_joint_states(desig.right_arm_poses) + robot.set_positions_of_all_joints(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_states(desig.left_arm_poses) + robot.set_positions_of_all_joints(desig.left_arm_poses) class PR2MoveJoints(ProcessModule): @@ -195,7 +195,7 @@ class PR2MoveJoints(ProcessModule): """ def _execute(self, desig: MoveJointsMotion.Motion): robot = BulletWorld.robot - robot.set_joint_states(dict(zip(desig.names, desig.positions))) + robot.set_positions_of_all_joints(dict(zip(desig.names, desig.positions))) class Pr2WorldStateDetecting(ProcessModule): @@ -223,8 +223,8 @@ def _execute(self, desig: OpeningMotion.Motion): _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - desig.object_part.bullet_world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( + desig.object_part.bullet_world_object.set_joint_position(container_joint, + part_of_object.get_joint_limits( container_joint)[1]) @@ -243,8 +243,8 @@ def _execute(self, desig: ClosingMotion.Motion): _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - desig.object_part.bullet_world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( + desig.object_part.bullet_world_object.set_joint_position(container_joint, + part_of_object.get_joint_limits( container_joint)[0]) @@ -301,8 +301,8 @@ def _execute(self, desig: LookingMotion.Motion): new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 - current_pan = robot.get_joint_state("head_pan_joint") - current_tilt = robot.get_joint_state("head_tilt_joint") + current_pan = robot.get_joint_position("head_pan_joint") + current_tilt = robot.get_joint_position("head_tilt_joint") giskard.avoid_all_collisions() giskard.achieve_joint_goal({"head_pan_joint": new_pan + current_pan, diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index c031ed840..cd2c76430 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -1,6 +1,6 @@ from pycram.external_interfaces.giskard import achieve_cartesian_goal from pycram.designators.location_designator import CostmapLocation -from pycram.bullet_world import Use_shadow_world, BulletWorld +from pycram.world import Use_shadow_world, BulletWorld from pycram.helper import _apply_ik from pycram.pose import Pose from pycram.robot_descriptions import robot_description diff --git a/src/pycram/resolver/location/jpt_location.py b/src/pycram/resolver/location/jpt_location.py index 2f728d14c..5c7bf1bae 100644 --- a/src/pycram/resolver/location/jpt_location.py +++ b/src/pycram/resolver/location/jpt_location.py @@ -12,7 +12,7 @@ from ...costmaps import OccupancyCostmap, plot_grid from ...plan_failures import PlanFailure from ...pose import Pose -from pycram.bullet_world import BulletWorld, Object +from pycram.world import BulletWorld, Object class JPTCostmapLocation(pycram.designators.location_designator.CostmapLocation): diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros/force_torque_sensor.py index d61f292a3..6da358d55 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros/force_torque_sensor.py @@ -7,7 +7,7 @@ from geometry_msgs.msg import WrenchStamped from std_msgs.msg import Header -from..bullet_world import BulletWorld +from..world import BulletWorld class ForceTorqueSensor: diff --git a/src/pycram/ros/joint_state_publisher.py b/src/pycram/ros/joint_state_publisher.py index 8cefe2547..1126c535d 100644 --- a/src/pycram/ros/joint_state_publisher.py +++ b/src/pycram/ros/joint_state_publisher.py @@ -6,7 +6,7 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header -from ..bullet_world import BulletWorld +from ..world import BulletWorld class JointStatePublisher: @@ -41,7 +41,7 @@ def _publish(self) -> None: seq = 0 while not self.kill_event.is_set(): - current_joint_states = [robot.get_joint_state(joint_name) for joint_name in joint_names] + current_joint_states = [robot.get_joint_position(joint_name) for joint_name in joint_names] h = Header() h.stamp = rospy.Time.now() h.seq = seq diff --git a/src/pycram/ros/robot_state_updater.py b/src/pycram/ros/robot_state_updater.py index 6f8382544..845d20351 100644 --- a/src/pycram/ros/robot_state_updater.py +++ b/src/pycram/ros/robot_state_updater.py @@ -6,7 +6,7 @@ from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState -from pycram.bullet_world import BulletWorld +from pycram.world import BulletWorld from pycram.robot_descriptions import robot_description from pycram.pose import Transform, Pose @@ -58,7 +58,7 @@ def _subscribe_joint_state(self, msg: JointState) -> None: try: msg = rospy.wait_for_message(self.joint_state_topic, JointState) for name, position in zip(msg.name, msg.position): - BulletWorld.robot.set_joint_state(name, position) + BulletWorld.robot.set_joint_position(name, position) except AttributeError: pass diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index 25c6878ed..ec0ab9cc3 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -4,7 +4,7 @@ import atexit from ..pose import Pose -from ..bullet_world import BulletWorld +from ..world import BulletWorld from tf2_msgs.msg import TFMessage diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 52bfe879c..90278afd4 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -5,7 +5,7 @@ from geometry_msgs.msg import Vector3 from std_msgs.msg import ColorRGBA -from pycram.bullet_world import BulletWorld, Object +from pycram.world import BulletWorld, Object from visualization_msgs.msg import MarkerArray, Marker import rospy import urdf_parser_py diff --git a/src/pycram/task.py b/src/pycram/task.py index f17e2b7e1..74df58952 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -14,7 +14,7 @@ import sqlalchemy.orm.session import tqdm -from .bullet_world import BulletWorld +from .world import BulletWorld from .orm.task import (Code as ORMCode, TaskTreeNode as ORMTaskTreeNode) from .orm.base import ProcessMetaData from .plan_failures import PlanFailure diff --git a/src/pycram/world.py b/src/pycram/world.py deleted file mode 100644 index 095604298..000000000 --- a/src/pycram/world.py +++ /dev/null @@ -1,1866 +0,0 @@ -# used for delayed evaluation of typing until python 3.11 becomes mainstream -from __future__ import annotations - -import logging -import os -import pathlib -import re -import threading -import time -import xml.etree.ElementTree -from queue import Queue -import tf -from typing import List, Optional, Dict, Tuple, Callable, Iterable -from typing import Union - -import numpy as np -import pybullet as p -import rospkg -import rospy -import rosgraph - -import urdf_parser_py.urdf -from geometry_msgs.msg import Quaternion, Point -from urdf_parser_py.urdf import URDF - -from .event import Event -from .robot_descriptions import robot_description -from .enums import JointType, ObjectType -from .local_transformer import LocalTransformer -from sensor_msgs.msg import JointState - -from .pose import Pose, Transform - - -class BulletWorld: - """ - The BulletWorld Class represents the physics Simulation and belief state. - """ - - current_bullet_world: BulletWorld = None - """ - Global reference to the currently used BulletWorld, usually this is the - graphical one. However, if you are inside a Use_shadow_world() environment the current_bullet_world points to the - shadow world. In this way you can comfortably use the current_bullet_world, which should point towards the BulletWorld - used at the moment. - """ - robot: Object = None - """ - Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the - URDF with the name of the URDF on the parameter server. - """ - - # Check is for sphinx autoAPI to be able to work in a CI workflow - if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): - rospy.init_node('pycram') - - def __init__(self, type: str = "GUI", is_shadow_world: bool = False): - """ - Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the - background. There can only be one rendered simulation. - The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. - - :param type: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" - :param is_shadow_world: For internal usage, decides if this BulletWorld should be used as a shadow world. - """ - self.objects: List[Object] = [] - self.client_id: int = -1 - self.detachment_event: Event = Event() - self.attachment_event: Event = Event() - self.manipulation_event: Event = Event() - self.type: str = type - self._gui_thread: Gui = Gui(self, type) - self._gui_thread.start() - # This disables file caching from PyBullet, since this would also cache - # files that can not be loaded - p.setPhysicsEngineParameter(enableFileCaching=0) - # Needed to let the other thread start the simulation, before Objects are spawned. - time.sleep(0.1) - if BulletWorld.current_bullet_world == None: - BulletWorld.current_bullet_world = self - self.vis_axis: Object = [] - self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} - self.data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] - self.shadow_world: BulletWorld = BulletWorld("DIRECT", True) if not is_shadow_world else None - self.world_sync: WorldSync = WorldSync(self, self.shadow_world) if not is_shadow_world else None - self.is_shadow_world: bool = is_shadow_world - self.local_transformer = LocalTransformer() - if not is_shadow_world: - self.world_sync.start() - self.local_transformer.bullet_world = self - self.local_transformer.shadow_world = self.shadow_world - - # Some default settings - self.set_gravity([0, 0, -9.8]) - if not is_shadow_world: - plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - # atexit.register(self.exit) - - def get_objects_by_name(self, name: str) -> List[Object]: - """ - Returns a list of all Objects in this BulletWorld with the same name as the given one. - - :param name: The name of the returned Objects. - :return: A list of all Objects with the name 'name'. - """ - return list(filter(lambda obj: obj.name == name, self.objects)) - - def get_objects_by_type(self, obj_type: str) -> List[Object]: - """ - Returns a list of all Objects which have the type 'obj_type'. - - :param obj_type: The type of the returned Objects. - :return: A list of all Objects that have the type 'obj_type'. - """ - return list(filter(lambda obj: obj.type == obj_type, self.objects)) - - def get_object_by_id(self, id: int) -> Object: - """ - Returns the single Object that has the unique id. - - :param id: The unique id for which the Object should be returned. - :return: The Object with the id 'id'. - """ - return list(filter(lambda obj: obj.id == id, self.objects))[0] - - def remove_object(self, obj_id: int) -> None: - """ - Remove an object by its ID. - - :param obj_id: The unique id of the object to be removed. - """ - - p.removeBody(obj_id, self.client_id) - - def attach(self, - parent_obj: Object, - child_obj: Object, - parent_link: Optional[str] = None, - loose: Optional[bool] = False) -> None: - """ - Attaches another object to this object. This is done by - saving the transformation between the given link, if there is one, and - the base pose of the other object. Additionally, the name of the link, to - which the object is attached, will be saved. - Furthermore, a constraint of pybullet will be created so the attachment - also works while simulation. - Loose attachments means that the attachment will only be one-directional. - For example, if the parent object moves, the child object will also move, but not the other way around. - - :param parent_obj: The parent object (jf loose, then this would not move when child moves) - :param child_obj: The child object - :param parent_link: The link of the parent object to which the child object should be attached - :param loose: If the attachment should be a loose attachment. - """ - link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 - link_to_object = parent_obj._calculate_transform(child_obj, parent_link) - parent_obj.attachments[child_obj] = [link_to_object, parent_link, loose] - child_obj.attachments[parent_obj] = [link_to_object.invert(), None, False] - - cid = p.createConstraint(parent_obj.id, link_id, child_obj.id, -1, p.JOINT_FIXED, - [0, 1, 0], link_to_object.translation_as_list(), [0, 0, 0], - link_to_object.rotation_as_list(), - physicsClientId=self.client_id) - parent_obj.cids[child_obj] = cid - child_obj.cids[parent_obj] = cid - self.attachment_event(parent_obj, [parent_obj, child_obj]) - - def detach(self, obj1: Object, obj2: Object) -> None: - """ - Detaches obj2 from obj1. This is done by - deleting the attachment from the attachments dictionary of both objects - and deleting the constraint of pybullet. - Afterward the detachment event of the corresponding BulletWorld will be fired. - - :param obj1: The object from which an object should be detached. - :param obj2: The object which should be detached. - """ - del obj1.attachments[obj2] - del obj2.attachments[obj1] - - p.removeConstraint(obj1.cids[obj2], physicsClientId=self.client_id) - - del obj1.cids[obj2] - del obj2.cids[obj1] - obj1.world.detachment_event(obj1, [obj1, obj2]) - - def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: - """ - Updates the positions of all attached objects. This is done - by calculating the new pose in world coordinate frame and setting the - base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. - - :param prev_object: A list of Objects that were already moved, - these will be excluded to prevent loops in the update. - """ - for att_obj in obj.attachments: - if att_obj in prev_object: - continue - if obj.attachments[att_obj][2]: - # Updates the attachment transformation and constraint if the - # attachment is loose, instead of updating the position of all attached objects - link_to_object = obj._calculate_transform(att_obj, obj.attachments[att_obj][1]) - link_id = obj.get_link_id(obj.attachments[att_obj][1]) if obj.attachments[att_obj][1] else -1 - obj.attachments[att_obj][0] = link_to_object - att_obj.attachments[obj][0] = link_to_object.invert() - p.removeConstraint(obj.cids[att_obj], physicsClientId=self.client_id) - cid = p.createConstraint(obj.id, link_id, att_obj.id, -1, p.JOINT_FIXED, [0, 0, 0], - link_to_object.translation_as_list(), - [0, 0, 0], link_to_object.rotation_as_list(), - physicsClientId=self.client_id) - obj.cids[att_obj] = cid - att_obj.cids[obj] = cid - else: - link_to_object = obj.attachments[att_obj][0] - - world_to_object = obj.local_transformer.transform_pose(link_to_object.to_pose(), "map") - p.resetBasePositionAndOrientation(att_obj.id, world_to_object.position_as_list(), - world_to_object.orientation_as_list(), - physicsClientId=self.client_id) - att_obj._current_pose = world_to_object - self._set_attached_objects(att_obj, prev_object + [obj]) - - def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: - """ - Get the joint limits of an articulated object - - :param obj: The object - :param joint_name: The name of the joint - :return: A tuple containing the upper and the lower limits of the joint. - """ - up_lim, low_lim = p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8:10] - return up_lim, low_lim - - def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: - """ - Returns the axis along which a joint is moving. The given joint_name has to be part of this object. - - :param obj: The object - :param joint_name: Name of the joint for which the axis should be returned. - :return: The axis a vector of xyz - """ - return p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[13] - - def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: - """ - Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - - :param obj: The object - :param joint_name: Joint name for which the type should be returned - :return: The type of the joint - """ - joint_type = p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[2] - return JointType(joint_type) - - def get_object_joint_position(self, obj: Object, joint_name: str) -> float: - """ - Get the state of a joint of an articulated object - - :param obj: The object - :param joint_name: The name of the joint - """ - return p.getJointState(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[0] - - def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: - """ - Get the pose of a link of an articulated object with respect to the world frame. - The pose is given as a tuple of position and orientation. - - :param obj: The object - :param link_name: The name of the link - """ - return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] - - def get_object_contact_points(self, obj: Object) -> List: - """ - Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ - - :param obj: The object. - :return: A list of all contact points with other objects - """ - return p.getContactPoints(obj.id) - - def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: - """ - Get the ID of a joint in an articulated object. - - :param obj: The object - :param joint_idx: The index of the joint (would indicate order). - """ - return p.getJointInfo(obj.id, joint_idx, self.client_id)[0] - - def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: - """ - Get the name of a joint in an articulated object. - - :param obj: The object - :param joint_idx: The index of the joint (would indicate order). - """ - return p.getJointInfo(obj.id, joint_idx, self.client_id)[1].decode('utf-8') - - def get_object_link_name(self, obj: Object, link_idx: int) -> str: - """ - Get the name of a link in an articulated object. - - :param obj: The object - :param link_idx: The index of the link (would indicate order). - """ - return p.getJointInfo(obj.id, link_idx, self.client_id)[12].decode('utf-8') - - def get_object_number_of_joints(self, obj: Object) -> int: - """ - Get the number of joints of an articulated object - - :param obj: The object - """ - return p.getNumJoints(obj.id, self.client_id) - - def reset_joint_state(self, obj: Object, joint_name: str, joint_pose: float) -> None: - """ - Reset the joint state instantly without physics simulation - - :param obj: The object - :param joint_name: The name of the joint - :param joint_pose: The new joint pose - """ - p.resetJointState(obj.id, obj.joints[joint_name], joint_pose, physicsClientId=self.client_id) - - def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): - """ - 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 position: The new position of the object as a vector of x,y,z - :param orientation: The new orientation of the object as a quaternion of x,y,z,w - """ - p.resetBasePositionAndOrientation(obj.id, position, orientation, self.client_id) - - def step(self): - """ - Step the world simulation using forward dynamics - """ - p.stepSimulation(self.client_id) - - def set_object_color(self, obj: Object, color: List[float], link: Optional[str] = ""): - """ - Changes the color of this object, the color has to be given as a list - of RGBA values. Optionally a link name can can be provided, if no link - name is provided all links of this object will be colored. - - :param obj: The object which should be colored - :param color: The color as RGBA values between 0 and 1 - :param link: The link name of the link which should be colored - """ - if link == "": - # Check if there is only one link, this is the case for primitive - # forms or if loaded from an .stl or .obj file - if obj.links != {}: - for link_id in obj.links.values(): - p.changeVisualShape(obj.id, link_id, rgbaColor=color, physicsClientId=self.client_id) - else: - p.changeVisualShape(obj.id, -1, rgbaColor=color, physicsClientId=self.client_id) - else: - p.changeVisualShape(obj.id, obj.links[link], rgbaColor=color, physicsClientId=self.client_id) - - def get_object_color(self, - obj: Object, - link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: - """ - This method returns the color of this object or a link of this object. If no link is given then the - return is either: - - 1. A list with the color as RGBA values, this is the case if the object only has one link (this - happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the color as value. The color is given as RGBA values. - Please keep in mind that not every link may have a color. This is dependent on the URDF from which the - object is spawned. - - If a link is specified then the return is a list with RGBA values representing the color of this link. - It may be that this link has no color, in this case the return is None as well as an error message. - - :param obj: The object for which the color should be returned. - :param link: the link name for which the color should be returned. - :return: The color of the object or link, or a dictionary containing every colored link with its color - """ - visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) - swap = {v: k for k, v in obj.links.items()} - links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) - colors = list(map(lambda x: x[7], visual_data)) - link_to_color = dict(zip(links, colors)) - - if link: - if link in link_to_color.keys(): - return link_to_color[link] - elif link not in obj.links.keys(): - rospy.logerr(f"The link '{link}' is not part of this obejct") - return None - else: - rospy.logerr(f"The link '{link}' has no color") - return None - - if len(visual_data) == 1: - return list(visual_data[0][7]) - else: - return link_to_color - - def get_object_AABB(self, obj: Object, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: - """ - Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case - the axis aligned bounding box of the link will be returned. The return of this method are two points in - world coordinate frame which define a bounding box. - - :param obj: The object for which the bounding box should be returned. - :param link_name: The Optional name of a link of this object. - :return: Two lists of x,y,z which define the bounding box. - """ - if link_name: - return p.getAABB(obj.id, obj.links[link_name], self.client_id) - else: - return p.getAABB(obj.id, physicsClientId=self.client_id) - - def get_attachment_event(self) -> Event: - """ - Returns the event reference that is fired if an attachment occurs. - - :return: The reference to the attachment event - """ - return self.attachment_event - - def get_detachment_event(self) -> Event: - """ - Returns the event reference that is fired if a detachment occurs. - - :return: The event reference for the detachment event. - """ - return self.detachment_event - - def get_manipulation_event(self) -> Event: - """ - Returns the event reference that is fired if any manipulation occurs. - - :return: The event reference for the manipulation event. - """ - return self.manipulation_event - - def set_realtime(self, real_time: bool) -> None: - """ - Enables the real time simulation of Physic in the BulletWorld. By default this is disabled and Physic is only - simulated to reason about it. - - :param real_time: Whether the BulletWorld should simulate Physic in real time. - """ - p.setRealTimeSimulation(1 if real_time else 0, self.client_id) - - def set_gravity(self, velocity: List[float]) -> None: - """ - Sets the gravity that is used in the BullteWorld, by default the is the gravity on earth ([0, 0, -9.8]). Gravity - is given as a vector in x,y,z. Gravity is only applied while simulating Physic. - - :param velocity: The gravity vector that should be used in the BulletWorld. - """ - p.setGravity(velocity[0], velocity[1], velocity[2], physicsClientId=self.client_id) - - def set_robot(self, robot: Object) -> None: - """ - Sets the global variable for the robot Object. This should be set on spawning the robot. - - :param robot: The Object reference to the Object representing the robot. - """ - BulletWorld.robot = robot - - def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: - """ - Simulates Physic in the BulletWorld for a given amount of seconds. Usually this simulation is faster than real - time, meaning you can simulate for example 10 seconds of Physic in the BulletWorld in 1 second real time. By - setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real - time. - - :param seconds: The amount of seconds that should be simulated. - :param real_time: If the simulation should happen in real time or faster. - """ - for i in range(0, int(seconds * 240)): - p.stepSimulation(self.client_id) - for objects, callback in self.coll_callbacks.items(): - contact_points = p.getContactPoints(objects[0].id, objects[1].id, physicsClientId=self.client_id) - # contact_points = p.getClosestPoints(objects[0].id, objects[1].id, 0.02) - # print(contact_points[0][5]) - if contact_points != (): - callback[0]() - elif callback[1] != None: # Call no collision callback - callback[1]() - if real_time: - # Simulation runs at 240 Hz - time.sleep(0.004167) - - def exit(self) -> None: - """ - Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the - preferred method to close the BulletWorld. - """ - # True if this is NOT the shadow world since it has a reference to the - # Shadow world - time.sleep(0.1) - if self.shadow_world: - self.world_sync.terminate = True - self.world_sync.join() - self.shadow_world.exit() - p.disconnect(self.client_id) - if self._gui_thread: - self._gui_thread.join() - if BulletWorld.current_bullet_world == self: - BulletWorld.current_bullet_world = None - BulletWorld.robot = None - - def save_state(self) -> Tuple[int, Dict]: - """ - Returns the id of the saved state of the BulletWorld. The saved state contains the position, orientation and joint - position of every Object in the BulletWorld. - - :return: A unique id of the state - """ - objects2attached = {} - # ToDo find out what this is for and where it is used - for o in self.objects: - objects2attached[o] = (o.attachments.copy(), o.cids.copy()) - return p.saveState(self.client_id), objects2attached - - def restore_state(self, state, objects2attached: Dict = {}) -> None: - """ - Restores the state of the BulletWorld according to the given state id. This includes position, orientation and - joint states. However, restore can not respawn objects if there are objects that were deleted between creation of - the state and restoring they will be skiped. - - :param state: The unique id representing the state, as returned by :func:`~save_state` - :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~bullet_world.Object.attachments` - """ - p.restoreState(state, physicsClientId=self.client_id) - for obj in self.objects: - try: - obj.attachments, obj.cids = objects2attached[obj] - except KeyError: - continue - - def copy(self) -> BulletWorld: - """ - Copies this Bullet World into another and returns it. The other BulletWorld - will be in Direct mode. The shadow world should always be preferred instead of creating a new BulletWorld. - This method should only be used if necessary since there can be unforeseen problems. - - :return: The reference to the new BulletWorld - """ - world = BulletWorld("DIRECT") - for obj in self.objects: - o = Object(obj.name, obj.type, obj.path, obj.get_position(), obj.get_orientation(), - world, obj.color) - for joint in obj.joints: - o.set_joint_state(joint, obj.get_joint_state(joint)) - return world - - def add_vis_axis(self, pose: Pose, - length: Optional[float] = 0.2) -> None: - """ - Creates a Visual object which represents the coordinate frame at the given - position and orientation. There can be an unlimited amount of vis axis objects. - - :param pose: The pose at which the axis should be spawned - :param length: Optional parameter to configure the length of the axes - """ - - position, orientation = pose.to_list() - - vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], - rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) - vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], - rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) - vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], - rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) - - obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], - basePosition=position, baseOrientation=orientation, - linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkParentIndices=[0, 0, 0], - linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], - linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], - linkCollisionShapeIndices=[-1, -1, -1]) - - self.vis_axis.append(obj) - - def remove_vis_axis(self) -> None: - """ - Removes all spawned vis axis objects that are currently in this BulletWorld. - """ - for id in self.vis_axis: - p.removeBody(id) - self.vis_axis = [] - - def register_collision_callback(self, objectA: Object, objectB: Object, - callback_collision: Callable, - callback_no_collision: Optional[Callable] = None) -> None: - """ - Registers callback methods for contact between two Objects. There can be a callback for when the two Objects - get in contact and, optionally, for when they are not in contact anymore. - - :param objectA: An object in the BulletWorld - :param objectB: Another object in the BulletWorld - :param callback_collision: A function that should be called if the objects are in contact - :param callback_no_collision: A function that should be called if the objects are not in contact - """ - self.coll_callbacks[(objectA, objectB)] = (callback_collision, callback_no_collision) - - def add_additional_resource_path(self, path: str) -> None: - """ - Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an - Object is spawned only with a filename. - - :param path: A path in the filesystem in which to search for files. - """ - self.data_directory.append(path) - - def get_shadow_object(self, object: Object) -> Object: - """ - Returns the corresponding object from the shadow world for the given object. If the given Object is already in - the shadow world it is returned. - - :param object: The object for which the shadow worlds object should be returned. - :return: The corresponding object in the shadow world. - """ - try: - return self.world_sync.object_mapping[object] - except KeyError: - shadow_world = self if self.is_shadow_world else self.shadow_world - if object in shadow_world.objects: - return object - else: - raise ValueError( - f"There is no shadow object for the given object: {object}, this could be the case if the object isn't anymore in the main (graphical) BulletWorld or if the given object is already a shadow object. ") - - def get_bullet_object_for_shadow(self, object: Object) -> Object: - """ - Returns the corresponding object from the main Bullet World for a given - object in the shadow world. If the given object is not in the shadow - world an error will be raised. - - :param object: The object for which the corresponding object in the main Bullet World should be found - :return: The object in the main Bullet World - """ - map = self.world_sync.object_mapping - try: - return list(map.keys())[list(map.values()).index(object)] - except ValueError: - raise ValueError("The given object is not in the shadow world.") - - def reset_bullet_world(self) -> None: - """ - Resets the BulletWorld to the state it was first spawned in. - All attached objects will be detached, all joints will be set to the - default position of 0 and all objects will be set to the position and - orientation in which they were spawned. - """ - for obj in self.objects: - if obj.attachments: - attached_objects = list(obj.attachments.keys()) - for att_obj in attached_objects: - obj.detach(att_obj) - joint_names = list(obj.joints.keys()) - joint_poses = [0 for j in joint_names] - obj.set_joint_states(dict(zip(joint_names, joint_poses))) - obj.set_pose(obj.original_pose) - - -class Use_shadow_world(): - """ - An environment for using the shadow world, while in this environment the :py:attr:`~BulletWorld.current_bullet_world` - variable will point to the shadow world. - - Example: - with Use_shadow_world(): - NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() - """ - - def __init__(self): - self.prev_world: BulletWorld = None - - def __enter__(self): - if not BulletWorld.current_bullet_world.is_shadow_world: - time.sleep(20 / 240) - # blocks until the adding queue is ready - BulletWorld.current_bullet_world.world_sync.add_obj_queue.join() - # **This is currently not used since the sleep(20/240) seems to be enough, but on weaker hardware this might - # not be a feasible solution** - # while not BulletWorld.current_bullet_world.world_sync.equal_states: - # time.sleep(0.1) - - self.prev_world = BulletWorld.current_bullet_world - BulletWorld.current_bullet_world.world_sync.pause_sync = True - BulletWorld.current_bullet_world = BulletWorld.current_bullet_world.shadow_world - - def __exit__(self, *args): - if not self.prev_world == None: - BulletWorld.current_bullet_world = self.prev_world - BulletWorld.current_bullet_world.world_sync.pause_sync = False - - -class WorldSync(threading.Thread): - """ - Synchronizes the state between the BulletWorld and its shadow world. - Meaning the cartesian and joint position of everything in the shadow world will be - synchronized with the main BulletWorld. - Adding and removing objects is done via queues, such that loading times of objects - in the shadow world does not affect the BulletWorld. - The class provides the possibility to pause the synchronization, this can be used - if reasoning should be done in the shadow world. - """ - - def __init__(self, world: BulletWorld, shadow_world: BulletWorld): - threading.Thread.__init__(self) - self.world: BulletWorld = world - self.shadow_world: BulletWorld = shadow_world - self.shadow_world.world_sync: WorldSync = self - - self.terminate: bool = False - self.add_obj_queue: Queue = Queue() - self.remove_obj_queue: Queue = Queue() - self.pause_sync: bool = False - # Maps bullet to shadow world objects - self.object_mapping: Dict[Object, Object] = {} - self.equal_states = False - - def run(self): - """ - Main method of the synchronization, this thread runs in a loop until the - terminate flag is set. - While this loop runs it continuously checks the cartesian and joint position of - every object in the BulletWorld and updates the corresponding object in the - shadow world. When there are entries in the adding or removing queue the corresponding objects will be added - or removed in the same iteration. - """ - while not self.terminate: - self.check_for_pause() - # self.equal_states = False - for i in range(self.add_obj_queue.qsize()): - obj = self.add_obj_queue.get() - # [name, type, path, position, orientation, self.world.shadow_world, color, bulletworld object] - o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) - # Maps the BulletWorld object to the shadow world object - self.object_mapping[obj[7]] = o - self.add_obj_queue.task_done() - for i in range(self.remove_obj_queue.qsize()): - obj = self.remove_obj_queue.get() - # Get shadow world object reference from object mapping - shadow_obj = self.object_mapping[obj] - shadow_obj.remove() - del self.object_mapping[obj] - self.remove_obj_queue.task_done() - - for bulletworld_obj, shadow_obj in self.object_mapping.items(): - b_pose = bulletworld_obj.get_pose() - s_pose = shadow_obj.get_pose() - if b_pose.dist(s_pose) != 0.0: - shadow_obj.set_pose(bulletworld_obj.get_pose()) - - # Manage joint positions - if len(bulletworld_obj.joints) > 2: - for joint_name in bulletworld_obj.joints.keys(): - if shadow_obj.get_joint_state(joint_name) != bulletworld_obj.get_joint_state(joint_name): - shadow_obj.set_joint_states(bulletworld_obj.get_complete_joint_state()) - break - - self.check_for_pause() - # self.check_for_equal() - time.sleep(1 / 240) - - self.add_obj_queue.join() - self.remove_obj_queue.join() - - def check_for_pause(self) -> None: - """ - Checks if :py:attr:`~self.pause_sync` is true and sleeps this thread until it isn't anymore. - """ - while self.pause_sync: - time.sleep(0.1) - - def check_for_equal(self) -> None: - """ - Checks if both BulletWorlds have the same state, meaning all objects are in the same position. - This is currently not used, but might be used in the future if synchronization issues worsen. - """ - eql = True - for obj, shadow_obj in self.object_mapping.items(): - eql = eql and obj.get_pose() == shadow_obj.get_pose() - self.equal_states = eql - - -class Gui(threading.Thread): - """ - For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~BulletWorld.exit` - Also contains the code for controlling the camera. - """ - - def __init__(self, world, type): - threading.Thread.__init__(self) - self.world: BulletWorld = world - self.type: str = type - - def run(self): - """ - Initializes the new simulation and checks in an endless loop - if it is still active. If it is the thread will be suspended for 1/80 seconds, if it is not the method and - thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. - """ - if self.type != "GUI": - self.world.client_id = p.connect(p.DIRECT) - else: - self.world.client_id = p.connect(p.GUI) - - # Disable the side windows of the GUI - p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) - # Change the init camera pose - p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, - cameraTargetPosition=[-2, 0, 1]) - - # Get the initial camera target location - cameraTargetPosition = p.getDebugVisualizerCamera()[11] - - sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1]) - - # Create a sphere with a radius of 0.05 and a mass of 0 - sphereUid = p.createMultiBody(baseMass=0.0, - baseInertialFramePosition=[0, 0, 0], - baseVisualShapeIndex=sphereVisualId, - basePosition=cameraTargetPosition) - - # Define the maxSpeed, used in calculations - maxSpeed = 16 - - # Set initial Camera Rotation - cameraYaw = 50 - cameraPitch = -35 - - # Keep track of the mouse state - mouseState = [0, 0, 0] - oldMouseX, oldMouseY = 0, 0 - - # Determines if the sphere at cameraTargetPosition is visible - visible = 1 - - # Loop to update the camera position based on keyboard events - while p.isConnected(self.world.client_id): - # Monitor user input - keys = p.getKeyboardEvents() - mouse = p.getMouseEvents() - - # Get infos about the camera - width, height, dist = p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], \ - p.getDebugVisualizerCamera()[10] - cameraTargetPosition = p.getDebugVisualizerCamera()[11] - - # Get vectors used for movement on x,y,z Vector - xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] - yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] - zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] - - # Check the mouse state - if mouse: - for m in mouse: - - mouseX = m[2] - mouseY = m[1] - - # update mouseState - # Left Mouse button - if m[0] == 2 and m[3] == 0: - mouseState[0] = m[4] - # Middle mouse butto (scroll wheel) - if m[0] == 2 and m[3] == 1: - mouseState[1] = m[4] - # right mouse button - if m[0] == 2 and m[3] == 2: - mouseState[2] = m[4] - - # change visibility by clicking the mousewheel - if m[4] == 6 and m[3] == 1 and visible == 1: - visible = 0 - elif m[4] == 6 and visible == 0: - visible = 1 - - # camera movement when the left mouse button is pressed - if mouseState[0] == 3: - speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < maxSpeed else maxSpeed - speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < maxSpeed else maxSpeed - - # max angle of 89.5 and -89.5 to make sure the camera does not flip (is annoying) - if mouseX < oldMouseX: - if (cameraPitch + speedX) < 89.5: - cameraPitch += (speedX / 4) + 1 - elif mouseX > oldMouseX: - if (cameraPitch - speedX) > -89.5: - cameraPitch -= (speedX / 4) + 1 - - if mouseY < oldMouseY: - cameraYaw += (speedY / 4) + 1 - elif mouseY > oldMouseY: - cameraYaw -= (speedY / 4) + 1 - - if mouseState[1] == 3: - speedX = abs(oldMouseX - mouseX) - factor = 0.05 - - if mouseX < oldMouseX: - dist = dist - speedX * factor - elif mouseX > oldMouseX: - dist = dist + speedX * factor - dist = max(dist, 0.1) - - # camera movement when the right mouse button is pressed - if mouseState[2] == 3: - speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < 5 else 5 - speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < 5 else 5 - factor = 0.05 - - if mouseX < oldMouseX: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(zVec, factor), speedX)) - elif mouseX > oldMouseX: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(zVec, factor), speedX)) - - if mouseY < oldMouseY: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(xVec, factor), speedY)) - elif mouseY > oldMouseY: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(xVec, factor), speedY)) - # update oldMouse values - oldMouseY, oldMouseX = mouseY, mouseX - - # check the keyboard state - if keys: - # if shift is pressed, double the speed - if p.B3G_SHIFT in keys: - speedMult = 5 - else: - speedMult = 2.5 - - # if control is pressed, the movements caused by the arrowkeys, the '+' as well as the '-' key - # change - if p.B3G_CONTROL in keys: - - # the up and down arrowkeys cause the targetPos to move along the z axis of the map - if p.B3G_DOWN_ARROW in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(zVec, 0.03), speedMult)) - elif p.B3G_UP_ARROW in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(zVec, 0.03), speedMult)) - - # left and right arrowkeys cause the targetPos to move horizontally relative to the camera - if p.B3G_LEFT_ARROW in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(xVec, 0.03), speedMult)) - elif p.B3G_RIGHT_ARROW in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(xVec, 0.03), speedMult)) - - # the '+' and '-' keys cause the targetpos to move forwards and backwards relative to the camera - # while the camera stays at a constant distance. SHIFT + '=' is for US layout - if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(yVec, 0.03), speedMult)) - elif ord("-") in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(yVec, 0.03), speedMult)) - - # standard bindings for thearrowkeys, the '+' as well as the '-' key - else: - - # left and right arrowkeys cause the camera to rotate around the yaw axis - if p.B3G_RIGHT_ARROW in keys: - cameraYaw += (360 / width) * speedMult - elif p.B3G_LEFT_ARROW in keys: - cameraYaw -= (360 / width) * speedMult - - # the up and down arrowkeys cause the camera to rotate around the pitch axis - if p.B3G_DOWN_ARROW in keys: - if (cameraPitch + (360 / height) * speedMult) < 89.5: - cameraPitch += (360 / height) * speedMult - elif p.B3G_UP_ARROW in keys: - if (cameraPitch - (360 / height) * speedMult) > -89.5: - cameraPitch -= (360 / height) * speedMult - - # the '+' and '-' keys cause the camera to zoom towards and away from the targetPos without - # moving it. SHIFT + '=' is for US layout since the events can't handle shift plus something - if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: - if (dist - (dist * 0.02) * speedMult) > 0.1: - dist -= dist * 0.02 * speedMult - elif ord("-") in keys: - dist += dist * 0.02 * speedMult - - p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, - cameraTargetPosition=cameraTargetPosition) - if visible == 0: - cameraTargetPosition = (0.0, -50, 50) - p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) - time.sleep(1. / 80.) - - -class Object: - """ - Represents a spawned Object in the BulletWorld. - """ - - def __init__(self, name: str, type: Union[str, ObjectType], path: str, - pose: Pose = None, - world: BulletWorld = None, - color: Optional[List[float]] = [1, 1, 1, 1], - ignoreCachedFiles: Optional[bool] = False): - """ - The constructor loads the urdf file into the given BulletWorld, if no BulletWorld is specified the - :py:attr:`~BulletWorld.current_bullet_world` will be used. It is also possible to load .obj and .stl file into the BulletWorld. - The color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. - - :param name: The name of the object - :param type: The type of the object - :param path: The path to the source file, if only a filename is provided then the resourcer directories will be searched - :param pose: The pose at which the Object should be spawned - :param world: The BulletWorld in which the object should be spawned, if no world is specified the :py:attr:`~BulletWorld.current_bullet_world` will be used - :param color: The color with which the object should be spawned. - :param ignoreCachedFiles: If true the file will be spawned while ignoring cached files. - """ - if not pose: - pose = Pose() - self.world: BulletWorld = world if world is not None else BulletWorld.current_bullet_world - self.local_transformer = LocalTransformer() - self.name: str = name - self.type: str = type - self.color: List[float] = color - pose_in_map = self.local_transformer.transform_pose(pose, "map") - position, orientation = pose_in_map.to_list() - self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignoreCachedFiles) - self.joints: Dict[str, int] = self._joint_or_link_name_to_id("joint") - self.links: Dict[str, int] = self._joint_or_link_name_to_id("link") - self.attachments: Dict[Object, List] = {} - self.cids: Dict[Object, int] = {} - self.original_pose = pose_in_map - - self.tf_frame = ("shadow/" if self.world.is_shadow_world else "") + self.name + "_" + str(self.id) - - # This means "world" is not the shadow world since it has a reference to a shadow world - if self.world.shadow_world != None: - self.world.world_sync.add_obj_queue.put( - [name, type, path, position, orientation, self.world.shadow_world, color, self]) - - with open(self.path) as f: - self.urdf_object = URDF.from_xml_string(f.read()) - if self.urdf_object.name == robot_description.name and not BulletWorld.robot: - BulletWorld.robot = self - - self.links[self.urdf_object.get_root()] = -1 - - self._current_pose = pose_in_map - self._current_link_poses = {} - self._current_link_transforms = {} - self._current_joint_states = {} - self._init_current_joint_states() - self._update_link_poses() - - self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) - self.local_transformer.update_transforms_for_object(self) - self.link_to_geometry = self._get_geometry_for_link() - - self.world.objects.append(self) - - def __repr__(self): - skip_attr = ["links", "joints", "urdf_object", "attachments", "cids", "_current_link_poses", - "_current_link_transforms", "link_to_geometry"] - return self.__class__.__qualname__ + f"(" + ', \n'.join( - [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" - - def remove(self) -> None: - """ - Removes this object from the BulletWorld it currently resides in. - For the object to be removed it has to be detached from all objects it - is currently attached to. After this is done a call to PyBullet is done - to remove this Object from the simulation. - """ - for obj in self.attachments.keys(): - self.world.detach(self, obj) - self.world.objects.remove(self) - # This means the current world of the object is not the shadow world, since it - # has a reference to the shadow world - if self.world.shadow_world is not None: - self.world.world_sync.remove_obj_queue.put(self) - self.world.world_sync.remove_obj_queue.join() - self.world.remove_object(self.id) - if BulletWorld.robot == self: - BulletWorld.robot = None - - def detach_all(self) -> None: - """ - Detach all objects attached to this object. - """ - attachments = self.attachments.copy() - for att in attachments.keys(): - self.world.detach(self, att) - - def get_position(self) -> Pose: - """ - Returns the position of this Object as a list of xyz. - - :return: The current position of this object - """ - return self.get_pose().position - - def get_orientation(self) -> Quaternion: - """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. - - :return: A list of xyzw - """ - return self.get_pose().orientation - - def get_pose(self) -> Pose: - """ - Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. - - :return: The current pose of this object - """ - return self._current_pose - - def set_pose(self, pose: Pose, base: bool = False) -> None: - """ - Sets the Pose of the object. - - :param pose: New Pose for the object - :param base: If True places the object base instead of origin at the specified position and orientation - """ - pose_in_map = self.local_transformer.transform_pose(pose, "map") - position, orientation = pose_in_map.to_list() - if base: - position = np.array(position) + self.base_origin_shift - self.world.reset_object_base_pose(self, position, orientation) - self._current_pose = pose_in_map - self._update_link_poses() - self.world._set_attached_objects(self, [self]) - - @property - def pose(self) -> Pose: - """ - Property that returns the current position of this Object. - - :return: The position as a list of xyz - """ - return self.get_pose() - - @pose.setter - def pose(self, value: Pose) -> None: - """ - Sets the Pose of the Object to the given value. Function for attribute use. - - :param value: New Pose of the Object - """ - self.set_pose(value) - - def move_base_to_origin_pos(self) -> None: - """ - Move the object such that its base will be at the current origin position. - This is useful when placing objects on surfaces where you want the object base in contact with the surface. - """ - self.set_pose(self.get_pose(), base=True) - - def _calculate_transform(self, obj: Object, link: str = None) -> Transform: - """ - Calculates the transformation between another object and the given - link of this object. If no link is provided then the base position will be used. - - :param obj: The other object for which the transformation should be calculated - :param link: The optional link name - :return: The transformation from the link (or base position) to the other objects base position - """ - transform = self.local_transformer.transform_to_object_frame(obj.pose, self, link) - - return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) - - def set_position(self, position: Union[Pose, Point], base=False) -> None: - """ - Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position - instead of the origin in the center of the Object. The given position can either be a Pose, - in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. - - :param position: Target position as xyz. - :param base: If the bottom of the Object should be placed or the origin in the center. - """ - pose = Pose() - if isinstance(position, Pose): - target_position = position.position - pose.frame = position.frame - else: - target_position = position - - pose.pose.position = target_position - pose.pose.orientation = self.get_orientation() - self.set_pose(pose, base=base) - - def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: - """ - Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only - the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. - - :param orientation: Target orientation given as a list of xyzw. - """ - pose = Pose() - if isinstance(orientation, Pose): - target_orientation = orientation.orientation - pose.frame = orientation.frame - else: - target_orientation = orientation - - pose.pose.position = self.get_position() - pose.pose.orientation = target_orientation - self.set_pose(pose) - - def _joint_or_link_name_to_id(self, name_type: str) -> Dict[str, int]: - """ - Creates a dictionary which maps the link or joint name to the unique ids used by pybullet. - - :param name_type: Determines if the dictionary should be for joints or links - :return: A dictionary that maps joint or link names to unique ids - """ - n_joints = self.world.get_object_number_of_joints(self) - joint_name_to_id = {} - for i in range(0, n_joints): - _id = self.world.get_object_joint_id(self, i) - if name_type == "joint": - _name = self.world.get_object_joint_name(self, i) - else: - _name = self.world.get_object_link_name(self, i) - joint_name_to_id[_name] = _id - return joint_name_to_id - - def get_joint_id(self, name: str) -> int: - """ - Returns the unique id for a joint name. As used by PyBullet. - - :param name: The joint name - :return: The unique id - """ - return self.joints[name] - - def get_link_id(self, name: str) -> int: - """ - Returns a unique id for a link name. As used by PyBullet. - - :param name: The link name - :return: The unique id - """ - return self.links[name] - - def get_link_by_id(self, id: int) -> str: - """ - Returns the name of a link for a given unique PyBullet id - - :param id: PyBullet id for link - :return: The link name - """ - return dict(zip(self.links.values(), self.links.keys()))[id] - - def get_joint_by_id(self, joint_id: int) -> str: - """ - Returns the joint name for a unique PyBullet id - - :param joint_id: The Pybullet id of for joint - :return: The joint name - """ - return dict(zip(self.joints.values(), self.joints.keys()))[joint_id] - - def get_link_relative_to_other_link(self, source_frame: str, target_frame: str) -> Pose: - """ - Calculates the position of a link in the coordinate frame of another link. - - :param source_frame: The name of the source frame - :param target_frame: The name of the target frame - :return: The pose of the source frame in the target frame - """ - source_pose = self.get_link_pose(source_frame) - return self.local_transformer.transform_to_object_frame(source_pose, self, target_frame) - - def get_link_position(self, name: str) -> Point: - """ - Returns the position of a link of this Object. Position is returned as a list of xyz. - - :param name: The link name - :return: The link position as xyz - """ - return self.get_link_pose(name).position - - def get_link_orientation(self, name: str) -> Quaternion: - """ - Returns the orientation of a link of this Object. Orientation is returned as a quaternion. - - :param name: The name of the link - :return: The orientation of the link as a quaternion - """ - return self.get_link_pose(name).orientation - - def get_link_pose(self, name: str) -> Pose: - """ - Returns a Pose of the link corresponding to the given name. The returned Pose will be in world coordinate frame. - - :param name: Link name for which a Pose should be returned - :return: The pose of the link - """ - if name in self.links.keys() and self.links[name] == -1: - return self.get_pose() - return self._current_link_poses[name] - # return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6]) - - def set_joint_state(self, joint_name: str, joint_pose: float) -> None: - """ - Sets the state of the given joint to the given joint pose. If the pose is outside the joint limits, as stated - in the URDF, an error will be printed. However, the joint will be set either way. - - :param joint_name: The name of the joint - :param joint_pose: The target pose for this joint - """ - # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly - up_lim, low_lim = self.world.get_object_joint_limits(self, joint_name) - if low_lim > up_lim: - low_lim, up_lim = up_lim, low_lim - if not low_lim <= joint_pose <= up_lim: - logging.error( - f"The joint position has to be within the limits of the joint. The joint limits for {joint_name} are {low_lim} and {up_lim}") - logging.error(f"The given joint position was: {joint_pose}") - # Temporarily disabled because kdl outputs values exciting joint limits - # return - self.world.reset_joint_state(self, joint_name, joint_pose) - self._current_joint_states[joint_name] = joint_pose - # self.local_transformer.update_transforms_for_object(self) - self._update_link_poses() - self.world._set_attached_objects(self, [self]) - - def set_joint_states(self, joint_poses: dict) -> None: - """ - Sets the current state of multiple joints at once, this method should be preferred when setting multiple joints - at once instead of running :func:`~Object.set_joint_state` in a loop. - - :param joint_poses: - :return: - """ - for joint_name, joint_pose in joint_poses.items(): - self.world.reset_joint_state(self, joint_name, joint_pose) - self._current_joint_states[joint_name] = joint_pose - self._update_link_poses() - self.world._set_attached_objects(self, [self]) - - def get_joint_state(self, joint_name: str) -> float: - """ - Returns the joint state for the given joint name. - - :param joint_name: The name of the joint - :return: The current pose of the joint - """ - return self._current_joint_states[joint_name] - - def contact_points(self) -> List: - """ - Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ - - :return: A list of all contact points with other objects - """ - return self.world.get_object_contact_points(self) - - def contact_points_simulated(self) -> List: - """ - Returns a list of all contact points between this Object and other Objects after stepping the simulation once. - For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ - - :return: A list of contact points between this Object and other Objects - """ - s = self.world.save_state() - self.world.step() - contact_points = self.contact_points() - self.world.restore_state(*s) - return contact_points - - def update_joints_from_topic(self, topic_name: str) -> None: - """ - Updates the joints of this object with positions obtained from a topic with the message type JointState. - Joint names on the topic have to correspond to the joints of this object otherwise an error message will be logged. - - :param topic_name: Name of the topic with the joint states - """ - msg = rospy.wait_for_message(topic_name, JointState) - joint_names = msg.name - joint_positions = msg.position - if set(joint_names).issubset(self.joints.keys()): - for i in range(len(joint_names)): - self.set_joint_state(joint_names[i], joint_positions[i]) - else: - add_joints = set(joint_names) - set(self.joints.keys()) - rospy.logerr(f"There are joints in the published joint state which are not in this model: /n \ - The following joint{'s' if len(add_joints) != 1 else ''}: {add_joints}") - - def update_pose_from_tf(self, frame: str) -> None: - """ - Updates the pose of this object from a TF message. - - :param frame: Name of the TF frame from which the position should be taken - """ - tf_listener = tf.TransformListener() - time.sleep(0.5) - position, orientation = tf_listener.lookupTransform(frame, "map", rospy.Time(0)) - position = [position[0][0] * -1, - position[0][1] * -1, - position[0][2]] - self.set_position(Pose(position, orientation)) - - def set_color(self, color: List[float], link: Optional[str] = "") -> None: - """ - Changes the color of this object, the color has to be given as a list - of RGBA values. Optionally a link name can can be provided, if no link - name is provided all links of this object will be colored. - - :param color: The color as RGBA values between 0 and 1 - :param link: The link name of the link which should be colored - """ - self.world.set_object_color(self, color, link) - - def get_color(self, link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: - """ - This method returns the color of this object or a link of this object. If no link is given then the - return is either: - - 1. A list with the color as RGBA values, this is the case if the object only has one link (this - happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the color as value. The color is given as RGBA values. - Please keep in mind that not every link may have a color. This is dependent on the URDF from which the - object is spawned. - - If a link is specified then the return is a list with RGBA values representing the color of this link. - It may be that this link has no color, in this case the return is None as well as an error message. - - :param link: the link name for which the color should be returned. - :return: The color of the object or link, or a dictionary containing every colored link with its color - """ - return self.world.get_object_color(self, link) - - def get_AABB(self, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: - """ - Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case - the axis aligned bounding box of the link will be returned. The return of this method are two points in - world coordinate frame which define a bounding box. - - :param link_name: The Optional name of a link of this object. - :return: Two lists of x,y,z which define the bounding box. - """ - return self.world.get_object_AABB(self, link_name) - - def get_base_origin(self, link_name: Optional[str] = None) -> Pose: - """ - Returns the origin of the base/bottom of an object/link - - :param link_name: The link name for which the bottom position should be returned - :return: The position of the bottom of this Object or link - """ - aabb = self.get_AABB(link_name=link_name) - base_width = np.absolute(aabb[0][0] - aabb[1][0]) - base_length = np.absolute(aabb[0][1] - aabb[1][1]) - return Pose([aabb[0][0] + base_width / 2, aabb[0][1] + base_length / 2, aabb[0][2]], - self.get_pose().orientation_as_list()) - - def get_joint_limits(self, joint: str) -> Tuple[float, float]: - """ - Returns the lower and upper limit of a joint, if the lower limit is higher - than the upper they are swapped to ensure the lower limit is always the smaller one. - - :param joint: The name of the joint for which the limits should be found. - :return: The lower and upper limit of the joint. - """ - if joint not in self.joints.keys(): - raise KeyError(f"The given Joint: {joint} is not part of this object") - lower, upper = self.world.get_object_joint_limits(self, joint) - if lower > upper: - lower, upper = upper, lower - return lower, upper - - def get_joint_axis(self, joint_name: str) -> Tuple[float]: - """ - Returns the axis along which a joint is moving. The given joint_name has to be part of this object. - - :param joint_name: Name of the joint for which the axis should be returned. - :return: The axis a vector of xyz - """ - return self.world.get_object_joint_axis(self, joint_name) - - def get_joint_type(self, joint_name: str) -> JointType: - """ - Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - - :param joint_name: Joint name for which the type should be returned - :return: The type of the joint - """ - return self.world.get_object_joint_type(self, joint_name) - - def find_joint_above(self, link_name: str, joint_type: JointType) -> str: - """ - Traverses the chain from 'link_name' to the URDF origin and returns the first joint that is of type 'joint_type'. - - :param link_name: Link name above which the joint should be found - :param joint_type: Joint type that should be searched for - :return: Name of the first joint which has the given type - """ - chain = self.urdf_object.get_chain(self.urdf_object.get_root(), link_name) - reversed_chain = reversed(chain) - container_joint = None - for element in reversed_chain: - if element in self.joints and self.get_joint_type(element) == joint_type: - container_joint = element - break - if not container_joint: - rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") - return container_joint - - def get_complete_joint_state(self) -> Dict[str: float]: - """ - Returns the complete joint state of the object as a dictionary of joint names and joint values. - - :return: A dictionary with the complete joint state - """ - return self._current_joint_states - - def get_link_tf_frame(self, link_name: str) -> str: - """ - Returns the name of the tf frame for the given link name. This method does not check if the given name is - actually a link of this object. - - :param link_name: Name of a link for which the tf frame should be returned - :return: A TF frame name for a specific link - """ - return self.tf_frame + "/" + link_name - - def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]: - """ - Extracts the geometry information for each collision of each link and links them to the respective link. - - :return: A dictionary with link name as key and geometry information as value - """ - link_to_geometry = {} - for link in self.links.keys(): - link_obj = self.urdf_object.link_map[link] - if not link_obj.collision: - link_to_geometry[link] = None - else: - link_to_geometry[link] = link_obj.collision.geometry - return link_to_geometry - - def _update_link_poses(self) -> None: - """ - Updates the cached poses and transforms for each link of this Object - """ - for link_name in self.links.keys(): - if link_name == self.urdf_object.get_root(): - self._current_link_poses[link_name] = self._current_pose - self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame) - else: - self._current_link_poses[link_name] = Pose(*self.world.get_object_link_pose(self, link_name)) - self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( - self.get_link_tf_frame(link_name)) - - def _init_current_joint_states(self) -> None: - """ - Initialize the cached joint position for each joint. - """ - for joint_name in self.joints.keys(): - self._current_joint_states[joint_name] = self.world.get_object_joint_position(self, joint_name) - - -def filter_contact_points(contact_points, exclude_ids) -> List: - """ - Returns a list of contact points where Objects that are in the 'exclude_ids' list are removed. - - :param contact_points: A list of contact points - :param exclude_ids: A list of unique ids of Objects that should be removed from the list - :return: A list containing 'contact_points' without Objects that are in 'exclude_ids' - """ - return list(filter(lambda cp: cp[2] not in exclude_ids, contact_points)) - - -def get_path_from_data_dir(file_name: str, data_directory: str) -> str: - """ - Returns the full path for a given file name in the given directory. If there is no file with the given filename - this method returns None. - - :param file_name: The filename of the searched file. - :param data_directory: The directory in which to search for the file. - :return: The full path in the filesystem or None if there is no file with the filename in the directory - """ - dir = pathlib.Path(data_directory) - for file in os.listdir(data_directory): - if file == file_name: - return data_directory + f"/{file_name}" - - -def _get_robot_name_from_urdf(urdf_string: str) -> str: - """ - Extracts the robot name from the 'robot_name' tag of a URDF. - - :param urdf_string: The URDF as string. - :return: The name of the robot described by the URDF. - """ - res = re.findall(r"robot\ *name\ *=\ *\"\ *[a-zA-Z_0-9]*\ *\"", urdf_string) - if len(res) == 1: - begin = res[0].find("\"") - end = res[0][begin + 1:].find("\"") - robot = res[0][begin + 1:begin + 1 + end].lower() - return robot - - -def _load_object(name: str, - path: str, - position: List[float], - orientation: List[float], - world: BulletWorld, - color: List[float], - ignoreCachedFiles: bool) -> Tuple[int, str]: - """ - Loads an object to the given BulletWorld with the given position and orientation. The color will only be - used when an .obj or .stl file is given. - If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created - and this URDf file will be loaded instead. - When spawning a URDf file a new file will be created in the cache directory, if there exists none. - This new file will have resolved mesh file paths, meaning there will be no references - to ROS packges instead there will be absolute file paths. - - :param name: The name of the object which should be spawned - :param path: The path to the source file or the name on the ROS parameter server - :param position: The position in which the object should be spawned - :param orientation: The orientation in which the object should be spawned - :param world: The BulletWorld to which the Object should be spawned - :param color: The color of the object, only used when .obj or .stl file is given - :param ignoreCachedFiles: Whether to ignore files in the cache directory. - :return: The unique id of the object and the path to the file used for spawning - """ - pa = pathlib.Path(path) - extension = pa.suffix - world, world_id = _world_and_id(world) - if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): - for dir in world.data_directory: - path = get_path_from_data_dir(path, dir) - if path: break - - if not path: - raise FileNotFoundError( - f"File {pa.name} could not be found in the resource directory {world.data_directory}") - # rospack = rospkg.RosPack() - # cach_dir = rospack.get_path('pycram') + '/resources/cached/' - cach_dir = world.data_directory[0] + '/cached/' - if not pathlib.Path(cach_dir).exists(): - os.mkdir(cach_dir) - - # if file is not yet cached corrcet the urdf and save if in the cache directory - if not _is_cached(path, name, cach_dir) or ignoreCachedFiles: - if extension == ".obj" or extension == ".stl": - path = _generate_urdf_file(name, path, color, cach_dir) - elif extension == ".urdf": - with open(path, mode="r") as f: - urdf_string = fix_missing_inertial(f.read()) - urdf_string = remove_error_tags(urdf_string) - urdf_string = fix_link_attributes(urdf_string) - try: - urdf_string = _correct_urdf_string(urdf_string) - except rospkg.ResourceNotFound as e: - rospy.logerr(f"Could not find resource package linked in this URDF") - raise e - path = cach_dir + pa.name - with open(path, mode="w") as f: - f.write(urdf_string) - else: # Using the urdf from the parameter server - urdf_string = rospy.get_param(path) - path = cach_dir + name + ".urdf" - with open(path, mode="w") as f: - f.write(_correct_urdf_string(urdf_string)) - # save correct path in case the file is already in the cache directory - elif extension == ".obj" or extension == ".stl": - path = cach_dir + pa.stem + ".urdf" - elif extension == ".urdf": - path = cach_dir + pa.name - else: - path = cach_dir + name + ".urdf" - - try: - obj = p.loadURDF(path, basePosition=position, baseOrientation=orientation, physicsClientId=world_id) - return obj, path - except p.error as e: - logging.error( - "The File could not be loaded. Plese note that the path has to be either a URDF, stl or obj file or the name of an URDF string on the parameter server.") - os.remove(path) - raise (e) - # print(f"{bcolors.BOLD}{bcolors.WARNING}The path has to be either a path to a URDf file, stl file, obj file or the name of a URDF on the parameter server.{bcolors.ENDC}") - - -def _is_cached(path: str, name: str, cach_dir: str) -> bool: - """ - Checks if the file in the given path is already cached or if - there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from - the parameter server is used. - - :param path: The path given by the user to the source file. - :param name: The name for this object. - :param cach_dir: The absolute path the cach directory in the pycram package. - :return: True if there already exists a chached file, False in any other case. - """ - file_name = pathlib.Path(path).name - p = pathlib.Path(cach_dir + file_name) - if p.exists(): - return True - # Returns filename without the filetype, e.g. returns "test" for "test.txt" - file_stem = pathlib.Path(path).stem - p = pathlib.Path(cach_dir + file_stem + ".urdf") - if p.exists(): - return True - return False - - -def _correct_urdf_string(urdf_string: str) -> str: - """ - Changes paths for files in the URDF from ROS paths to paths in the file system. Since PyBullet can't deal with ROS - package paths. - - :param urdf_name: The name of the URDf on the parameter server - :return: The URDF string with paths in the filesystem instead of ROS packages - """ - r = rospkg.RosPack() - new_urdf_string = "" - for line in urdf_string.split('\n'): - if "package://" in line: - s = line.split('//') - s1 = s[1].split('/') - path = r.get_path(s1[0]) - line = line.replace("package://" + s1[0], path) - new_urdf_string += line + '\n' - - return fix_missing_inertial(new_urdf_string) - - -def fix_missing_inertial(urdf_string: str) -> str: - """ - Insert inertial tags for every URDF link that has no inertia. - This is used to prevent PyBullet from dumping warnings in the terminal - - :param urdf_string: The URDF description as string - :returns: The new, corrected URDF description as string. - """ - - inertia_tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.Element("inertial")) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("mass", {"value": "0.1"})) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("inertia", {"ixx": "0.01", - "ixy": "0", - "ixz": "0", - "iyy": "0.01", - "iyz": "0", - "izz": "0.01"})) - - # create tree from string - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) - - for link_element in tree.iter("link"): - inertial = [*link_element.iter("inertial")] - if len(inertial) == 0: - link_element.append(inertia_tree.getroot()) - - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') - - -def remove_error_tags(urdf_string: str) -> str: - """ - Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the - URDF_parser - - :param urdf_string: String of the URDF from which the tags should be removed - :return: The URDF string with the tags removed - """ - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) - removing_tags = ["gazebo", "transmission"] - for tag_name in removing_tags: - all_tags = tree.findall(tag_name) - for tag in all_tags: - tree.getroot().remove(tag) - - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') - - -def fix_link_attributes(urdf_string: str) -> str: - """ - Removes the attribute 'type' from links since this is not parsable by the URDF parser. - - :param urdf_string: The string of the URDF from which the attributes should be removed - :return: The URDF string with the attributes removed - """ - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) - - for link in tree.iter("link"): - if "type" in link.attrib.keys(): - del link.attrib["type"] - - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') - - -def _generate_urdf_file(name: str, path: str, color: List[float], cach_dir: str) -> str: - """ - Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given color will be - used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with the name - as filename. - - :param name: The name of the object - :param path: The path to the .obj or .stl file - :param color: The color which should be used for the material tag - :param cach_dir The absolute file path to the cach directory in the pycram package - :return: The absolute path of the created file - """ - urdf_template = ' \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - ' - urdf_template = fix_missing_inertial(urdf_template) - rgb = " ".join(list(map(str, color))) - pathlib_obj = pathlib.Path(path) - path = str(pathlib_obj.resolve()) - content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) - with open(cach_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: - file.write(content) - return cach_dir + pathlib_obj.stem + ".urdf" - - -def _world_and_id(world: BulletWorld) -> Tuple[BulletWorld, int]: - """ - Selects the world to be used. If the given world is None the 'current_bullet_world' is used. - - :param world: The world which should be used or None if 'current_bullet_world' should be used - :return: The BulletWorld object and the id of this BulletWorld - """ - world = world if world is not None else BulletWorld.current_bullet_world - id = world.client_id if world is not None else BulletWorld.current_bullet_world.client_id - return world, id diff --git a/src/pycram/bullet_world_reasoning.py b/src/pycram/world_reasoning.py similarity index 99% rename from src/pycram/bullet_world_reasoning.py rename to src/pycram/world_reasoning.py index 9a6c0ee94..23e0c8924 100644 --- a/src/pycram/bullet_world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -3,7 +3,7 @@ import numpy as np import rospy -from .bullet_world import _world_and_id, Object, Use_shadow_world, BulletWorld +from .world import _world_and_id, Object, Use_shadow_world, BulletWorld from .external_interfaces.ik import request_ik from .local_transformer import LocalTransformer from .plan_failures import IKError @@ -360,5 +360,5 @@ def link_pose_for_joint_config(object: Object, joint_config: Dict[str, float], l shadow_object = BulletWorld.current_bullet_world.get_shadow_object(object) with Use_shadow_world(): for joint, pose in joint_config.items(): - shadow_object.set_joint_state(joint, pose) + shadow_object.set_joint_position(joint, pose) return shadow_object.get_link_pose(link_name) diff --git a/test/local_transformer_tests.py b/test/local_transformer_tests.py index ee7375fcb..fc8c48f3e 100644 --- a/test/local_transformer_tests.py +++ b/test/local_transformer_tests.py @@ -3,7 +3,7 @@ from time import sleep -from src.pycram.bullet_world import BulletWorld, Object +from src.pycram.world import BulletWorld, Object from src.pycram.local_transformer import LocalTransformer from pycram.robot_descriptions import robot_description from pycram.pose import Pose, Transform diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 49c58c436..5c5b41b3b 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -18,7 +18,7 @@ def test_move_torso(self): self.assertEqual(description.ground().position, 0.3) with simulated_robot: description.resolve().perform() - self.assertEqual(self.world.robot.get_joint_state(robot_description.torso_joint), 0.3) + self.assertEqual(self.world.robot.get_joint_position(robot_description.torso_joint), 0.3) def test_set_gripper(self): description = action_designator.SetGripperAction(["left"], ["open", "close"]) @@ -28,7 +28,7 @@ def test_set_gripper(self): with simulated_robot: description.resolve().perform() for joint, state in robot_description.get_static_gripper_chain("left", "open").items(): - self.assertEqual(self.world.robot.get_joint_state(joint), state) + self.assertEqual(self.world.robot.get_joint_position(joint), state) def test_release(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) @@ -48,9 +48,9 @@ def test_park_arms(self): with simulated_robot: description.resolve().perform() for joint, pose in robot_description.get_static_joint_chain("right", "park").items(): - self.assertEqual(self.world.robot.get_joint_state(joint), pose) + self.assertEqual(self.world.robot.get_joint_position(joint), pose) for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): - self.assertEqual(self.world.robot.get_joint_state(joint), pose) + self.assertEqual(self.world.robot.get_joint_position(joint), pose) def test_navigate(self): description = action_designator.NavigateAction([Pose([0, 0, 0], [0, 0, 0, 1])]) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 6417e739c..fbe4d3605 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -1,7 +1,7 @@ import unittest import numpy as np -from pycram.bullet_world import BulletWorld, Object, fix_missing_inertial +from pycram.world import BulletWorld, Object, fix_missing_inertial from pycram.pose import Pose from pycram.robot_descriptions import robot_description from pycram.process_module import ProcessModule diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index da537fff3..085b15d47 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -1,7 +1,7 @@ import time from test_bullet_world import BulletWorldTest -import pycram.bullet_world_reasoning as btr +import pycram.world_reasoning as btr from pycram.pose import Pose from pycram.robot_descriptions import robot_description diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 585a77774..124057c4d 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -5,7 +5,7 @@ import sqlalchemy.orm import pycram.plan_failures -from pycram.bullet_world import BulletWorld, Object +from pycram.world import BulletWorld, Object from pycram.designators import action_designator, object_designator from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index 98884a0d7..5a9101ebe 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -5,7 +5,7 @@ import requests import pycram.plan_failures -from pycram.bullet_world import BulletWorld, Object +from pycram.world import BulletWorld, Object from pycram.designators import action_designator, object_designator from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot diff --git a/test/test_location_designator.py b/test/test_location_designator.py index ab94ec6bb..f4a9fdb74 100644 --- a/test/test_location_designator.py +++ b/test/test_location_designator.py @@ -11,7 +11,7 @@ class TestActionDesignatorGrounding(test_bullet_world.BulletWorldTest): def test_reachability(self): - self.robot.set_joint_state(robot_description.torso_joint, 0.3) + self.robot.set_joint_position(robot_description.torso_joint, 0.3) object_desig = ObjectDesignatorDescription(names=["milk"]) robot_desig = ObjectDesignatorDescription(names=[robot_description.name]) location_desig = CostmapLocation(object_desig.resolve(), reachable_for=robot_desig.resolve()) @@ -37,7 +37,7 @@ def test_visibility(self): self.assertTrue(len(location.pose.orientation_as_list()) == 4) def test_reachability_and_visibility(self): - self.robot.set_joint_state(robot_description.torso_joint, 0.3) + self.robot.set_joint_position(robot_description.torso_joint, 0.3) object_desig = ObjectDesignatorDescription(names=["milk"]) robot_desig = ObjectDesignatorDescription(names=[robot_description.name]) location_desig = CostmapLocation(object_desig.resolve(), reachable_for=robot_desig.resolve(), visible_for=robot_desig.resolve()) From 40d2d47c6ff418f1e3bc0eb4ea113e5196e48d25 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 30 Nov 2023 12:23:56 +0100 Subject: [PATCH 03/69] [RefactoringObjectClass] Now object class is independent of pybullet, and the tests are passing --- src/pycram/world.py | 1907 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1907 insertions(+) create mode 100644 src/pycram/world.py diff --git a/src/pycram/world.py b/src/pycram/world.py new file mode 100644 index 000000000..695900c90 --- /dev/null +++ b/src/pycram/world.py @@ -0,0 +1,1907 @@ +# used for delayed evaluation of typing until python 3.11 becomes mainstream +from __future__ import annotations + +import logging +import os +import pathlib +import re +import threading +import time +import xml.etree.ElementTree +from queue import Queue +import tf +from typing import List, Optional, Dict, Tuple, Callable +from typing import Union + +import numpy as np +import pybullet as p +import rospkg +import rospy +import rosgraph + +import urdf_parser_py.urdf +from geometry_msgs.msg import Quaternion, Point +from urdf_parser_py.urdf import URDF + +from .event import Event +from .robot_descriptions import robot_description +from .enums import JointType, ObjectType +from .local_transformer import LocalTransformer +from sensor_msgs.msg import JointState + +from .pose import Pose, Transform + + +class BulletWorld: + """ + The BulletWorld Class represents the physics Simulation and belief state. + """ + + current_bullet_world: BulletWorld = None + """ + Global reference to the currently used BulletWorld, usually this is the + graphical one. However, if you are inside a Use_shadow_world() environment the current_bullet_world points to the + shadow world. In this way you can comfortably use the current_bullet_world, which should point towards the BulletWorld + used at the moment. + """ + robot: Object = None + """ + Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the + URDF with the name of the URDF on the parameter server. + """ + + # Check is for sphinx autoAPI to be able to work in a CI workflow + if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): + rospy.init_node('pycram') + + def __init__(self, type: str = "GUI", is_shadow_world: bool = False): + """ + Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the + background. There can only be one rendered simulation. + The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. + + :param type: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" + :param is_shadow_world: For internal usage, decides if this BulletWorld should be used as a shadow world. + """ + self.objects: List[Object] = [] + self.client_id: int = -1 + self.detachment_event: Event = Event() + self.attachment_event: Event = Event() + self.manipulation_event: Event = Event() + self.type: str = type + self._gui_thread: Gui = Gui(self, type) + self._gui_thread.start() + # This disables file caching from PyBullet, since this would also cache + # files that can not be loaded + p.setPhysicsEngineParameter(enableFileCaching=0) + # Needed to let the other thread start the simulation, before Objects are spawned. + time.sleep(0.1) + if BulletWorld.current_bullet_world == None: + BulletWorld.current_bullet_world = self + self.vis_axis: List[Object] = [] + self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} + self.data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] + self.shadow_world: BulletWorld = BulletWorld("DIRECT", True) if not is_shadow_world else None + self.world_sync: WorldSync = WorldSync(self, self.shadow_world) if not is_shadow_world else None + self.is_shadow_world: bool = is_shadow_world + self.local_transformer = LocalTransformer() + if not is_shadow_world: + self.world_sync.start() + self.local_transformer.bullet_world = self + self.local_transformer.shadow_world = self.shadow_world + + # Some default settings + self.set_gravity([0, 0, -9.8]) + if not is_shadow_world: + plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) + # atexit.register(self.exit) + + def get_objects_by_name(self, name: str) -> List[Object]: + """ + Returns a list of all Objects in this BulletWorld with the same name as the given one. + + :param name: The name of the returned Objects. + :return: A list of all Objects with the name 'name'. + """ + return list(filter(lambda obj: obj.name == name, self.objects)) + + def get_objects_by_type(self, obj_type: str) -> List[Object]: + """ + Returns a list of all Objects which have the type 'obj_type'. + + :param obj_type: The type of the returned Objects. + :return: A list of all Objects that have the type 'obj_type'. + """ + return list(filter(lambda obj: obj.type == obj_type, self.objects)) + + def get_object_by_id(self, id: int) -> Object: + """ + Returns the single Object that has the unique id. + + :param id: The unique id for which the Object should be returned. + :return: The Object with the id 'id'. + """ + return list(filter(lambda obj: obj.id == id, self.objects))[0] + + def remove_object(self, obj_id: int) -> None: + """ + Remove an object by its ID. + + :param obj_id: The unique id of the object to be removed. + """ + + p.removeBody(obj_id, self.client_id) + + def attach_objects(self, + parent_obj: Object, + child_obj: Object, + parent_link: Optional[str] = None, + loose: Optional[bool] = False) -> None: + """ + Attaches another object to this object. This is done by + saving the transformation between the given link, if there is one, and + the base pose of the other object. Additionally, the name of the link, to + which the object is attached, will be saved. + Furthermore, a constraint of pybullet will be created so the attachment + also works while simulation. + Loose attachments means that the attachment will only be one-directional. + For example, if the parent object moves, the child object will also move, but not the other way around. + + :param parent_obj: The parent object (jf loose, then this would not move when child moves) + :param child_obj: The child object + :param parent_link: The link of the parent object to which the child object should be attached + :param loose: If the attachment should be a loose attachment. + """ + link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 + link_to_object = parent_obj._calculate_transform(child_obj, parent_link) + parent_obj.attachments[child_obj] = [link_to_object, parent_link, loose] + child_obj.attachments[parent_obj] = [link_to_object.invert(), None, False] + + cid = p.createConstraint(parent_obj.id, link_id, child_obj.id, -1, p.JOINT_FIXED, + [0, 1, 0], link_to_object.translation_as_list(), [0, 0, 0], + link_to_object.rotation_as_list(), + physicsClientId=self.client_id) + parent_obj.cids[child_obj] = cid + child_obj.cids[parent_obj] = cid + self.attachment_event(parent_obj, [parent_obj, child_obj]) + + def detach_objects(self, obj1: Object, obj2: Object) -> None: + """ + Detaches obj2 from obj1. This is done by + deleting the attachment from the attachments dictionary of both objects + and deleting the constraint of pybullet. + Afterward the detachment event of the corresponding BulletWorld will be fired. + + :param obj1: The object from which an object should be detached. + :param obj2: The object which should be detached. + """ + del obj1.attachments[obj2] + del obj2.attachments[obj1] + + p.removeConstraint(obj1.cids[obj2], physicsClientId=self.client_id) + + del obj1.cids[obj2] + del obj2.cids[obj1] + obj1.world.detachment_event(obj1, [obj1, obj2]) + + def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: + """ + Updates the positions of all attached objects. This is done + by calculating the new pose in world coordinate frame and setting the + base pose of the attached objects to this new pose. + After this the _set_attached_objects method of all attached objects + will be called. + + :param prev_object: A list of Objects that were already moved, + these will be excluded to prevent loops in the update. + """ + for att_obj in obj.attachments: + if att_obj in prev_object: + continue + if obj.attachments[att_obj][2]: + # Updates the attachment transformation and constraint if the + # attachment is loose, instead of updating the position of all attached objects + link_to_object = obj._calculate_transform(att_obj, obj.attachments[att_obj][1]) + link_id = obj.get_link_id(obj.attachments[att_obj][1]) if obj.attachments[att_obj][1] else -1 + obj.attachments[att_obj][0] = link_to_object + att_obj.attachments[obj][0] = link_to_object.invert() + p.removeConstraint(obj.cids[att_obj], physicsClientId=self.client_id) + cid = p.createConstraint(obj.id, link_id, att_obj.id, -1, p.JOINT_FIXED, [0, 0, 0], + link_to_object.translation_as_list(), + [0, 0, 0], link_to_object.rotation_as_list(), + physicsClientId=self.client_id) + obj.cids[att_obj] = cid + att_obj.cids[obj] = cid + else: + link_to_object = obj.attachments[att_obj][0] + + world_to_object = obj.local_transformer.transform_pose(link_to_object.to_pose(), "map") + p.resetBasePositionAndOrientation(att_obj.id, world_to_object.position_as_list(), + world_to_object.orientation_as_list(), + physicsClientId=self.client_id) + att_obj._current_pose = world_to_object + self._set_attached_objects(att_obj, prev_object + [obj]) + + def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: + """ + Get the joint limits of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + :return: A tuple containing the upper and the lower limits of the joint. + """ + up_lim, low_lim = p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8:10] + return up_lim, low_lim + + def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: + """ + Returns the axis along which a joint is moving. The given joint_name has to be part of this object. + + :param obj: The object + :param joint_name: Name of the joint for which the axis should be returned. + :return: The axis a vector of xyz + """ + return p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[13] + + def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: + """ + Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. + + :param obj: The object + :param joint_name: Joint name for which the type should be returned + :return: The type of the joint + """ + joint_type = p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[2] + return JointType(joint_type) + + def get_object_joint_position(self, obj: Object, joint_name: str) -> float: + """ + Get the state of a joint of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + """ + return p.getJointState(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[0] + + def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: + """ + Get the pose of a link of an articulated object with respect to the world frame. + The pose is given as a tuple of position and orientation. + + :param obj: The object + :param link_name: The name of the link + """ + return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] + + def get_object_contact_points(self, obj: Object) -> List: + """ + Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned + list please look at `PyBullet Doc `_ + + :param obj: The object. + :return: A list of all contact points with other objects + """ + return p.getContactPoints(obj.id) + + def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: + """ + Get the ID of a joint in an articulated object. + + :param obj: The object + :param joint_idx: The index of the joint (would indicate order). + """ + return p.getJointInfo(obj.id, joint_idx, self.client_id)[0] + + def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: + """ + Get the name of a joint in an articulated object. + + :param obj: The object + :param joint_idx: The index of the joint (would indicate order). + """ + return p.getJointInfo(obj.id, joint_idx, self.client_id)[1].decode('utf-8') + + def get_object_link_name(self, obj: Object, link_idx: int) -> str: + """ + Get the name of a link in an articulated object. + + :param obj: The object + :param link_idx: The index of the link (would indicate order). + """ + return p.getJointInfo(obj.id, link_idx, self.client_id)[12].decode('utf-8') + + def get_object_number_of_joints(self, obj: Object) -> int: + """ + Get the number of joints of an articulated object + + :param obj: The object + """ + return p.getNumJoints(obj.id, self.client_id) + + def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + """ + Reset the joint position instantly without physics simulation + + :param obj: The object + :param joint_name: The name of the joint + :param joint_pose: The new joint pose + """ + p.resetJointState(obj.id, obj.joints[joint_name], joint_pose, physicsClientId=self.client_id) + + def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): + """ + 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 position: The new position of the object as a vector of x,y,z + :param orientation: The new orientation of the object as a quaternion of x,y,z,w + """ + p.resetBasePositionAndOrientation(obj.id, position, orientation, self.client_id) + + def step(self): + """ + Step the world simulation using forward dynamics + """ + p.stepSimulation(self.client_id) + + def set_object_color(self, obj: Object, color: List[float], link: Optional[str] = ""): + """ + Changes the color of this object, the color has to be given as a list + of RGBA values. Optionally a link name can can be provided, if no link + name is provided all links of this object will be colored. + + :param obj: The object which should be colored + :param color: The color as RGBA values between 0 and 1 + :param link: The link name of the link which should be colored + """ + if link == "": + # Check if there is only one link, this is the case for primitive + # forms or if loaded from an .stl or .obj file + if obj.links != {}: + for link_id in obj.links.values(): + p.changeVisualShape(obj.id, link_id, rgbaColor=color, physicsClientId=self.client_id) + else: + p.changeVisualShape(obj.id, -1, rgbaColor=color, physicsClientId=self.client_id) + else: + p.changeVisualShape(obj.id, obj.links[link], rgbaColor=color, physicsClientId=self.client_id) + + def get_object_color(self, + obj: Object, + link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: + """ + This method returns the color of this object or a link of this object. If no link is given then the + return is either: + + 1. A list with the color as RGBA values, this is the case if the object only has one link (this + happens for example if the object is spawned from a .obj or .stl file) + 2. A dict with the link name as key and the color as value. The color is given as RGBA values. + Please keep in mind that not every link may have a color. This is dependent on the URDF from which the + object is spawned. + + If a link is specified then the return is a list with RGBA values representing the color of this link. + It may be that this link has no color, in this case the return is None as well as an error message. + + :param obj: The object for which the color should be returned. + :param link: the link name for which the color should be returned. + :return: The color of the object or link, or a dictionary containing every colored link with its color + """ + visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) + swap = {v: k for k, v in obj.links.items()} + links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) + colors = list(map(lambda x: x[7], visual_data)) + link_to_color = dict(zip(links, colors)) + + if link: + if link in link_to_color.keys(): + return link_to_color[link] + elif link not in obj.links.keys(): + rospy.logerr(f"The link '{link}' is not part of this obejct") + return None + else: + rospy.logerr(f"The link '{link}' has no color") + return None + + if len(visual_data) == 1: + return list(visual_data[0][7]) + else: + return link_to_color + + def get_object_AABB(self, obj: Object, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case + the axis aligned bounding box of the link will be returned. The return of this method are two points in + world coordinate frame which define a bounding box. + + :param obj: The object for which the bounding box should be returned. + :param link_name: The Optional name of a link of this object. + :return: Two lists of x,y,z which define the bounding box. + """ + if link_name: + return p.getAABB(obj.id, obj.links[link_name], self.client_id) + else: + return p.getAABB(obj.id, physicsClientId=self.client_id) + + def get_attachment_event(self) -> Event: + """ + Returns the event reference that is fired if an attachment occurs. + + :return: The reference to the attachment event + """ + return self.attachment_event + + def get_detachment_event(self) -> Event: + """ + Returns the event reference that is fired if a detachment occurs. + + :return: The event reference for the detachment event. + """ + return self.detachment_event + + def get_manipulation_event(self) -> Event: + """ + Returns the event reference that is fired if any manipulation occurs. + + :return: The event reference for the manipulation event. + """ + return self.manipulation_event + + def set_realtime(self, real_time: bool) -> None: + """ + Enables the real time simulation of Physic in the BulletWorld. By default this is disabled and Physic is only + simulated to reason about it. + + :param real_time: Whether the BulletWorld should simulate Physic in real time. + """ + p.setRealTimeSimulation(1 if real_time else 0, self.client_id) + + def set_gravity(self, velocity: List[float]) -> None: + """ + Sets the gravity that is used in the BullteWorld, by default the is the gravity on earth ([0, 0, -9.8]). Gravity + is given as a vector in x,y,z. Gravity is only applied while simulating Physic. + + :param velocity: The gravity vector that should be used in the BulletWorld. + """ + p.setGravity(velocity[0], velocity[1], velocity[2], physicsClientId=self.client_id) + + def set_robot(self, robot: Object) -> None: + """ + Sets the global variable for the robot Object. This should be set on spawning the robot. + + :param robot: The Object reference to the Object representing the robot. + """ + BulletWorld.robot = robot + + def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: + """ + Simulates Physic in the BulletWorld for a given amount of seconds. Usually this simulation is faster than real + time, meaning you can simulate for example 10 seconds of Physic in the BulletWorld in 1 second real time. By + setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real + time. + + :param seconds: The amount of seconds that should be simulated. + :param real_time: If the simulation should happen in real time or faster. + """ + for i in range(0, int(seconds * 240)): + p.stepSimulation(self.client_id) + for objects, callback in self.coll_callbacks.items(): + contact_points = p.getContactPoints(objects[0].id, objects[1].id, physicsClientId=self.client_id) + # contact_points = p.getClosestPoints(objects[0].id, objects[1].id, 0.02) + # print(contact_points[0][5]) + if contact_points != (): + callback[0]() + elif callback[1] != None: # Call no collision callback + callback[1]() + if real_time: + # Simulation runs at 240 Hz + time.sleep(0.004167) + + def exit(self) -> None: + """ + Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the + preferred method to close the BulletWorld. + """ + # True if this is NOT the shadow world since it has a reference to the + # Shadow world + time.sleep(0.1) + if self.shadow_world: + self.world_sync.terminate = True + self.world_sync.join() + self.shadow_world.exit() + p.disconnect(self.client_id) + if self._gui_thread: + self._gui_thread.join() + if BulletWorld.current_bullet_world == self: + BulletWorld.current_bullet_world = None + BulletWorld.robot = None + + def save_state(self) -> Tuple[int, Dict]: + """ + Returns the id of the saved state of the BulletWorld. The saved state contains the position, orientation and joint + position of every Object in the BulletWorld. + + :return: A unique id of the state + """ + objects2attached = {} + # ToDo find out what this is for and where it is used + for o in self.objects: + objects2attached[o] = (o.attachments.copy(), o.cids.copy()) + return p.saveState(self.client_id), objects2attached + + def restore_state(self, state, objects2attached: Optional[Dict] = None) -> None: + """ + Restores the state of the BulletWorld according to the given state id. This includes position, orientation and + joint states. However, restore can not respawn objects if there are objects that were deleted between creation of + the state and restoring they will be skiped. + + :param state: The unique id representing the state, as returned by :func:`~save_state` + :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~bullet_world.Object.attachments` + """ + p.restoreState(state, physicsClientId=self.client_id) + objects2attached = {} if objects2attached is None else objects2attached + for obj in self.objects: + try: + obj.attachments, obj.cids = objects2attached[obj] + except KeyError: + continue + + def copy(self) -> BulletWorld: + """ + Copies this Bullet World into another and returns it. The other BulletWorld + will be in Direct mode. The shadow world should always be preferred instead of creating a new BulletWorld. + This method should only be used if necessary since there can be unforeseen problems. + + :return: The reference to the new BulletWorld + """ + world = BulletWorld("DIRECT") + for obj in self.objects: + o = Object(obj.name, obj.type, obj.path, Pose(obj.get_position(), obj.get_orientation()), + world, obj.color) + for joint in obj.joints: + o.set_joint_position(joint, obj.get_joint_position(joint)) + return world + + def add_vis_axis(self, pose: Pose, + length: Optional[float] = 0.2) -> None: + """ + Creates a Visual object which represents the coordinate frame at the given + position and orientation. There can be an unlimited amount of vis axis objects. + + :param pose: The pose at which the axis should be spawned + :param length: Optional parameter to configure the length of the axes + """ + + position, orientation = pose.to_list() + + vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], + rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) + vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], + rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) + vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], + rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) + + obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], + basePosition=position, baseOrientation=orientation, + linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], + linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], + linkParentIndices=[0, 0, 0], + linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], + linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], + linkCollisionShapeIndices=[-1, -1, -1]) + + self.vis_axis.append(obj) + + def remove_vis_axis(self) -> None: + """ + Removes all spawned vis axis objects that are currently in this BulletWorld. + """ + for id in self.vis_axis: + p.removeBody(id) + self.vis_axis = [] + + def register_collision_callback(self, objectA: Object, objectB: Object, + callback_collision: Callable, + callback_no_collision: Optional[Callable] = None) -> None: + """ + Registers callback methods for contact between two Objects. There can be a callback for when the two Objects + get in contact and, optionally, for when they are not in contact anymore. + + :param objectA: An object in the BulletWorld + :param objectB: Another object in the BulletWorld + :param callback_collision: A function that should be called if the objects are in contact + :param callback_no_collision: A function that should be called if the objects are not in contact + """ + self.coll_callbacks[(objectA, objectB)] = (callback_collision, callback_no_collision) + + def add_additional_resource_path(self, path: str) -> None: + """ + Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an + Object is spawned only with a filename. + + :param path: A path in the filesystem in which to search for files. + """ + self.data_directory.append(path) + + def get_shadow_object(self, object: Object) -> Object: + """ + Returns the corresponding object from the shadow world for the given object. If the given Object is already in + the shadow world it is returned. + + :param object: The object for which the shadow worlds object should be returned. + :return: The corresponding object in the shadow world. + """ + try: + return self.world_sync.object_mapping[object] + except KeyError: + shadow_world = self if self.is_shadow_world else self.shadow_world + if object in shadow_world.objects: + return object + else: + raise ValueError( + f"There is no shadow object for the given object: {object}, this could be the case if the object isn't anymore in the main (graphical) BulletWorld or if the given object is already a shadow object. ") + + def get_bullet_object_for_shadow(self, object: Object) -> Object: + """ + Returns the corresponding object from the main Bullet World for a given + object in the shadow world. If the given object is not in the shadow + world an error will be raised. + + :param object: The object for which the corresponding object in the main Bullet World should be found + :return: The object in the main Bullet World + """ + map = self.world_sync.object_mapping + try: + return list(map.keys())[list(map.values()).index(object)] + except ValueError: + raise ValueError("The given object is not in the shadow world.") + + def reset_bullet_world(self) -> None: + """ + Resets the BulletWorld to the state it was first spawned in. + All attached objects will be detached, all joints will be set to the + default position of 0 and all objects will be set to the position and + orientation in which they were spawned. + """ + for obj in self.objects: + if obj.attachments: + attached_objects = list(obj.attachments.keys()) + for att_obj in attached_objects: + obj.detach(att_obj) + joint_names = list(obj.joints.keys()) + joint_poses = [0 for j in joint_names] + obj.set_positions_of_all_joints(dict(zip(joint_names, joint_poses))) + obj.set_pose(obj.original_pose) + + +class Use_shadow_world(): + """ + An environment for using the shadow world, while in this environment the :py:attr:`~BulletWorld.current_bullet_world` + variable will point to the shadow world. + + Example: + with Use_shadow_world(): + NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() + """ + + def __init__(self): + self.prev_world: BulletWorld = None + + def __enter__(self): + if not BulletWorld.current_bullet_world.is_shadow_world: + time.sleep(20 / 240) + # blocks until the adding queue is ready + BulletWorld.current_bullet_world.world_sync.add_obj_queue.join() + # **This is currently not used since the sleep(20/240) seems to be enough, but on weaker hardware this might + # not be a feasible solution** + # while not BulletWorld.current_bullet_world.world_sync.equal_states: + # time.sleep(0.1) + + self.prev_world = BulletWorld.current_bullet_world + BulletWorld.current_bullet_world.world_sync.pause_sync = True + BulletWorld.current_bullet_world = BulletWorld.current_bullet_world.shadow_world + + def __exit__(self, *args): + if not self.prev_world == None: + BulletWorld.current_bullet_world = self.prev_world + BulletWorld.current_bullet_world.world_sync.pause_sync = False + + +class WorldSync(threading.Thread): + """ + Synchronizes the state between the BulletWorld and its shadow world. + Meaning the cartesian and joint position of everything in the shadow world will be + synchronized with the main BulletWorld. + Adding and removing objects is done via queues, such that loading times of objects + in the shadow world does not affect the BulletWorld. + The class provides the possibility to pause the synchronization, this can be used + if reasoning should be done in the shadow world. + """ + + def __init__(self, world: BulletWorld, shadow_world: BulletWorld): + threading.Thread.__init__(self) + self.world: BulletWorld = world + self.shadow_world: BulletWorld = shadow_world + self.shadow_world.world_sync: WorldSync = self + + self.terminate: bool = False + self.add_obj_queue: Queue = Queue() + self.remove_obj_queue: Queue = Queue() + self.pause_sync: bool = False + # Maps bullet to shadow world objects + self.object_mapping: Dict[Object, Object] = {} + self.equal_states = False + + def run(self): + """ + Main method of the synchronization, this thread runs in a loop until the + terminate flag is set. + While this loop runs it continuously checks the cartesian and joint position of + every object in the BulletWorld and updates the corresponding object in the + shadow world. When there are entries in the adding or removing queue the corresponding objects will be added + or removed in the same iteration. + """ + while not self.terminate: + self.check_for_pause() + # self.equal_states = False + for i in range(self.add_obj_queue.qsize()): + obj = self.add_obj_queue.get() + # [name, type, path, position, orientation, self.world.shadow_world, color, bulletworld object] + o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) + # Maps the BulletWorld object to the shadow world object + self.object_mapping[obj[7]] = o + self.add_obj_queue.task_done() + for i in range(self.remove_obj_queue.qsize()): + obj = self.remove_obj_queue.get() + # Get shadow world object reference from object mapping + shadow_obj = self.object_mapping[obj] + shadow_obj.remove() + del self.object_mapping[obj] + self.remove_obj_queue.task_done() + + for bulletworld_obj, shadow_obj in self.object_mapping.items(): + b_pose = bulletworld_obj.get_pose() + s_pose = shadow_obj.get_pose() + if b_pose.dist(s_pose) != 0.0: + shadow_obj.set_pose(bulletworld_obj.get_pose()) + + # Manage joint positions + if len(bulletworld_obj.joints) > 2: + for joint_name in bulletworld_obj.joints.keys(): + if shadow_obj.get_joint_position(joint_name) != bulletworld_obj.get_joint_position(joint_name): + shadow_obj.set_positions_of_all_joints(bulletworld_obj.get_positions_of_all_joints()) + break + + self.check_for_pause() + # self.check_for_equal() + time.sleep(1 / 240) + + self.add_obj_queue.join() + self.remove_obj_queue.join() + + def check_for_pause(self) -> None: + """ + Checks if :py:attr:`~self.pause_sync` is true and sleeps this thread until it isn't anymore. + """ + while self.pause_sync: + time.sleep(0.1) + + def check_for_equal(self) -> None: + """ + Checks if both BulletWorlds have the same state, meaning all objects are in the same position. + This is currently not used, but might be used in the future if synchronization issues worsen. + """ + eql = True + for obj, shadow_obj in self.object_mapping.items(): + eql = eql and obj.get_pose() == shadow_obj.get_pose() + self.equal_states = eql + + +class Gui(threading.Thread): + """ + For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~BulletWorld.exit` + Also contains the code for controlling the camera. + """ + + def __init__(self, world, type): + threading.Thread.__init__(self) + self.world: BulletWorld = world + self.type: str = type + + def run(self): + """ + Initializes the new simulation and checks in an endless loop + if it is still active. If it is the thread will be suspended for 1/80 seconds, if it is not the method and + thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. + """ + if self.type != "GUI": + self.world.client_id = p.connect(p.DIRECT) + else: + self.world.client_id = p.connect(p.GUI) + + # Disable the side windows of the GUI + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + # Change the init camera pose + p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, + cameraTargetPosition=[-2, 0, 1]) + + # Get the initial camera target location + cameraTargetPosition = p.getDebugVisualizerCamera()[11] + + sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1]) + + # Create a sphere with a radius of 0.05 and a mass of 0 + sphereUid = p.createMultiBody(baseMass=0.0, + baseInertialFramePosition=[0, 0, 0], + baseVisualShapeIndex=sphereVisualId, + basePosition=cameraTargetPosition) + + # Define the maxSpeed, used in calculations + maxSpeed = 16 + + # Set initial Camera Rotation + cameraYaw = 50 + cameraPitch = -35 + + # Keep track of the mouse state + mouseState = [0, 0, 0] + oldMouseX, oldMouseY = 0, 0 + + # Determines if the sphere at cameraTargetPosition is visible + visible = 1 + + # Loop to update the camera position based on keyboard events + while p.isConnected(self.world.client_id): + # Monitor user input + keys = p.getKeyboardEvents() + mouse = p.getMouseEvents() + + # Get infos about the camera + width, height, dist = p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], \ + p.getDebugVisualizerCamera()[10] + cameraTargetPosition = p.getDebugVisualizerCamera()[11] + + # Get vectors used for movement on x,y,z Vector + xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] + yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] + zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] + + # Check the mouse state + if mouse: + for m in mouse: + + mouseX = m[2] + mouseY = m[1] + + # update mouseState + # Left Mouse button + if m[0] == 2 and m[3] == 0: + mouseState[0] = m[4] + # Middle mouse butto (scroll wheel) + if m[0] == 2 and m[3] == 1: + mouseState[1] = m[4] + # right mouse button + if m[0] == 2 and m[3] == 2: + mouseState[2] = m[4] + + # change visibility by clicking the mousewheel + if m[4] == 6 and m[3] == 1 and visible == 1: + visible = 0 + elif m[4] == 6 and visible == 0: + visible = 1 + + # camera movement when the left mouse button is pressed + if mouseState[0] == 3: + speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < maxSpeed else maxSpeed + speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < maxSpeed else maxSpeed + + # max angle of 89.5 and -89.5 to make sure the camera does not flip (is annoying) + if mouseX < oldMouseX: + if (cameraPitch + speedX) < 89.5: + cameraPitch += (speedX / 4) + 1 + elif mouseX > oldMouseX: + if (cameraPitch - speedX) > -89.5: + cameraPitch -= (speedX / 4) + 1 + + if mouseY < oldMouseY: + cameraYaw += (speedY / 4) + 1 + elif mouseY > oldMouseY: + cameraYaw -= (speedY / 4) + 1 + + if mouseState[1] == 3: + speedX = abs(oldMouseX - mouseX) + factor = 0.05 + + if mouseX < oldMouseX: + dist = dist - speedX * factor + elif mouseX > oldMouseX: + dist = dist + speedX * factor + dist = max(dist, 0.1) + + # camera movement when the right mouse button is pressed + if mouseState[2] == 3: + speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < 5 else 5 + speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < 5 else 5 + factor = 0.05 + + if mouseX < oldMouseX: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(zVec, factor), speedX)) + elif mouseX > oldMouseX: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(zVec, factor), speedX)) + + if mouseY < oldMouseY: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(xVec, factor), speedY)) + elif mouseY > oldMouseY: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(xVec, factor), speedY)) + # update oldMouse values + oldMouseY, oldMouseX = mouseY, mouseX + + # check the keyboard state + if keys: + # if shift is pressed, double the speed + if p.B3G_SHIFT in keys: + speedMult = 5 + else: + speedMult = 2.5 + + # if control is pressed, the movements caused by the arrowkeys, the '+' as well as the '-' key + # change + if p.B3G_CONTROL in keys: + + # the up and down arrowkeys cause the targetPos to move along the z axis of the map + if p.B3G_DOWN_ARROW in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(zVec, 0.03), speedMult)) + elif p.B3G_UP_ARROW in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(zVec, 0.03), speedMult)) + + # left and right arrowkeys cause the targetPos to move horizontally relative to the camera + if p.B3G_LEFT_ARROW in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(xVec, 0.03), speedMult)) + elif p.B3G_RIGHT_ARROW in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(xVec, 0.03), speedMult)) + + # the '+' and '-' keys cause the targetpos to move forwards and backwards relative to the camera + # while the camera stays at a constant distance. SHIFT + '=' is for US layout + if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(yVec, 0.03), speedMult)) + elif ord("-") in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(yVec, 0.03), speedMult)) + + # standard bindings for thearrowkeys, the '+' as well as the '-' key + else: + + # left and right arrowkeys cause the camera to rotate around the yaw axis + if p.B3G_RIGHT_ARROW in keys: + cameraYaw += (360 / width) * speedMult + elif p.B3G_LEFT_ARROW in keys: + cameraYaw -= (360 / width) * speedMult + + # the up and down arrowkeys cause the camera to rotate around the pitch axis + if p.B3G_DOWN_ARROW in keys: + if (cameraPitch + (360 / height) * speedMult) < 89.5: + cameraPitch += (360 / height) * speedMult + elif p.B3G_UP_ARROW in keys: + if (cameraPitch - (360 / height) * speedMult) > -89.5: + cameraPitch -= (360 / height) * speedMult + + # the '+' and '-' keys cause the camera to zoom towards and away from the targetPos without + # moving it. SHIFT + '=' is for US layout since the events can't handle shift plus something + if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: + if (dist - (dist * 0.02) * speedMult) > 0.1: + dist -= dist * 0.02 * speedMult + elif ord("-") in keys: + dist += dist * 0.02 * speedMult + + p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, + cameraTargetPosition=cameraTargetPosition) + if visible == 0: + cameraTargetPosition = (0.0, -50, 50) + p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) + time.sleep(1. / 80.) + + +class Object: + """ + Represents a spawned Object in the BulletWorld. + """ + + def __init__(self, name: str, type: Union[str, ObjectType], path: str, + pose: Pose = None, + world: BulletWorld = None, + color: Optional[List[float]] = [1, 1, 1, 1], + ignoreCachedFiles: Optional[bool] = False): + """ + The constructor loads the urdf file into the given BulletWorld, if no BulletWorld is specified the + :py:attr:`~BulletWorld.current_bullet_world` will be used. It is also possible to load .obj and .stl file into the BulletWorld. + The color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. + + :param name: The name of the object + :param type: The type of the object + :param path: The path to the source file, if only a filename is provided then the resourcer directories will be searched + :param pose: The pose at which the Object should be spawned + :param world: The BulletWorld in which the object should be spawned, if no world is specified the :py:attr:`~BulletWorld.current_bullet_world` will be used + :param color: The color with which the object should be spawned. + :param ignoreCachedFiles: If true the file will be spawned while ignoring cached files. + """ + if not pose: + pose = Pose() + self.world: BulletWorld = world if world is not None else BulletWorld.current_bullet_world + self.local_transformer = LocalTransformer() + self.name: str = name + self.type: str = type + self.color: List[float] = color + pose_in_map = self.local_transformer.transform_pose(pose, "map") + position, orientation = pose_in_map.to_list() + self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignoreCachedFiles) + self.joints: Dict[str, int] = self._joint_or_link_name_to_id("joint") + self.links: Dict[str, int] = self._joint_or_link_name_to_id("link") + self.attachments: Dict[Object, List] = {} + self.cids: Dict[Object, int] = {} + self.original_pose = pose_in_map + + self.tf_frame = ("shadow/" if self.world.is_shadow_world else "") + self.name + "_" + str(self.id) + + # This means "world" is not the shadow world since it has a reference to a shadow world + if self.world.shadow_world != None: + self.world.world_sync.add_obj_queue.put( + [name, type, path, position, orientation, self.world.shadow_world, color, self]) + + with open(self.path) as f: + self.urdf_object = URDF.from_xml_string(f.read()) + if self.urdf_object.name == robot_description.name and not BulletWorld.robot: + BulletWorld.robot = self + + self.links[self.urdf_object.get_root()] = -1 + + self._current_pose = pose_in_map + self._current_link_poses = {} + self._current_link_transforms = {} + self._current_joints_positions = {} + self._init_current_positions_of_joint() + self._update_link_poses() + + self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) + self.local_transformer.update_transforms_for_object(self) + self.link_to_geometry = self._get_geometry_for_link() + + self.world.objects.append(self) + + def __repr__(self): + skip_attr = ["links", "joints", "urdf_object", "attachments", "cids", "_current_link_poses", + "_current_link_transforms", "link_to_geometry"] + return self.__class__.__qualname__ + f"(" + ', \n'.join( + [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" + + def remove(self) -> None: + """ + Removes this object from the BulletWorld it currently resides in. + For the object to be removed it has to be detached from all objects it + is currently attached to. After this is done a call to PyBullet is done + to remove this Object from the simulation. + """ + for obj in self.attachments.keys(): + self.world.detach(self, obj) + self.world.objects.remove(self) + # This means the current world of the object is not the shadow world, since it + # has a reference to the shadow world + if self.world.shadow_world is not None: + self.world.world_sync.remove_obj_queue.put(self) + self.world.world_sync.remove_obj_queue.join() + self.world.remove_object(self.id) + if BulletWorld.robot == self: + BulletWorld.robot = None + + def attach(self, obj: Object, link: Optional[str] = None, loose: Optional[bool] = False) -> None: + """ + Attaches another object to this object. This is done by + saving the transformation between the given link, if there is one, and + the base pose of the other object. Additionally, the name of the link, to + which the object is attached, will be saved. + Furthermore, a constraint of pybullet will be created so the attachment + also works while simulation. + Loose attachments means that the attachment will only be one-directional. For example, if this object moves the + other, attached, object will also move but not the other way around. + + :param object: The other object that should be attached + :param link: The link of this object to which the other object should be + :param loose: If the attachment should be a loose attachment. + """ + self.world.attach_objects(self, obj, link, loose) + + def detach(self, obj: Object) -> None: + """ + Detaches another object from this object. This is done by + deleting the attachment from the attachments dictionary of both objects + and deleting the constraint of pybullet. + Afterward the detachment event of the corresponding BulletWorld will be fired. + + :param object: The object which should be detached + """ + self.world.detach_objects(self, obj) + + def detach_all(self) -> None: + """ + Detach all objects attached to this object. + """ + attachments = self.attachments.copy() + for att in attachments.keys(): + self.world.detach(self, att) + + def get_position(self) -> Pose: + """ + Returns the position of this Object as a list of xyz. + + :return: The current position of this object + """ + return self.get_pose().position + + def get_orientation(self) -> Quaternion: + """ + Returns the orientation of this object as a list of xyzw, representing a quaternion. + + :return: A list of xyzw + """ + return self.get_pose().orientation + + def get_pose(self) -> Pose: + """ + Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. + + :return: The current pose of this object + """ + return self._current_pose + + def set_pose(self, pose: Pose, base: bool = False) -> None: + """ + Sets the Pose of the object. + + :param pose: New Pose for the object + :param base: If True places the object base instead of origin at the specified position and orientation + """ + pose_in_map = self.local_transformer.transform_pose(pose, "map") + position, orientation = pose_in_map.to_list() + if base: + position = np.array(position) + self.base_origin_shift + self.world.reset_object_base_pose(self, position, orientation) + self._current_pose = pose_in_map + self._update_link_poses() + self.world._set_attached_objects(self, [self]) + + @property + def pose(self) -> Pose: + """ + Property that returns the current position of this Object. + + :return: The position as a list of xyz + """ + return self.get_pose() + + @pose.setter + def pose(self, value: Pose) -> None: + """ + Sets the Pose of the Object to the given value. Function for attribute use. + + :param value: New Pose of the Object + """ + self.set_pose(value) + + def move_base_to_origin_pos(self) -> None: + """ + Move the object such that its base will be at the current origin position. + This is useful when placing objects on surfaces where you want the object base in contact with the surface. + """ + self.set_pose(self.get_pose(), base=True) + + def _set_attached_objects(self, prev_object: List[Object]) -> None: + """ + Updates the positions of all attached objects. This is done + by calculating the new pose in world coordinate frame and setting the + base pose of the attached objects to this new pose. + After this the _set_attached_objects method of all attached objects + will be called. + + :param prev_object: A list of Objects that were already moved, these will be excluded to prevent loops in the update. + """ + self.world._set_attached_objects(self, prev_object) + + def _calculate_transform(self, obj: Object, link: str = None) -> Transform: + """ + Calculates the transformation between another object and the given + link of this object. If no link is provided then the base position will be used. + + :param obj: The other object for which the transformation should be calculated + :param link: The optional link name + :return: The transformation from the link (or base position) to the other objects base position + """ + transform = self.local_transformer.transform_to_object_frame(obj.pose, self, link) + + return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) + + def set_position(self, position: Union[Pose, Point], base=False) -> None: + """ + Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position + instead of the origin in the center of the Object. The given position can either be a Pose, + in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. + + :param position: Target position as xyz. + :param base: If the bottom of the Object should be placed or the origin in the center. + """ + pose = Pose() + if isinstance(position, Pose): + target_position = position.position + pose.frame = position.frame + else: + target_position = position + + pose.pose.position = target_position + pose.pose.orientation = self.get_orientation() + self.set_pose(pose, base=base) + + def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: + """ + Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only + the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. + + :param orientation: Target orientation given as a list of xyzw. + """ + pose = Pose() + if isinstance(orientation, Pose): + target_orientation = orientation.orientation + pose.frame = orientation.frame + else: + target_orientation = orientation + + pose.pose.position = self.get_position() + pose.pose.orientation = target_orientation + self.set_pose(pose) + + def _joint_or_link_name_to_id(self, name_type: str) -> Dict[str, int]: + """ + Creates a dictionary which maps the link or joint name to the unique ids used by pybullet. + + :param name_type: Determines if the dictionary should be for joints or links + :return: A dictionary that maps joint or link names to unique ids + """ + n_joints = self.world.get_object_number_of_joints(self) + joint_name_to_id = {} + for i in range(0, n_joints): + _id = self.world.get_object_joint_id(self, i) + if name_type == "joint": + _name = self.world.get_object_joint_name(self, i) + else: + _name = self.world.get_object_link_name(self, i) + joint_name_to_id[_name] = _id + return joint_name_to_id + + def get_joint_id(self, name: str) -> int: + """ + Returns the unique id for a joint name. As used by PyBullet. + + :param name: The joint name + :return: The unique id + """ + return self.joints[name] + + def get_link_id(self, name: str) -> int: + """ + Returns a unique id for a link name. As used by PyBullet. + + :param name: The link name + :return: The unique id + """ + return self.links[name] + + def get_link_by_id(self, id: int) -> str: + """ + Returns the name of a link for a given unique PyBullet id + + :param id: PyBullet id for link + :return: The link name + """ + return dict(zip(self.links.values(), self.links.keys()))[id] + + def get_joint_by_id(self, joint_id: int) -> str: + """ + Returns the joint name for a unique PyBullet id + + :param joint_id: The Pybullet id of for joint + :return: The joint name + """ + return dict(zip(self.joints.values(), self.joints.keys()))[joint_id] + + def get_link_relative_to_other_link(self, source_frame: str, target_frame: str) -> Pose: + """ + Calculates the position of a link in the coordinate frame of another link. + + :param source_frame: The name of the source frame + :param target_frame: The name of the target frame + :return: The pose of the source frame in the target frame + """ + source_pose = self.get_link_pose(source_frame) + return self.local_transformer.transform_to_object_frame(source_pose, self, target_frame) + + def get_link_position(self, name: str) -> Point: + """ + Returns the position of a link of this Object. Position is returned as a list of xyz. + + :param name: The link name + :return: The link position as xyz + """ + return self.get_link_pose(name).position + + def get_link_orientation(self, name: str) -> Quaternion: + """ + Returns the orientation of a link of this Object. Orientation is returned as a quaternion. + + :param name: The name of the link + :return: The orientation of the link as a quaternion + """ + return self.get_link_pose(name).orientation + + def get_link_pose(self, name: str) -> Pose: + """ + Returns a Pose of the link corresponding to the given name. The returned Pose will be in world coordinate frame. + + :param name: Link name for which a Pose should be returned + :return: The pose of the link + """ + if name in self.links.keys() and self.links[name] == -1: + return self.get_pose() + return self._current_link_poses[name] + # return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6]) + + def set_joint_position(self, joint_name: str, joint_pose: float) -> None: + """ + Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, as stated + in the URDF, an error will be printed. However, the joint will be set either way. + + :param joint_name: The name of the joint + :param joint_pose: The target pose for this joint + """ + # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly + up_lim, low_lim = self.world.get_object_joint_limits(self, joint_name) + if low_lim > up_lim: + low_lim, up_lim = up_lim, low_lim + if not low_lim <= joint_pose <= up_lim: + logging.error( + f"The joint position has to be within the limits of the joint. The joint limits for {joint_name} are {low_lim} and {up_lim}") + logging.error(f"The given joint position was: {joint_pose}") + # Temporarily disabled because kdl outputs values exciting joint limits + # return + self.world.reset_joint_position(self, joint_name, joint_pose) + self._current_joints_positions[joint_name] = joint_pose + # self.local_transformer.update_transforms_for_object(self) + self._update_link_poses() + self.world._set_attached_objects(self, [self]) + + def set_positions_of_all_joints(self, joint_poses: dict) -> 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: + :return: + """ + for joint_name, joint_pose in joint_poses.items(): + self.world.reset_joint_position(self, joint_name, joint_pose) + self._current_joints_positions[joint_name] = joint_pose + self._update_link_poses() + self.world._set_attached_objects(self, [self]) + + def get_joint_position(self, joint_name: str) -> float: + """ + Returns the joint position for the given joint name. + + :param joint_name: The name of the joint + :return: The current pose of the joint + """ + return self._current_joints_positions[joint_name] + + def contact_points(self) -> List: + """ + Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned + list please look at `PyBullet Doc `_ + + :return: A list of all contact points with other objects + """ + return self.world.get_object_contact_points(self) + + def contact_points_simulated(self) -> List: + """ + Returns a list of all contact points between this Object and other Objects after stepping the simulation once. + For a more detailed explanation of the returned + list please look at `PyBullet Doc `_ + + :return: A list of contact points between this Object and other Objects + """ + s = self.world.save_state() + self.world.step() + contact_points = self.contact_points() + self.world.restore_state(*s) + return contact_points + + def update_joints_from_topic(self, topic_name: str) -> None: + """ + Updates the joints of this object with positions obtained from a topic with the message type JointState. + Joint names on the topic have to correspond to the joints of this object otherwise an error message will be logged. + + :param topic_name: Name of the topic with the joint states + """ + msg = rospy.wait_for_message(topic_name, JointState) + joint_names = msg.name + joint_positions = msg.position + if set(joint_names).issubset(self.joints.keys()): + for i in range(len(joint_names)): + self.set_joint_position(joint_names[i], joint_positions[i]) + else: + add_joints = set(joint_names) - set(self.joints.keys()) + rospy.logerr(f"There are joints in the published joint state which are not in this model: /n \ + The following joint{'s' if len(add_joints) != 1 else ''}: {add_joints}") + + def update_pose_from_tf(self, frame: str) -> None: + """ + Updates the pose of this object from a TF message. + + :param frame: Name of the TF frame from which the position should be taken + """ + tf_listener = tf.TransformListener() + time.sleep(0.5) + position, orientation = tf_listener.lookupTransform(frame, "map", rospy.Time(0)) + position = [position[0][0] * -1, + position[0][1] * -1, + position[0][2]] + self.set_position(Pose(position, orientation)) + + def set_color(self, color: List[float], link: Optional[str] = "") -> None: + """ + Changes the color of this object, the color has to be given as a list + of RGBA values. Optionally a link name can can be provided, if no link + name is provided all links of this object will be colored. + + :param color: The color as RGBA values between 0 and 1 + :param link: The link name of the link which should be colored + """ + self.world.set_object_color(self, color, link) + + def get_color(self, link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: + """ + This method returns the color of this object or a link of this object. If no link is given then the + return is either: + + 1. A list with the color as RGBA values, this is the case if the object only has one link (this + happens for example if the object is spawned from a .obj or .stl file) + 2. A dict with the link name as key and the color as value. The color is given as RGBA values. + Please keep in mind that not every link may have a color. This is dependent on the URDF from which the + object is spawned. + + If a link is specified then the return is a list with RGBA values representing the color of this link. + It may be that this link has no color, in this case the return is None as well as an error message. + + :param link: the link name for which the color should be returned. + :return: The color of the object or link, or a dictionary containing every colored link with its color + """ + return self.world.get_object_color(self, link) + + def get_AABB(self, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case + the axis aligned bounding box of the link will be returned. The return of this method are two points in + world coordinate frame which define a bounding box. + + :param link_name: The Optional name of a link of this object. + :return: Two lists of x,y,z which define the bounding box. + """ + return self.world.get_object_AABB(self, link_name) + + def get_base_origin(self, link_name: Optional[str] = None) -> Pose: + """ + Returns the origin of the base/bottom of an object/link + + :param link_name: The link name for which the bottom position should be returned + :return: The position of the bottom of this Object or link + """ + aabb = self.get_AABB(link_name=link_name) + base_width = np.absolute(aabb[0][0] - aabb[1][0]) + base_length = np.absolute(aabb[0][1] - aabb[1][1]) + return Pose([aabb[0][0] + base_width / 2, aabb[0][1] + base_length / 2, aabb[0][2]], + self.get_pose().orientation_as_list()) + + def get_joint_limits(self, joint: str) -> Tuple[float, float]: + """ + Returns the lower and upper limit of a joint, if the lower limit is higher + than the upper they are swapped to ensure the lower limit is always the smaller one. + + :param joint: The name of the joint for which the limits should be found. + :return: The lower and upper limit of the joint. + """ + if joint not in self.joints.keys(): + raise KeyError(f"The given Joint: {joint} is not part of this object") + lower, upper = self.world.get_object_joint_limits(self, joint) + if lower > upper: + lower, upper = upper, lower + return lower, upper + + def get_joint_axis(self, joint_name: str) -> Tuple[float]: + """ + Returns the axis along which a joint is moving. The given joint_name has to be part of this object. + + :param joint_name: Name of the joint for which the axis should be returned. + :return: The axis a vector of xyz + """ + return self.world.get_object_joint_axis(self, joint_name) + + def get_joint_type(self, joint_name: str) -> JointType: + """ + Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. + + :param joint_name: Joint name for which the type should be returned + :return: The type of the joint + """ + return self.world.get_object_joint_type(self, joint_name) + + def find_joint_above(self, link_name: str, joint_type: JointType) -> str: + """ + Traverses the chain from 'link_name' to the URDF origin and returns the first joint that is of type 'joint_type'. + + :param link_name: Link name above which the joint should be found + :param joint_type: Joint type that should be searched for + :return: Name of the first joint which has the given type + """ + chain = self.urdf_object.get_chain(self.urdf_object.get_root(), link_name) + reversed_chain = reversed(chain) + container_joint = None + for element in reversed_chain: + if element in self.joints and self.get_joint_type(element) == joint_type: + container_joint = element + break + if not container_joint: + rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") + return container_joint + + 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. + + :return: A dictionary with all joints positions'. + """ + return self._current_joints_positions + + def get_link_tf_frame(self, link_name: str) -> str: + """ + Returns the name of the tf frame for the given link name. This method does not check if the given name is + actually a link of this object. + + :param link_name: Name of a link for which the tf frame should be returned + :return: A TF frame name for a specific link + """ + return self.tf_frame + "/" + link_name + + def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]: + """ + Extracts the geometry information for each collision of each link and links them to the respective link. + + :return: A dictionary with link name as key and geometry information as value + """ + link_to_geometry = {} + for link in self.links.keys(): + link_obj = self.urdf_object.link_map[link] + if not link_obj.collision: + link_to_geometry[link] = None + else: + link_to_geometry[link] = link_obj.collision.geometry + return link_to_geometry + + def _update_link_poses(self) -> None: + """ + Updates the cached poses and transforms for each link of this Object + """ + for link_name in self.links.keys(): + if link_name == self.urdf_object.get_root(): + self._current_link_poses[link_name] = self._current_pose + self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame) + else: + self._current_link_poses[link_name] = Pose(*self.world.get_object_link_pose(self, link_name)) + self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( + self.get_link_tf_frame(link_name)) + + def _init_current_positions_of_joint(self) -> None: + """ + Initialize the cached joint position for each joint. + """ + for joint_name in self.joints.keys(): + self._current_joints_positions[joint_name] = self.world.get_object_joint_position(self, joint_name) + + +def filter_contact_points(contact_points, exclude_ids) -> List: + """ + Returns a list of contact points where Objects that are in the 'exclude_ids' list are removed. + + :param contact_points: A list of contact points + :param exclude_ids: A list of unique ids of Objects that should be removed from the list + :return: A list containing 'contact_points' without Objects that are in 'exclude_ids' + """ + return list(filter(lambda cp: cp[2] not in exclude_ids, contact_points)) + + +def get_path_from_data_dir(file_name: str, data_directory: str) -> str: + """ + Returns the full path for a given file name in the given directory. If there is no file with the given filename + this method returns None. + + :param file_name: The filename of the searched file. + :param data_directory: The directory in which to search for the file. + :return: The full path in the filesystem or None if there is no file with the filename in the directory + """ + dir = pathlib.Path(data_directory) + for file in os.listdir(data_directory): + if file == file_name: + return data_directory + f"/{file_name}" + + +def _get_robot_name_from_urdf(urdf_string: str) -> str: + """ + Extracts the robot name from the 'robot_name' tag of a URDF. + + :param urdf_string: The URDF as string. + :return: The name of the robot described by the URDF. + """ + res = re.findall(r"robot\ *name\ *=\ *\"\ *[a-zA-Z_0-9]*\ *\"", urdf_string) + if len(res) == 1: + begin = res[0].find("\"") + end = res[0][begin + 1:].find("\"") + robot = res[0][begin + 1:begin + 1 + end].lower() + return robot + + +def _load_object(name: str, + path: str, + position: List[float], + orientation: List[float], + world: BulletWorld, + color: List[float], + ignoreCachedFiles: bool) -> Tuple[int, str]: + """ + Loads an object to the given BulletWorld with the given position and orientation. The color will only be + used when an .obj or .stl file is given. + If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created + and this URDf file will be loaded instead. + When spawning a URDf file a new file will be created in the cache directory, if there exists none. + This new file will have resolved mesh file paths, meaning there will be no references + to ROS packges instead there will be absolute file paths. + + :param name: The name of the object which should be spawned + :param path: The path to the source file or the name on the ROS parameter server + :param position: The position in which the object should be spawned + :param orientation: The orientation in which the object should be spawned + :param world: The BulletWorld to which the Object should be spawned + :param color: The color of the object, only used when .obj or .stl file is given + :param ignoreCachedFiles: Whether to ignore files in the cache directory. + :return: The unique id of the object and the path to the file used for spawning + """ + pa = pathlib.Path(path) + extension = pa.suffix + world, world_id = _world_and_id(world) + if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): + for dir in world.data_directory: + path = get_path_from_data_dir(path, dir) + if path: break + + if not path: + raise FileNotFoundError( + f"File {pa.name} could not be found in the resource directory {world.data_directory}") + # rospack = rospkg.RosPack() + # cach_dir = rospack.get_path('pycram') + '/resources/cached/' + cach_dir = world.data_directory[0] + '/cached/' + if not pathlib.Path(cach_dir).exists(): + os.mkdir(cach_dir) + + # if file is not yet cached corrcet the urdf and save if in the cache directory + if not _is_cached(path, name, cach_dir) or ignoreCachedFiles: + if extension == ".obj" or extension == ".stl": + path = _generate_urdf_file(name, path, color, cach_dir) + elif extension == ".urdf": + with open(path, mode="r") as f: + urdf_string = fix_missing_inertial(f.read()) + urdf_string = remove_error_tags(urdf_string) + urdf_string = fix_link_attributes(urdf_string) + try: + urdf_string = _correct_urdf_string(urdf_string) + except rospkg.ResourceNotFound as e: + rospy.logerr(f"Could not find resource package linked in this URDF") + raise e + path = cach_dir + pa.name + with open(path, mode="w") as f: + f.write(urdf_string) + else: # Using the urdf from the parameter server + urdf_string = rospy.get_param(path) + path = cach_dir + name + ".urdf" + with open(path, mode="w") as f: + f.write(_correct_urdf_string(urdf_string)) + # save correct path in case the file is already in the cache directory + elif extension == ".obj" or extension == ".stl": + path = cach_dir + pa.stem + ".urdf" + elif extension == ".urdf": + path = cach_dir + pa.name + else: + path = cach_dir + name + ".urdf" + + try: + obj = p.loadURDF(path, basePosition=position, baseOrientation=orientation, physicsClientId=world_id) + return obj, path + except p.error as e: + logging.error( + "The File could not be loaded. Plese note that the path has to be either a URDF, stl or obj file or the name of an URDF string on the parameter server.") + os.remove(path) + raise (e) + # print(f"{bcolors.BOLD}{bcolors.WARNING}The path has to be either a path to a URDf file, stl file, obj file or the name of a URDF on the parameter server.{bcolors.ENDC}") + + +def _is_cached(path: str, name: str, cach_dir: str) -> bool: + """ + Checks if the file in the given path is already cached or if + there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from + the parameter server is used. + + :param path: The path given by the user to the source file. + :param name: The name for this object. + :param cach_dir: The absolute path the cach directory in the pycram package. + :return: True if there already exists a chached file, False in any other case. + """ + file_name = pathlib.Path(path).name + p = pathlib.Path(cach_dir + file_name) + if p.exists(): + return True + # Returns filename without the filetype, e.g. returns "test" for "test.txt" + file_stem = pathlib.Path(path).stem + p = pathlib.Path(cach_dir + file_stem + ".urdf") + if p.exists(): + return True + return False + + +def _correct_urdf_string(urdf_string: str) -> str: + """ + Changes paths for files in the URDF from ROS paths to paths in the file system. Since PyBullet can't deal with ROS + package paths. + + :param urdf_name: The name of the URDf on the parameter server + :return: The URDF string with paths in the filesystem instead of ROS packages + """ + r = rospkg.RosPack() + new_urdf_string = "" + for line in urdf_string.split('\n'): + if "package://" in line: + s = line.split('//') + s1 = s[1].split('/') + path = r.get_path(s1[0]) + line = line.replace("package://" + s1[0], path) + new_urdf_string += line + '\n' + + return fix_missing_inertial(new_urdf_string) + + +def fix_missing_inertial(urdf_string: str) -> str: + """ + Insert inertial tags for every URDF link that has no inertia. + This is used to prevent PyBullet from dumping warnings in the terminal + + :param urdf_string: The URDF description as string + :returns: The new, corrected URDF description as string. + """ + + inertia_tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.Element("inertial")) + inertia_tree.getroot().append(xml.etree.ElementTree.Element("mass", {"value": "0.1"})) + inertia_tree.getroot().append(xml.etree.ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) + inertia_tree.getroot().append(xml.etree.ElementTree.Element("inertia", {"ixx": "0.01", + "ixy": "0", + "ixz": "0", + "iyy": "0.01", + "iyz": "0", + "izz": "0.01"})) + + # create tree from string + tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) + + for link_element in tree.iter("link"): + inertial = [*link_element.iter("inertial")] + if len(inertial) == 0: + link_element.append(inertia_tree.getroot()) + + return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') + + +def remove_error_tags(urdf_string: str) -> str: + """ + Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the + URDF_parser + + :param urdf_string: String of the URDF from which the tags should be removed + :return: The URDF string with the tags removed + """ + tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) + removing_tags = ["gazebo", "transmission"] + for tag_name in removing_tags: + all_tags = tree.findall(tag_name) + for tag in all_tags: + tree.getroot().remove(tag) + + return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') + + +def fix_link_attributes(urdf_string: str) -> str: + """ + Removes the attribute 'type' from links since this is not parsable by the URDF parser. + + :param urdf_string: The string of the URDF from which the attributes should be removed + :return: The URDF string with the attributes removed + """ + tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) + + for link in tree.iter("link"): + if "type" in link.attrib.keys(): + del link.attrib["type"] + + return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') + + +def _generate_urdf_file(name: str, path: str, color: List[float], cach_dir: str) -> str: + """ + Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given color will be + used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with the name + as filename. + + :param name: The name of the object + :param path: The path to the .obj or .stl file + :param color: The color which should be used for the material tag + :param cach_dir The absolute file path to the cach directory in the pycram package + :return: The absolute path of the created file + """ + urdf_template = ' \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + ' + urdf_template = fix_missing_inertial(urdf_template) + rgb = " ".join(list(map(str, color))) + pathlib_obj = pathlib.Path(path) + path = str(pathlib_obj.resolve()) + content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) + with open(cach_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: + file.write(content) + return cach_dir + pathlib_obj.stem + ".urdf" + + +def _world_and_id(world: BulletWorld) -> Tuple[BulletWorld, int]: + """ + Selects the world to be used. If the given world is None the 'current_bullet_world' is used. + + :param world: The world which should be used or None if 'current_bullet_world' should be used + :return: The BulletWorld object and the id of this BulletWorld + """ + world = world if world is not None else BulletWorld.current_bullet_world + id = world.client_id if world is not None else BulletWorld.current_bullet_world.client_id + return world, id From 3af92d4fe401551d34c770688f25c034dd946122 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 30 Nov 2023 17:44:16 +0100 Subject: [PATCH 04/69] [WorldAbstractClass] InProgress , currently in add_constraint method --- doc/source/conf.py | 2 +- src/pycram/bullet_world.py | 8 +- src/pycram/local_transformer.py | 28 +- src/pycram/robot_description.py | 6 +- src/pycram/ros/force_torque_sensor.py | 4 +- src/pycram/ros/joint_state_publisher.py | 4 +- src/pycram/world.py | 702 ++++++++++++++++++++++-- test/test_action_designator.py | 2 +- 8 files changed, 690 insertions(+), 66 deletions(-) diff --git a/doc/source/conf.py b/doc/source/conf.py index cb9889b21..a21923062 100644 --- a/doc/source/conf.py +++ b/doc/source/conf.py @@ -196,7 +196,7 @@ # Example Thumbnail config nbsphinx_thumbnails = { - 'notebooks/bullet_world': os.path.abspath("notebooks/thumbnails/bullet_world.png"), + 'notebooks/world': os.path.abspath("notebooks/thumbnails/bullet_world.png"), 'notebooks/minimal_task_tree': os.path.abspath("notebooks/thumbnails/tree.png"), 'notebooks/action_designator': os.path.abspath("notebooks/thumbnails/action_designator.png"), 'notebooks/motion_designator': os.path.abspath("notebooks/thumbnails/motion_designator.png"), diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 19671d8c3..75de52e45 100644 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -90,8 +90,8 @@ def __init__(self, type: str = "GUI", is_shadow_world: bool = False): self.local_transformer = LocalTransformer() if not is_shadow_world: self.world_sync.start() - self.local_transformer.bullet_world = self - self.local_transformer.shadow_world = self.shadow_world + self.local_transformer.world = self + self.local_transformer.prospection_world = self.shadow_world # Some default settings self.set_gravity([0, 0, -9.8]) @@ -239,7 +239,7 @@ def restore_state(self, state, objects2attached: Dict = {}) -> None: the state and restoring they will be skiped. :param state: The unique id representing the state, as returned by :func:`~save_state` - :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~bullet_world.Object.attachments` + :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~world.Object.attachments` """ p.restoreState(state, physicsClientId=self.client_id) for obj in self.objects: @@ -450,7 +450,7 @@ def run(self): # self.equal_states = False for i in range(self.add_obj_queue.qsize()): obj = self.add_obj_queue.get() - # [name, type, path, position, orientation, self.world.shadow_world, color, bulletworld object] + # [name, type, path, position, orientation, self.world.prospection_world, color, bulletworld object] o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) # Maps the BulletWorld object to the shadow world object self.object_mapping[obj[7]] = o diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index c68945182..9c0ede811 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -3,8 +3,8 @@ import tf -if 'bullet_world' in sys.modules: - logging.warning("(publisher) Make sure that you are not loading this module from pycram.bullet_world.") +if 'world' in sys.modules: + logging.warning("(publisher) Make sure that you are not loading this module from pycram.world.") import rospkg import rospy import atexit @@ -54,18 +54,18 @@ def __init__(self): self.static_tf_stampeds: List[TransformStamped] = [] # Since this file can't import world.py this holds the reference to the current_bullet_world - self.bullet_world = None - self.shadow_world = None + self.world = None + self.prospection_world = None # If the singelton was already initialized self._initialized = True def update_objects_for_current_world(self) -> None: """ - Updates transformations for all objects that are currently in :py:attr:`~pycram.bullet_world.BulletWorld.current_bullet_world` + Updates transformations for all objects that are currently in :py:attr:`~pycram.world.World.current_world` """ curr_time = rospy.Time.now() - for obj in list(self.bullet_world.current_bullet_world.objects): + for obj in list(self.world.current_bullet_world.objects): self.update_transforms_for_object(obj, curr_time) def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: @@ -92,20 +92,20 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: return Pose(*copy_pose.to_list(), frame=new_pose.header.frame_id) def transform_to_object_frame(self, pose: Pose, - bullet_object: 'bullet_world.Object', link_name: str = None) -> Union[Pose, None]: + world_object: 'world.Object', link_name: str = None) -> Union[Pose, None]: """ Transforms the given pose to the coordinate frame of the given BulletWorld object. If no link name is given the base frame of the Object is used, otherwise the link frame is used as target for the transformation. :param pose: Pose that should be transformed - :param bullet_object: BulletWorld Object in which frame the pose should be transformed - :param link_name: A link of the BulletWorld Object which will be used as target coordinate frame instead + :param world_object: World Object in which frame the pose should be transformed + :param link_name: A link of the World Object which will be used as target coordinate frame instead :return: The new pose the in coordinate frame of the object """ if link_name: - target_frame = bullet_object.get_link_tf_frame(link_name) + target_frame = world_object.get_link_tf_frame(link_name) else: - target_frame = bullet_object.tf_frame + target_frame = world_object.tf_frame return self.transform_pose(pose, target_frame) def tf_transform(self, source_frame: str, target_frame: str, @@ -124,15 +124,15 @@ def tf_transform(self, source_frame: str, target_frame: str, translation, rotation = self.lookupTransform(source_frame, target_frame, tf_time) return Transform(translation, rotation, source_frame, target_frame) - def update_transforms_for_object(self, bullet_object: 'bullet_world.Object', time: rospy.Time = None) -> None: + def update_transforms_for_object(self, world_object: 'world.Object', time: rospy.Time = None) -> None: """ Updates local transforms for a Bullet Object, this includes the base as well as all links - :param bullet_object: Object for which the Transforms should be updated + :param world_object: Object for which the Transforms should be updated :param time: a specific time that should be used """ time = time if time else rospy.Time.now() - for transform in bullet_object._current_link_transforms.values(): + for transform in world_object._current_link_transforms.values(): transform.header.stamp = time self.setTransform(transform) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index d8df6f5b8..ab896d1e1 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -186,7 +186,7 @@ class GraspingDescription: """ def __init__(self, grasp_dir: Optional[Dict] = None): self.grasps: Dict[str, List[float]] = grasp_dir if grasp_dir else {} - self.grasps_for_object: Dict['bullet_world.Object', List[str]] = {} + self.grasps_for_object: Dict['world.Object', List[str]] = {} def add_grasp(self, grasp: str, orientation: List[float]) -> None: """ @@ -198,7 +198,7 @@ def add_grasp(self, grasp: str, orientation: List[float]) -> None: """ self.grasps[grasp] = orientation - def add_graspings_for_object(self, grasps: List[str], object: 'bullet_world.Object') -> None: + def add_graspings_for_object(self, grasps: List[str], object: 'world.Object') -> None: """ Adds all possible Grasps for the specified object. The used grasps have to be registered beforehand via the add_grasp method. @@ -214,7 +214,7 @@ def get_all_grasps(self) -> List[str]: def get_orientation_for_grasp(self, grasp: str) -> List[float]: return self.grasps[grasp] - def get_grasps_for_object(self, object: 'bullet_world.Object') -> List[str]: + def get_grasps_for_object(self, object: 'world.Object') -> List[str]: return self.grasps_for_object[object] diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros/force_torque_sensor.py index 6da358d55..314a9239a 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros/force_torque_sensor.py @@ -13,12 +13,12 @@ class ForceTorqueSensor: """ Simulated force-torque sensor for a joint with a given name. - Reads simulated forces and torques at that joint from bullet_world and publishes geometry_msgs/Wrench messages + Reads simulated forces and torques at that joint from world and publishes geometry_msgs/Wrench messages to the given topic. """ def __init__(self, joint_name, fts_topic="/pycram/fts", interval=0.1): """ - The given joint_name has to be part of :py:attr:`~pycram.bullet_world.BulletWorld.robot` otherwise a + The given joint_name has to be part of :py:attr:`~pycram.world.BulletWorld.robot` otherwise a RuntimeError will be raised. :param joint_name: Name of the joint for which force-torque should be simulated diff --git a/src/pycram/ros/joint_state_publisher.py b/src/pycram/ros/joint_state_publisher.py index 1126c535d..f771b5565 100644 --- a/src/pycram/ros/joint_state_publisher.py +++ b/src/pycram/ros/joint_state_publisher.py @@ -15,7 +15,7 @@ class JointStatePublisher: """ def __init__(self, joint_state_topic="/pycram/joint_state", interval=0.1): """ - Robot object is from :py:attr:`~pycram.bullet_world.BulletWorld.robot` and current joint states are published to + Robot object is from :py:attr:`~pycram.world.BulletWorld.robot` and current joint states are published to the given joint_state_topic as a JointState message. :param joint_state_topic: Topic name to which the joint states should be published @@ -33,7 +33,7 @@ def __init__(self, joint_state_topic="/pycram/joint_state", interval=0.1): def _publish(self) -> None: """ - Publishes the current joint states of the :py:attr:`~pycram.bullet_world.BulletWorld.robot` in an infinite loop. + Publishes the current joint states of the :py:attr:`~pycram.world.BulletWorld.robot` in an infinite loop. The joint states are published as long as the kill_event is not set by :py:meth:`~JointStatePublisher._stop_publishing` """ robot = BulletWorld.robot diff --git a/src/pycram/world.py b/src/pycram/world.py index 695900c90..2e3c0f159 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -31,8 +31,648 @@ from .pose import Pose, Transform +from abc import ABC, abstractproperty, abstractmethod -class BulletWorld: + +class World(ABC): + """ + The World Class represents the physics Simulation and belief state. + """ + current_world: World = None + """ + Global reference to the currently used World, usually this is the + graphical one. However, if you are inside a Use_shadow_world() environment the current_world points to the + shadow world. In this way you can comfortably use the current_world, which should point towards the World + used at the moment. + """ + robot: Object = None + """ + Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the + URDF with the name of the URDF on the parameter server. + """ + def __init__(self, mode: str, is_prospection_world: bool): + """ + Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the + background. There can only be one rendered simulation. + The World object also initializes the Events for attachment, detachment and for manipulating the world. + + :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" + :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. + """ + self.objects: List[Object] = [] + self.client_id: int = -1 + self.detachment_event: Event = Event() + self.attachment_event: Event = Event() + self.manipulation_event: Event = Event() + self.mode: str = mode + if World.current_world is None: + World.current_world = self + self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} + self.data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] + self.prospection_world: World = World("DIRECT", True) if not is_prospection_world else None + self.world_sync: WorldSync = WorldSync(self, self.prospection_world) if not is_prospection_world else None + self.is_prospection_world: bool = is_prospection_world + self.local_transformer = LocalTransformer() + if not is_prospection_world: + self.world_sync.start() + self.local_transformer.world = self + self.local_transformer.prospection_world = self.prospection_world + + def get_objects_by_name(self, name: str) -> List[Object]: + """ + Returns a list of all Objects in this World with the same name as the given one. + + :param name: The name of the returned Objects. + :return: A list of all Objects with the name 'name'. + """ + return list(filter(lambda obj: obj.name == name, self.objects)) + + def get_objects_by_type(self, obj_type: str) -> List[Object]: + """ + Returns a list of all Objects which have the type 'obj_type'. + + :param obj_type: The type of the returned Objects. + :return: A list of all Objects that have the type 'obj_type'. + """ + return list(filter(lambda obj: obj.type == obj_type, self.objects)) + + def get_object_by_id(self, id: int) -> Object: + """ + Returns the single Object that has the unique id. + + :param id: The unique id for which the Object should be returned. + :return: The Object with the id 'id'. + """ + return list(filter(lambda obj: obj.id == id, self.objects))[0] + + @abstractmethod + def remove_object(self, obj_id: int) -> None: + """ + Remove an object by its ID. + + :param obj_id: The unique id of the object to be removed. + """ + pass + + def add_constraint(self, + parent_obj_id: int, + child_obj_id: int, + parent_link_id: int, + joint_type: int, + parent_to_child_transform: Transform, + joint_axis: List, + child_link_id: int) -> int: + """ + Add a constraint between two objects so that attachment they become attached + """ + cid = p.createConstraint(parent_obj_id, parent_link_id, child_obj_id, child_link_id, joint_type, + joint_axis, parent_to_child_transform.translation_as_list(), [0, 0, 0], + parent_to_child_transform.rotation_as_list(), + physicsClientId=self.client_id) + return cid + def attach_objects(self, + parent_obj: Object, + child_obj: Object, + parent_link: Optional[str] = None, + loose: Optional[bool] = False) -> None: + """ + Attaches another object to this object. This is done by + saving the transformation between the given link, if there is one, and + the base pose of the other object. Additionally, the name of the link, to + which the object is attached, will be saved. + Furthermore, a constraint will be created so the attachment + also works while simulation. + Loose attachments means that the attachment will only be one-directional. + For example, if the parent object moves, the child object will also move, but not the other way around. + + :param parent_obj: The parent object (jf loose, then this would not move when child moves) + :param child_obj: The child object + :param parent_link: The link of the parent object to which the child object should be attached + :param loose: If the attachment should be a loose attachment. + """ + link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 + link_to_object = parent_obj._calculate_transform(child_obj, parent_link) + parent_obj.attachments[child_obj] = [link_to_object, parent_link, loose] + child_obj.attachments[parent_obj] = [link_to_object.invert(), None, False] + + cid = p.createConstraint(parent_obj.id, link_id, child_obj.id, -1, p.JOINT_FIXED, + [0, 1, 0], link_to_object.translation_as_list(), [0, 0, 0], + link_to_object.rotation_as_list(), + physicsClientId=self.client_id) + parent_obj.cids[child_obj] = cid + child_obj.cids[parent_obj] = cid + self.attachment_event(parent_obj, [parent_obj, child_obj]) + + def detach_objects(self, obj1: Object, obj2: Object) -> None: + """ + Detaches obj2 from obj1. This is done by + deleting the attachment from the attachments dictionary of both objects + and deleting the constraint of pybullet. + Afterward the detachment event of the corresponding BulletWorld will be fired. + + :param obj1: The object from which an object should be detached. + :param obj2: The object which should be detached. + """ + del obj1.attachments[obj2] + del obj2.attachments[obj1] + + p.removeConstraint(obj1.cids[obj2], physicsClientId=self.client_id) + + del obj1.cids[obj2] + del obj2.cids[obj1] + obj1.world.detachment_event(obj1, [obj1, obj2]) + + def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: + """ + Updates the positions of all attached objects. This is done + by calculating the new pose in world coordinate frame and setting the + base pose of the attached objects to this new pose. + After this the _set_attached_objects method of all attached objects + will be called. + + :param prev_object: A list of Objects that were already moved, + these will be excluded to prevent loops in the update. + """ + for att_obj in obj.attachments: + if att_obj in prev_object: + continue + if obj.attachments[att_obj][2]: + # Updates the attachment transformation and constraint if the + # attachment is loose, instead of updating the position of all attached objects + link_to_object = obj._calculate_transform(att_obj, obj.attachments[att_obj][1]) + link_id = obj.get_link_id(obj.attachments[att_obj][1]) if obj.attachments[att_obj][1] else -1 + obj.attachments[att_obj][0] = link_to_object + att_obj.attachments[obj][0] = link_to_object.invert() + p.removeConstraint(obj.cids[att_obj], physicsClientId=self.client_id) + cid = p.createConstraint(obj.id, link_id, att_obj.id, -1, p.JOINT_FIXED, [0, 0, 0], + link_to_object.translation_as_list(), + [0, 0, 0], link_to_object.rotation_as_list(), + physicsClientId=self.client_id) + obj.cids[att_obj] = cid + att_obj.cids[obj] = cid + else: + link_to_object = obj.attachments[att_obj][0] + + world_to_object = obj.local_transformer.transform_pose(link_to_object.to_pose(), "map") + p.resetBasePositionAndOrientation(att_obj.id, world_to_object.position_as_list(), + world_to_object.orientation_as_list(), + physicsClientId=self.client_id) + att_obj._current_pose = world_to_object + self._set_attached_objects(att_obj, prev_object + [obj]) + + def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: + """ + Get the joint limits of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + :return: A tuple containing the upper and the lower limits of the joint. + """ + up_lim, low_lim = p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8:10] + return up_lim, low_lim + + def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: + """ + Returns the axis along which a joint is moving. The given joint_name has to be part of this object. + + :param obj: The object + :param joint_name: Name of the joint for which the axis should be returned. + :return: The axis a vector of xyz + """ + return p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[13] + + def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: + """ + Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. + + :param obj: The object + :param joint_name: Joint name for which the type should be returned + :return: The type of the joint + """ + joint_type = p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[2] + return JointType(joint_type) + + def get_object_joint_position(self, obj: Object, joint_name: str) -> float: + """ + Get the state of a joint of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + """ + return p.getJointState(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[0] + + def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: + """ + Get the pose of a link of an articulated object with respect to the world frame. + The pose is given as a tuple of position and orientation. + + :param obj: The object + :param link_name: The name of the link + """ + return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] + + def get_object_contact_points(self, obj: Object) -> List: + """ + Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned + list please look at `PyBullet Doc `_ + + :param obj: The object. + :return: A list of all contact points with other objects + """ + return p.getContactPoints(obj.id) + + def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: + """ + Get the ID of a joint in an articulated object. + + :param obj: The object + :param joint_idx: The index of the joint (would indicate order). + """ + return p.getJointInfo(obj.id, joint_idx, self.client_id)[0] + + def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: + """ + Get the name of a joint in an articulated object. + + :param obj: The object + :param joint_idx: The index of the joint (would indicate order). + """ + return p.getJointInfo(obj.id, joint_idx, self.client_id)[1].decode('utf-8') + + def get_object_link_name(self, obj: Object, link_idx: int) -> str: + """ + Get the name of a link in an articulated object. + + :param obj: The object + :param link_idx: The index of the link (would indicate order). + """ + return p.getJointInfo(obj.id, link_idx, self.client_id)[12].decode('utf-8') + + def get_object_number_of_joints(self, obj: Object) -> int: + """ + Get the number of joints of an articulated object + + :param obj: The object + """ + return p.getNumJoints(obj.id, self.client_id) + + def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + """ + Reset the joint position instantly without physics simulation + + :param obj: The object + :param joint_name: The name of the joint + :param joint_pose: The new joint pose + """ + p.resetJointState(obj.id, obj.joints[joint_name], joint_pose, physicsClientId=self.client_id) + + def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): + """ + 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 position: The new position of the object as a vector of x,y,z + :param orientation: The new orientation of the object as a quaternion of x,y,z,w + """ + p.resetBasePositionAndOrientation(obj.id, position, orientation, self.client_id) + + def step(self): + """ + Step the world simulation using forward dynamics + """ + p.stepSimulation(self.client_id) + + def set_object_color(self, obj: Object, color: List[float], link: Optional[str] = ""): + """ + Changes the color of this object, the color has to be given as a list + of RGBA values. Optionally a link name can can be provided, if no link + name is provided all links of this object will be colored. + + :param obj: The object which should be colored + :param color: The color as RGBA values between 0 and 1 + :param link: The link name of the link which should be colored + """ + if link == "": + # Check if there is only one link, this is the case for primitive + # forms or if loaded from an .stl or .obj file + if obj.links != {}: + for link_id in obj.links.values(): + p.changeVisualShape(obj.id, link_id, rgbaColor=color, physicsClientId=self.client_id) + else: + p.changeVisualShape(obj.id, -1, rgbaColor=color, physicsClientId=self.client_id) + else: + p.changeVisualShape(obj.id, obj.links[link], rgbaColor=color, physicsClientId=self.client_id) + + def get_object_color(self, + obj: Object, + link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: + """ + This method returns the color of this object or a link of this object. If no link is given then the + return is either: + + 1. A list with the color as RGBA values, this is the case if the object only has one link (this + happens for example if the object is spawned from a .obj or .stl file) + 2. A dict with the link name as key and the color as value. The color is given as RGBA values. + Please keep in mind that not every link may have a color. This is dependent on the URDF from which the + object is spawned. + + If a link is specified then the return is a list with RGBA values representing the color of this link. + It may be that this link has no color, in this case the return is None as well as an error message. + + :param obj: The object for which the color should be returned. + :param link: the link name for which the color should be returned. + :return: The color of the object or link, or a dictionary containing every colored link with its color + """ + visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) + swap = {v: k for k, v in obj.links.items()} + links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) + colors = list(map(lambda x: x[7], visual_data)) + link_to_color = dict(zip(links, colors)) + + if link: + if link in link_to_color.keys(): + return link_to_color[link] + elif link not in obj.links.keys(): + rospy.logerr(f"The link '{link}' is not part of this obejct") + return None + else: + rospy.logerr(f"The link '{link}' has no color") + return None + + if len(visual_data) == 1: + return list(visual_data[0][7]) + else: + return link_to_color + + def get_object_AABB(self, obj: Object, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case + the axis aligned bounding box of the link will be returned. The return of this method are two points in + world coordinate frame which define a bounding box. + + :param obj: The object for which the bounding box should be returned. + :param link_name: The Optional name of a link of this object. + :return: Two lists of x,y,z which define the bounding box. + """ + if link_name: + return p.getAABB(obj.id, obj.links[link_name], self.client_id) + else: + return p.getAABB(obj.id, physicsClientId=self.client_id) + + def get_attachment_event(self) -> Event: + """ + Returns the event reference that is fired if an attachment occurs. + + :return: The reference to the attachment event + """ + return self.attachment_event + + def get_detachment_event(self) -> Event: + """ + Returns the event reference that is fired if a detachment occurs. + + :return: The event reference for the detachment event. + """ + return self.detachment_event + + def get_manipulation_event(self) -> Event: + """ + Returns the event reference that is fired if any manipulation occurs. + + :return: The event reference for the manipulation event. + """ + return self.manipulation_event + + def set_realtime(self, real_time: bool) -> None: + """ + Enables the real time simulation of Physic in the BulletWorld. By default this is disabled and Physic is only + simulated to reason about it. + + :param real_time: Whether the BulletWorld should simulate Physic in real time. + """ + p.setRealTimeSimulation(1 if real_time else 0, self.client_id) + + def set_gravity(self, velocity: List[float]) -> None: + """ + Sets the gravity that is used in the BullteWorld, by default the is the gravity on earth ([0, 0, -9.8]). Gravity + is given as a vector in x,y,z. Gravity is only applied while simulating Physic. + + :param velocity: The gravity vector that should be used in the BulletWorld. + """ + p.setGravity(velocity[0], velocity[1], velocity[2], physicsClientId=self.client_id) + + def set_robot(self, robot: Object) -> None: + """ + Sets the global variable for the robot Object. This should be set on spawning the robot. + + :param robot: The Object reference to the Object representing the robot. + """ + BulletWorld.robot = robot + + def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: + """ + Simulates Physic in the BulletWorld for a given amount of seconds. Usually this simulation is faster than real + time, meaning you can simulate for example 10 seconds of Physic in the BulletWorld in 1 second real time. By + setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real + time. + + :param seconds: The amount of seconds that should be simulated. + :param real_time: If the simulation should happen in real time or faster. + """ + for i in range(0, int(seconds * 240)): + p.stepSimulation(self.client_id) + for objects, callback in self.coll_callbacks.items(): + contact_points = p.getContactPoints(objects[0].id, objects[1].id, physicsClientId=self.client_id) + # contact_points = p.getClosestPoints(objects[0].id, objects[1].id, 0.02) + # print(contact_points[0][5]) + if contact_points != (): + callback[0]() + elif callback[1] != None: # Call no collision callback + callback[1]() + if real_time: + # Simulation runs at 240 Hz + time.sleep(0.004167) + + def exit(self) -> None: + """ + Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the + preferred method to close the BulletWorld. + """ + # True if this is NOT the shadow world since it has a reference to the + # Shadow world + time.sleep(0.1) + if self.prospection_world: + self.world_sync.terminate = True + self.world_sync.join() + self.prospection_world.exit() + p.disconnect(self.client_id) + if self._gui_thread: + self._gui_thread.join() + if BulletWorld.current_bullet_world == self: + BulletWorld.current_bullet_world = None + BulletWorld.robot = None + + def save_state(self) -> Tuple[int, Dict]: + """ + Returns the id of the saved state of the BulletWorld. The saved state contains the position, orientation and joint + position of every Object in the BulletWorld. + + :return: A unique id of the state + """ + objects2attached = {} + # ToDo find out what this is for and where it is used + for o in self.objects: + objects2attached[o] = (o.attachments.copy(), o.cids.copy()) + return p.saveState(self.client_id), objects2attached + + def restore_state(self, state, objects2attached: Optional[Dict] = None) -> None: + """ + Restores the state of the BulletWorld according to the given state id. This includes position, orientation and + joint states. However, restore can not respawn objects if there are objects that were deleted between creation of + the state and restoring they will be skiped. + + :param state: The unique id representing the state, as returned by :func:`~save_state` + :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~world.Object.attachments` + """ + p.restoreState(state, physicsClientId=self.client_id) + objects2attached = {} if objects2attached is None else objects2attached + for obj in self.objects: + try: + obj.attachments, obj.cids = objects2attached[obj] + except KeyError: + continue + + def copy(self) -> BulletWorld: + """ + Copies this Bullet World into another and returns it. The other BulletWorld + will be in Direct mode. The shadow world should always be preferred instead of creating a new BulletWorld. + This method should only be used if necessary since there can be unforeseen problems. + + :return: The reference to the new BulletWorld + """ + world = BulletWorld("DIRECT") + for obj in self.objects: + o = Object(obj.name, obj.type, obj.path, Pose(obj.get_position(), obj.get_orientation()), + world, obj.color) + for joint in obj.joints: + o.set_joint_position(joint, obj.get_joint_position(joint)) + return world + + def add_vis_axis(self, pose: Pose, + length: Optional[float] = 0.2) -> None: + """ + Creates a Visual object which represents the coordinate frame at the given + position and orientation. There can be an unlimited amount of vis axis objects. + + :param pose: The pose at which the axis should be spawned + :param length: Optional parameter to configure the length of the axes + """ + + position, orientation = pose.to_list() + + vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], + rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) + vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], + rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) + vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], + rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) + + obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], + basePosition=position, baseOrientation=orientation, + linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], + linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], + linkParentIndices=[0, 0, 0], + linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], + linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], + linkCollisionShapeIndices=[-1, -1, -1]) + + self.vis_axis.append(obj) + + def remove_vis_axis(self) -> None: + """ + Removes all spawned vis axis objects that are currently in this BulletWorld. + """ + for id in self.vis_axis: + p.removeBody(id) + self.vis_axis = [] + + def register_collision_callback(self, objectA: Object, objectB: Object, + callback_collision: Callable, + callback_no_collision: Optional[Callable] = None) -> None: + """ + Registers callback methods for contact between two Objects. There can be a callback for when the two Objects + get in contact and, optionally, for when they are not in contact anymore. + + :param objectA: An object in the BulletWorld + :param objectB: Another object in the BulletWorld + :param callback_collision: A function that should be called if the objects are in contact + :param callback_no_collision: A function that should be called if the objects are not in contact + """ + self.coll_callbacks[(objectA, objectB)] = (callback_collision, callback_no_collision) + + def add_additional_resource_path(self, path: str) -> None: + """ + Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an + Object is spawned only with a filename. + + :param path: A path in the filesystem in which to search for files. + """ + self.data_directory.append(path) + + def get_shadow_object(self, object: Object) -> Object: + """ + Returns the corresponding object from the shadow world for the given object. If the given Object is already in + the shadow world it is returned. + + :param object: The object for which the shadow worlds object should be returned. + :return: The corresponding object in the shadow world. + """ + try: + return self.world_sync.object_mapping[object] + except KeyError: + shadow_world = self if self.is_prospection_world else self.prospection_world + if object in shadow_world.objects: + return object + else: + raise ValueError( + f"There is no shadow object for the given object: {object}, this could be the case if the object isn't anymore in the main (graphical) BulletWorld or if the given object is already a shadow object. ") + + def get_bullet_object_for_shadow(self, object: Object) -> Object: + """ + Returns the corresponding object from the main Bullet World for a given + object in the shadow world. If the given object is not in the shadow + world an error will be raised. + + :param object: The object for which the corresponding object in the main Bullet World should be found + :return: The object in the main Bullet World + """ + map = self.world_sync.object_mapping + try: + return list(map.keys())[list(map.values()).index(object)] + except ValueError: + raise ValueError("The given object is not in the shadow world.") + + def reset_bullet_world(self) -> None: + """ + Resets the BulletWorld to the state it was first spawned in. + All attached objects will be detached, all joints will be set to the + default position of 0 and all objects will be set to the position and + orientation in which they were spawned. + """ + for obj in self.objects: + if obj.attachments: + attached_objects = list(obj.attachments.keys()) + for att_obj in attached_objects: + obj.detach(att_obj) + joint_names = list(obj.joints.keys()) + joint_poses = [0 for j in joint_names] + obj.set_positions_of_all_joints(dict(zip(joint_names, joint_poses))) + obj.set_pose(obj.original_pose) + +class BulletWorld(World): """ The BulletWorld Class represents the physics Simulation and belief state. """ @@ -54,45 +694,29 @@ class BulletWorld: if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): rospy.init_node('pycram') - def __init__(self, type: str = "GUI", is_shadow_world: bool = False): + def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): """ Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. - :param type: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" - :param is_shadow_world: For internal usage, decides if this BulletWorld should be used as a shadow world. + :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" + :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. """ - self.objects: List[Object] = [] - self.client_id: int = -1 - self.detachment_event: Event = Event() - self.attachment_event: Event = Event() - self.manipulation_event: Event = Event() - self.type: str = type - self._gui_thread: Gui = Gui(self, type) + super().__init__(mode=mode, is_prospection_world=is_prospection_world) + + self._gui_thread: Gui = Gui(self, mode) self._gui_thread.start() # This disables file caching from PyBullet, since this would also cache # files that can not be loaded p.setPhysicsEngineParameter(enableFileCaching=0) # Needed to let the other thread start the simulation, before Objects are spawned. time.sleep(0.1) - if BulletWorld.current_bullet_world == None: - BulletWorld.current_bullet_world = self self.vis_axis: List[Object] = [] - self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} - self.data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] - self.shadow_world: BulletWorld = BulletWorld("DIRECT", True) if not is_shadow_world else None - self.world_sync: WorldSync = WorldSync(self, self.shadow_world) if not is_shadow_world else None - self.is_shadow_world: bool = is_shadow_world - self.local_transformer = LocalTransformer() - if not is_shadow_world: - self.world_sync.start() - self.local_transformer.bullet_world = self - self.local_transformer.shadow_world = self.shadow_world # Some default settings self.set_gravity([0, 0, -9.8]) - if not is_shadow_world: + if not is_prospection_world: plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) # atexit.register(self.exit) @@ -504,10 +1128,10 @@ def exit(self) -> None: # True if this is NOT the shadow world since it has a reference to the # Shadow world time.sleep(0.1) - if self.shadow_world: + if self.prospection_world: self.world_sync.terminate = True self.world_sync.join() - self.shadow_world.exit() + self.prospection_world.exit() p.disconnect(self.client_id) if self._gui_thread: self._gui_thread.join() @@ -535,7 +1159,7 @@ def restore_state(self, state, objects2attached: Optional[Dict] = None) -> None: the state and restoring they will be skiped. :param state: The unique id representing the state, as returned by :func:`~save_state` - :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~bullet_world.Object.attachments` + :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~world.Object.attachments` """ p.restoreState(state, physicsClientId=self.client_id) objects2attached = {} if objects2attached is None else objects2attached @@ -635,7 +1259,7 @@ def get_shadow_object(self, object: Object) -> Object: try: return self.world_sync.object_mapping[object] except KeyError: - shadow_world = self if self.is_shadow_world else self.shadow_world + shadow_world = self if self.is_prospection_world else self.prospection_world if object in shadow_world.objects: return object else: @@ -689,7 +1313,7 @@ def __init__(self): self.prev_world: BulletWorld = None def __enter__(self): - if not BulletWorld.current_bullet_world.is_shadow_world: + if not BulletWorld.current_bullet_world.is_prospection_world: time.sleep(20 / 240) # blocks until the adding queue is ready BulletWorld.current_bullet_world.world_sync.add_obj_queue.join() @@ -700,7 +1324,7 @@ def __enter__(self): self.prev_world = BulletWorld.current_bullet_world BulletWorld.current_bullet_world.world_sync.pause_sync = True - BulletWorld.current_bullet_world = BulletWorld.current_bullet_world.shadow_world + BulletWorld.current_bullet_world = BulletWorld.current_bullet_world.prospection_world def __exit__(self, *args): if not self.prev_world == None: @@ -719,11 +1343,11 @@ class WorldSync(threading.Thread): if reasoning should be done in the shadow world. """ - def __init__(self, world: BulletWorld, shadow_world: BulletWorld): + def __init__(self, world: World, prospection_world: World): threading.Thread.__init__(self) - self.world: BulletWorld = world - self.shadow_world: BulletWorld = shadow_world - self.shadow_world.world_sync: WorldSync = self + self.world: World = world + self.prospection_world: World = prospection_world + self.prospection_world.world_sync = self self.terminate: bool = False self.add_obj_queue: Queue = Queue() @@ -747,7 +1371,7 @@ def run(self): # self.equal_states = False for i in range(self.add_obj_queue.qsize()): obj = self.add_obj_queue.get() - # [name, type, path, position, orientation, self.world.shadow_world, color, bulletworld object] + # [name, type, path, position, orientation, self.world.prospection_world, color, bulletworld object] o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) # Maps the BulletWorld object to the shadow world object self.object_mapping[obj[7]] = o @@ -1050,12 +1674,12 @@ def __init__(self, name: str, type: Union[str, ObjectType], path: str, self.cids: Dict[Object, int] = {} self.original_pose = pose_in_map - self.tf_frame = ("shadow/" if self.world.is_shadow_world else "") + self.name + "_" + str(self.id) + self.tf_frame = ("shadow/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) # This means "world" is not the shadow world since it has a reference to a shadow world - if self.world.shadow_world != None: + if self.world.prospection_world != None: self.world.world_sync.add_obj_queue.put( - [name, type, path, position, orientation, self.world.shadow_world, color, self]) + [name, type, path, position, orientation, self.world.prospection_world, color, self]) with open(self.path) as f: self.urdf_object = URDF.from_xml_string(f.read()) @@ -1095,7 +1719,7 @@ def remove(self) -> None: self.world.objects.remove(self) # This means the current world of the object is not the shadow world, since it # has a reference to the shadow world - if self.world.shadow_world is not None: + if self.world.prospection_world is not None: self.world.world_sync.remove_obj_queue.put(self) self.world.world_sync.remove_obj_queue.join() self.world.remove_object(self.id) diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 5c5b41b3b..e196bb51e 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -94,7 +94,7 @@ def test_detect(self): with simulated_robot: detected_object = description.resolve().perform() self.assertEqual(detected_object.name, "milk") - self.assertEqual(detected_object.type, ObjectType.MILK) + self.assertEqual(detected_object.mode, ObjectType.MILK) self.assertEqual(detected_object.bullet_world_object, self.milk) # Skipped since open and close work only in the apartment at the moment From bbb180043654c139961e7663e29a2abdb24bc80c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 5 Dec 2023 17:43:09 +0100 Subject: [PATCH 05/69] [WorldAbstractClass] InProgress , refactoring transform_pose, _calculate_transform, and attach. --- src/pycram/bullet_world.py | 1685 ----------------- src/pycram/costmaps.py | 2 +- src/pycram/designator.py | 2 +- src/pycram/designators/action_designator.py | 20 +- src/pycram/external_interfaces/ik.py | 4 +- src/pycram/external_interfaces/robokudo.py | 2 +- src/pycram/helper.py | 2 +- src/pycram/local_transformer.py | 28 +- .../process_modules/pr2_process_modules.py | 14 +- src/pycram/world.py | 144 +- src/pycram/world_reasoning.py | 2 +- test/local_transformer_tests.py | 4 +- 12 files changed, 151 insertions(+), 1758 deletions(-) delete mode 100644 src/pycram/bullet_world.py diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py deleted file mode 100644 index 75de52e45..000000000 --- a/src/pycram/bullet_world.py +++ /dev/null @@ -1,1685 +0,0 @@ -# used for delayed evaluation of typing until python 3.11 becomes mainstream -from __future__ import annotations - -import logging -import os -import pathlib -import re -import threading -import time -import xml.etree.ElementTree -from queue import Queue -import tf -from typing import List, Optional, Dict, Tuple, Callable -from typing import Union - -import numpy as np -import pybullet as p -import rospkg -import rospy -import rosgraph -import rosnode -import atexit - -import urdf_parser_py.urdf -from geometry_msgs.msg import Quaternion, Point, TransformStamped -from urdf_parser_py.urdf import URDF - -from . import utils -from .event import Event -from .robot_descriptions import robot_description -from .enums import JointType, ObjectType -from .local_transformer import LocalTransformer -from sensor_msgs.msg import JointState - -from .pose import Pose, Transform - - -class BulletWorld: - """ - The BulletWorld Class represents the physics Simulation and belief state. - """ - - current_bullet_world: BulletWorld = None - """ - Global reference to the currently used BulletWorld, usually this is the - graphical one. However, if you are inside a Use_shadow_world() environment the current_bullet_world points to the - shadow world. In this way you can comfortably use the current_bullet_world, which should point towards the BulletWorld - used at the moment. - """ - robot: Object = None - """ - Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the - URDF with the name of the URDF on the parameter server. - """ - - # Check is for sphinx autoAPI to be able to work in a CI workflow - if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): - rospy.init_node('pycram') - - def __init__(self, type: str = "GUI", is_shadow_world: bool = False): - """ - Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the - background. There can only be one rendered simulation. - The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. - - :param type: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" - :param is_shadow_world: For internal usage, decides if this BulletWorld should be used as a shadow world. - """ - self.objects: List[Object] = [] - self.client_id: int = -1 - self.detachment_event: Event = Event() - self.attachment_event: Event = Event() - self.manipulation_event: Event = Event() - self.type: str = type - self._gui_thread: Gui = Gui(self, type) - self._gui_thread.start() - # This disables file caching from PyBullet, since this would also cache - # files that can not be loaded - p.setPhysicsEngineParameter(enableFileCaching=0) - # Needed to let the other thread start the simulation, before Objects are spawned. - time.sleep(0.1) - if BulletWorld.current_bullet_world == None: - BulletWorld.current_bullet_world = self - self.vis_axis: Object = [] - self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} - self.data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] - self.shadow_world: BulletWorld = BulletWorld("DIRECT", True) if not is_shadow_world else None - self.world_sync: WorldSync = WorldSync(self, self.shadow_world) if not is_shadow_world else None - self.is_shadow_world: bool = is_shadow_world - self.local_transformer = LocalTransformer() - if not is_shadow_world: - self.world_sync.start() - self.local_transformer.world = self - self.local_transformer.prospection_world = self.shadow_world - - # Some default settings - self.set_gravity([0, 0, -9.8]) - if not is_shadow_world: - plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - # atexit.register(self.exit) - - def get_objects_by_name(self, name: str) -> List[Object]: - """ - Returns a list of all Objects in this BulletWorld with the same name as the given one. - - :param name: The name of the returned Objects. - :return: A list of all Objects with the name 'name'. - """ - return list(filter(lambda obj: obj.name == name, self.objects)) - - def get_objects_by_type(self, obj_type: str) -> List[Object]: - """ - Returns a list of all Objects which have the type 'obj_type'. - - :param obj_type: The type of the returned Objects. - :return: A list of all Objects that have the type 'obj_type'. - """ - return list(filter(lambda obj: obj.type == obj_type, self.objects)) - - def get_object_by_id(self, id: int) -> Object: - """ - Returns the single Object that has the unique id. - - :param id: The unique id for which the Object should be returned. - :return: The Object with the id 'id'. - """ - return list(filter(lambda obj: obj.id == id, self.objects))[0] - - def get_attachment_event(self) -> Event: - """ - Returns the event reference that is fired if an attachment occurs. - - :return: The reference to the attachment event - """ - return self.attachment_event - - def get_detachment_event(self) -> Event: - """ - Returns the event reference that is fired if a detachment occurs. - - :return: The event reference for the detachment event. - """ - return self.detachment_event - - def get_manipulation_event(self) -> Event: - """ - Returns the event reference that is fired if any manipulation occurs. - - :return: The event reference for the manipulation event. - """ - return self.manipulation_event - - def set_realtime(self, real_time: bool) -> None: - """ - Enables the real time simulation of Physic in the BulletWorld. By default this is disabled and Physic is only - simulated to reason about it. - - :param real_time: Whether the BulletWorld should simulate Physic in real time. - """ - p.setRealTimeSimulation(1 if real_time else 0, self.client_id) - - def set_gravity(self, velocity: List[float]) -> None: - """ - Sets the gravity that is used in the BullteWorld, by default the is the gravity on earth ([0, 0, -9.8]). Gravity - is given as a vector in x,y,z. Gravity is only applied while simulating Physic. - - :param velocity: The gravity vector that should be used in the BulletWorld. - """ - p.setGravity(velocity[0], velocity[1], velocity[2], physicsClientId=self.client_id) - - def set_robot(self, robot: Object) -> None: - """ - Sets the global variable for the robot Object. This should be set on spawning the robot. - - :param robot: The Object reference to the Object representing the robot. - """ - BulletWorld.robot = robot - - def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: - """ - Simulates Physic in the BulletWorld for a given amount of seconds. Usually this simulation is faster than real - time, meaning you can simulate for example 10 seconds of Physic in the BulletWorld in 1 second real time. By - setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real - time. - - :param seconds: The amount of seconds that should be simulated. - :param real_time: If the simulation should happen in real time or faster. - """ - for i in range(0, int(seconds * 240)): - p.stepSimulation(self.client_id) - for objects, callback in self.coll_callbacks.items(): - contact_points = p.getContactPoints(objects[0].id, objects[1].id, physicsClientId=self.client_id) - # contact_points = p.getClosestPoints(objects[0].id, objects[1].id, 0.02) - # print(contact_points[0][5]) - if contact_points != (): - callback[0]() - elif callback[1] != None: # Call no collision callback - callback[1]() - if real_time: - # Simulation runs at 240 Hz - time.sleep(0.004167) - - def exit(self) -> None: - """ - Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the - preferred method to close the BulletWorld. - """ - # True if this is NOT the shadow world since it has a reference to the - # Shadow world - time.sleep(0.1) - if self.shadow_world: - self.world_sync.terminate = True - self.world_sync.join() - self.shadow_world.exit() - p.disconnect(self.client_id) - if self._gui_thread: - self._gui_thread.join() - if BulletWorld.current_bullet_world == self: - BulletWorld.current_bullet_world = None - BulletWorld.robot = None - - def save_state(self) -> int: - """ - Returns the id of the saved state of the BulletWorld. The saved state contains the position, orientation and joint - position of every Object in the BulletWorld. - - :return: A unique id of the state - """ - objects2attached = {} - # ToDo find out what this is for and where it is used - for o in self.objects: - objects2attached[o] = (o.attachments.copy(), o.cids.copy()) - return p.saveState(self.client_id), objects2attached - - def restore_state(self, state, objects2attached: Dict = {}) -> None: - """ - Restores the state of the BulletWorld according to the given state id. This includes position, orientation and - joint states. However, restore can not respawn objects if there are objects that were deleted between creation of - the state and restoring they will be skiped. - - :param state: The unique id representing the state, as returned by :func:`~save_state` - :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~world.Object.attachments` - """ - p.restoreState(state, physicsClientId=self.client_id) - for obj in self.objects: - try: - obj.attachments, obj.cids = objects2attached[obj] - except KeyError: - continue - - def copy(self) -> BulletWorld: - """ - Copies this Bullet World into another and returns it. The other BulletWorld - will be in Direct mode. The shadow world should always be preferred instead of creating a new BulletWorld. - This method should only be used if necessary since there can be unforeseen problems. - - :return: The reference to the new BulletWorld - """ - world = BulletWorld("DIRECT") - for obj in self.objects: - o = Object(obj.name, obj.type, obj.path, obj.get_position(), obj.get_orientation(), - world, obj.color) - for joint in obj.joints: - o.set_joint_position(joint, obj.get_joint_position(joint)) - return world - - def add_vis_axis(self, pose: Pose, - length: Optional[float] = 0.2) -> None: - """ - Creates a Visual object which represents the coordinate frame at the given - position and orientation. There can be an unlimited amount of vis axis objects. - - :param pose: The pose at which the axis should be spawned - :param length: Optional parameter to configure the length of the axes - """ - - position, orientation = pose.to_list() - - vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], - rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) - vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], - rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) - vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], - rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) - - obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], - basePosition=position, baseOrientation=orientation, - linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkParentIndices=[0, 0, 0], - linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], - linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], - linkCollisionShapeIndices=[-1, -1, -1]) - - self.vis_axis.append(obj) - - def remove_vis_axis(self) -> None: - """ - Removes all spawned vis axis objects that are currently in this BulletWorld. - """ - for id in self.vis_axis: - p.removeBody(id) - self.vis_axis = [] - - def register_collision_callback(self, objectA: Object, objectB: Object, - callback_collision: Callable, - callback_no_collision: Optional[Callable] = None) -> None: - """ - Registers callback methods for contact between two Objects. There can be a callback for when the two Objects - get in contact and, optionally, for when they are not in contact anymore. - - :param objectA: An object in the BulletWorld - :param objectB: Another object in the BulletWorld - :param callback_collision: A function that should be called if the objects are in contact - :param callback_no_collision: A function that should be called if the objects are not in contact - """ - self.coll_callbacks[(objectA, objectB)] = (callback_collision, callback_no_collision) - - def add_additional_resource_path(self, path: str) -> None: - """ - Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an - Object is spawned only with a filename. - - :param path: A path in the filesystem in which to search for files. - """ - self.data_directory.append(path) - - def get_shadow_object(self, object: Object) -> Object: - """ - Returns the corresponding object from the shadow world for the given object. If the given Object is already in - the shadow world it is returned. - - :param object: The object for which the shadow worlds object should be returned. - :return: The corresponding object in the shadow world. - """ - try: - return self.world_sync.object_mapping[object] - except KeyError: - shadow_world = self if self.is_shadow_world else self.shadow_world - if object in shadow_world.objects: - return object - else: - raise ValueError( - f"There is no shadow object for the given object: {object}, this could be the case if the object isn't anymore in the main (graphical) BulletWorld or if the given object is already a shadow object. ") - - def get_bullet_object_for_shadow(self, object: Object) -> Object: - """ - Returns the corresponding object from the main Bullet World for a given - object in the shadow world. If the given object is not in the shadow - world an error will be raised. - - :param object: The object for which the corresponding object in the main Bullet World should be found - :return: The object in the main Bullet World - """ - map = self.world_sync.object_mapping - try: - return list(map.keys())[list(map.values()).index(object)] - except ValueError: - raise ValueError("The given object is not in the shadow world.") - - def reset_bullet_world(self) -> None: - """ - Resets the BulletWorld to the state it was first spawned in. - All attached objects will be detached, all joints will be set to the - default position of 0 and all objects will be set to the position and - orientation in which they were spawned. - """ - for obj in self.objects: - if obj.attachments: - attached_objects = list(obj.attachments.keys()) - for att_obj in attached_objects: - obj.detach(att_obj) - joint_names = list(obj.joints.keys()) - joint_poses = [0 for j in joint_names] - obj.set_joint_positions(dict(zip(joint_names, joint_poses))) - obj.set_pose(obj.original_pose) - - -class Use_shadow_world(): - """ - An environment for using the shadow world, while in this environment the :py:attr:`~BulletWorld.current_bullet_world` - variable will point to the shadow world. - - Example: - with Use_shadow_world(): - NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() - """ - - def __init__(self): - self.prev_world: BulletWorld = None - - def __enter__(self): - if not BulletWorld.current_bullet_world.is_shadow_world: - time.sleep(20 / 240) - # blocks until the adding queue is ready - BulletWorld.current_bullet_world.world_sync.add_obj_queue.join() - # **This is currently not used since the sleep(20/240) seems to be enough, but on weaker hardware this might - # not be a feasible solution** - # while not BulletWorld.current_bullet_world.world_sync.equal_states: - # time.sleep(0.1) - - self.prev_world = BulletWorld.current_bullet_world - BulletWorld.current_bullet_world.world_sync.pause_sync = True - BulletWorld.current_bullet_world = BulletWorld.current_bullet_world.shadow_world - - def __exit__(self, *args): - if not self.prev_world == None: - BulletWorld.current_bullet_world = self.prev_world - BulletWorld.current_bullet_world.world_sync.pause_sync = False - - -class WorldSync(threading.Thread): - """ - Synchronizes the state between the BulletWorld and its shadow world. - Meaning the cartesian and joint position of everything in the shadow world will be - synchronized with the main BulletWorld. - Adding and removing objects is done via queues, such that loading times of objects - in the shadow world does not affect the BulletWorld. - The class provides the possibility to pause the synchronization, this can be used - if reasoning should be done in the shadow world. - """ - - def __init__(self, world: BulletWorld, shadow_world: BulletWorld): - threading.Thread.__init__(self) - self.world: BulletWorld = world - self.shadow_world: BulletWorld = shadow_world - self.shadow_world.world_sync: WorldSync = self - - self.terminate: bool = False - self.add_obj_queue: Queue = Queue() - self.remove_obj_queue: Queue = Queue() - self.pause_sync: bool = False - # Maps bullet to shadow world objects - self.object_mapping: Dict[Object, Object] = {} - self.equal_states = False - - def run(self): - """ - Main method of the synchronization, this thread runs in a loop until the - terminate flag is set. - While this loop runs it continuously checks the cartesian and joint position of - every object in the BulletWorld and updates the corresponding object in the - shadow world. When there are entries in the adding or removing queue the corresponding objects will be added - or removed in the same iteration. - """ - while not self.terminate: - self.check_for_pause() - # self.equal_states = False - for i in range(self.add_obj_queue.qsize()): - obj = self.add_obj_queue.get() - # [name, type, path, position, orientation, self.world.prospection_world, color, bulletworld object] - o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) - # Maps the BulletWorld object to the shadow world object - self.object_mapping[obj[7]] = o - self.add_obj_queue.task_done() - for i in range(self.remove_obj_queue.qsize()): - obj = self.remove_obj_queue.get() - # Get shadow world object reference from object mapping - shadow_obj = self.object_mapping[obj] - shadow_obj.remove() - del self.object_mapping[obj] - self.remove_obj_queue.task_done() - - for bulletworld_obj, shadow_obj in self.object_mapping.items(): - b_pose = bulletworld_obj.get_pose() - s_pose = shadow_obj.get_pose() - if b_pose.dist(s_pose) != 0.0: - shadow_obj.set_pose(bulletworld_obj.get_pose()) - - # Manage joint positions - if len(bulletworld_obj.joints) > 2: - for joint_name in bulletworld_obj.joints.keys(): - if shadow_obj.get_joint_position(joint_name) != bulletworld_obj.get_joint_position(joint_name): - shadow_obj.set_joint_positions(bulletworld_obj.get_positions_of_all_joints()) - break - - self.check_for_pause() - # self.check_for_equal() - time.sleep(1 / 240) - - self.add_obj_queue.join() - self.remove_obj_queue.join() - - def check_for_pause(self) -> None: - """ - Checks if :py:attr:`~self.pause_sync` is true and sleeps this thread until it isn't anymore. - """ - while self.pause_sync: - time.sleep(0.1) - - def check_for_equal(self) -> None: - """ - Checks if both BulletWorlds have the same state, meaning all objects are in the same position. - This is currently not used, but might be used in the future if synchronization issues worsen. - """ - eql = True - for obj, shadow_obj in self.object_mapping.items(): - eql = eql and obj.get_pose() == shadow_obj.get_pose() - self.equal_states = eql - - -class Gui(threading.Thread): - """ - For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~BulletWorld.exit` - Also contains the code for controlling the camera. - """ - - def __init__(self, world, type): - threading.Thread.__init__(self) - self.world: BulletWorld = world - self.type: str = type - - def run(self): - """ - Initializes the new simulation and checks in an endless loop - if it is still active. If it is the thread will be suspended for 1/80 seconds, if it is not the method and - thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. - """ - if self.type != "GUI": - self.world.client_id = p.connect(p.DIRECT) - else: - self.world.client_id = p.connect(p.GUI) - - # Disable the side windows of the GUI - p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) - # Change the init camera pose - p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, - cameraTargetPosition=[-2, 0, 1]) - - # Get the initial camera target location - cameraTargetPosition = p.getDebugVisualizerCamera()[11] - - sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1]) - - # Create a sphere with a radius of 0.05 and a mass of 0 - sphereUid = p.createMultiBody(baseMass=0.0, - baseInertialFramePosition=[0, 0, 0], - baseVisualShapeIndex=sphereVisualId, - basePosition=cameraTargetPosition) - - # Define the maxSpeed, used in calculations - maxSpeed = 16 - - # Set initial Camera Rotation - cameraYaw = 50 - cameraPitch = -35 - - # Keep track of the mouse state - mouseState = [0, 0, 0] - oldMouseX, oldMouseY = 0, 0 - - # Determines if the sphere at cameraTargetPosition is visible - visible = 1 - - # Loop to update the camera position based on keyboard events - while p.isConnected(self.world.client_id): - # Monitor user input - keys = p.getKeyboardEvents() - mouse = p.getMouseEvents() - - # Get infos about the camera - width, height, dist = p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], \ - p.getDebugVisualizerCamera()[10] - cameraTargetPosition = p.getDebugVisualizerCamera()[11] - - # Get vectors used for movement on x,y,z Vector - xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] - yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] - zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] - - # Check the mouse state - if mouse: - for m in mouse: - - mouseX = m[2] - mouseY = m[1] - - # update mouseState - # Left Mouse button - if m[0] == 2 and m[3] == 0: - mouseState[0] = m[4] - # Middle mouse butto (scroll wheel) - if m[0] == 2 and m[3] == 1: - mouseState[1] = m[4] - # right mouse button - if m[0] == 2 and m[3] == 2: - mouseState[2] = m[4] - - # change visibility by clicking the mousewheel - if m[4] == 6 and m[3] == 1 and visible == 1: - visible = 0 - elif m[4] == 6 and visible == 0: - visible = 1 - - # camera movement when the left mouse button is pressed - if mouseState[0] == 3: - speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < maxSpeed else maxSpeed - speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < maxSpeed else maxSpeed - - # max angle of 89.5 and -89.5 to make sure the camera does not flip (is annoying) - if mouseX < oldMouseX: - if (cameraPitch + speedX) < 89.5: - cameraPitch += (speedX / 4) + 1 - elif mouseX > oldMouseX: - if (cameraPitch - speedX) > -89.5: - cameraPitch -= (speedX / 4) + 1 - - if mouseY < oldMouseY: - cameraYaw += (speedY / 4) + 1 - elif mouseY > oldMouseY: - cameraYaw -= (speedY / 4) + 1 - - if mouseState[1] == 3: - speedX = abs(oldMouseX - mouseX) - factor = 0.05 - - if mouseX < oldMouseX: - dist = dist - speedX * factor - elif mouseX > oldMouseX: - dist = dist + speedX * factor - dist = max(dist, 0.1) - - # camera movement when the right mouse button is pressed - if mouseState[2] == 3: - speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < 5 else 5 - speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < 5 else 5 - factor = 0.05 - - if mouseX < oldMouseX: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(zVec, factor), speedX)) - elif mouseX > oldMouseX: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(zVec, factor), speedX)) - - if mouseY < oldMouseY: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(xVec, factor), speedY)) - elif mouseY > oldMouseY: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(xVec, factor), speedY)) - # update oldMouse values - oldMouseY, oldMouseX = mouseY, mouseX - - # check the keyboard state - if keys: - # if shift is pressed, double the speed - if p.B3G_SHIFT in keys: - speedMult = 5 - else: - speedMult = 2.5 - - # if control is pressed, the movements caused by the arrowkeys, the '+' as well as the '-' key - # change - if p.B3G_CONTROL in keys: - - # the up and down arrowkeys cause the targetPos to move along the z axis of the map - if p.B3G_DOWN_ARROW in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(zVec, 0.03), speedMult)) - elif p.B3G_UP_ARROW in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(zVec, 0.03), speedMult)) - - # left and right arrowkeys cause the targetPos to move horizontally relative to the camera - if p.B3G_LEFT_ARROW in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(xVec, 0.03), speedMult)) - elif p.B3G_RIGHT_ARROW in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(xVec, 0.03), speedMult)) - - # the '+' and '-' keys cause the targetpos to move forwards and backwards relative to the camera - # while the camera stays at a constant distance. SHIFT + '=' is for US layout - if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(yVec, 0.03), speedMult)) - elif ord("-") in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(yVec, 0.03), speedMult)) - - # standard bindings for thearrowkeys, the '+' as well as the '-' key - else: - - # left and right arrowkeys cause the camera to rotate around the yaw axis - if p.B3G_RIGHT_ARROW in keys: - cameraYaw += (360 / width) * speedMult - elif p.B3G_LEFT_ARROW in keys: - cameraYaw -= (360 / width) * speedMult - - # the up and down arrowkeys cause the camera to rotate around the pitch axis - if p.B3G_DOWN_ARROW in keys: - if (cameraPitch + (360 / height) * speedMult) < 89.5: - cameraPitch += (360 / height) * speedMult - elif p.B3G_UP_ARROW in keys: - if (cameraPitch - (360 / height) * speedMult) > -89.5: - cameraPitch -= (360 / height) * speedMult - - # the '+' and '-' keys cause the camera to zoom towards and away from the targetPos without - # moving it. SHIFT + '=' is for US layout since the events can't handle shift plus something - if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: - if (dist - (dist * 0.02) * speedMult) > 0.1: - dist -= dist * 0.02 * speedMult - elif ord("-") in keys: - dist += dist * 0.02 * speedMult - - p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, - cameraTargetPosition=cameraTargetPosition) - if visible == 0: - cameraTargetPosition = (0.0, -50, 50) - p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) - time.sleep(1. / 80.) - - -class Object: - """ - Represents a spawned Object in the BulletWorld. - """ - - def __init__(self, name: str, type: Union[str, ObjectType], path: str, - pose: Pose = None, - world: BulletWorld = None, - color: Optional[List[float]] = [1, 1, 1, 1], - ignoreCachedFiles: Optional[bool] = False): - """ - The constructor loads the urdf file into the given BulletWorld, if no BulletWorld is specified the - :py:attr:`~BulletWorld.current_bullet_world` will be used. It is also possible to load .obj and .stl file into the BulletWorld. - The color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. - - :param name: The name of the object - :param type: The type of the object - :param path: The path to the source file, if only a filename is provided then the resourcer directories will be searched - :param pose: The pose at which the Object should be spawned - :param world: The BulletWorld in which the object should be spawned, if no world is specified the :py:attr:`~BulletWorld.current_bullet_world` will be used - :param color: The color with which the object should be spawned. - :param ignoreCachedFiles: If true the file will be spawned while ignoring cached files. - """ - if not pose: - pose = Pose() - self.world: BulletWorld = world if world is not None else BulletWorld.current_bullet_world - self.local_transformer = LocalTransformer() - self.name: str = name - self.type: str = type - self.color: List[float] = color - pose_in_map = self.local_transformer.transform_pose(pose, "map") - position, orientation = pose_in_map.to_list() - self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignoreCachedFiles) - self.joints: Dict[str, int] = self._joint_or_link_name_to_id("joint") - self.links: Dict[str, int] = self._joint_or_link_name_to_id("link") - self.attachments: Dict[Object, List] = {} - self.cids: Dict[Object, int] = {} - self.original_pose = pose_in_map - - self.tf_frame = ("shadow/" if self.world.is_shadow_world else "") + self.name + "_" + str(self.id) - - # This means "world" is not the shadow world since it has a reference to a shadow world - if self.world.shadow_world != None: - self.world.world_sync.add_obj_queue.put( - [name, type, path, position, orientation, self.world.shadow_world, color, self]) - - with open(self.path) as f: - self.urdf_object = URDF.from_xml_string(f.read()) - if self.urdf_object.name == robot_description.name and not BulletWorld.robot: - BulletWorld.robot = self - - self.links[self.urdf_object.get_root()] = -1 - - self._current_pose = pose_in_map - self._current_link_poses = {} - self._current_link_transforms = {} - self._current_joints_positions = {} - self._init_current_positions_of_joints() - self._update_link_poses() - - self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) - self.local_transformer.update_transforms_for_object(self) - self.link_to_geometry = self._get_geometry_for_link() - - self.world.objects.append(self) - - def __repr__(self): - skip_attr = ["links", "joints", "urdf_object", "attachments", "cids", "_current_link_poses", - "_current_link_transforms", "link_to_geometry"] - return self.__class__.__qualname__ + f"(" + ', \n'.join( - [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" - - def remove(self) -> None: - """ - Removes this object from the BulletWorld it currently resides in. - For the object to be removed it has to be detached from all objects it - is currently attached to. After this is done a call to PyBullet is done - to remove this Object from the simulation. - """ - for obj in self.attachments.keys(): - self.detach(obj) - self.world.objects.remove(self) - # This means the current world of the object is not the shaow world, since it - # has a reference to the shadow world - if self.world.shadow_world != None: - self.world.world_sync.remove_obj_queue.put(self) - self.world.world_sync.remove_obj_queue.join() - p.removeBody(self.id, physicsClientId=self.world.client_id) - if BulletWorld.robot == self: - BulletWorld.robot = None - - def attach(self, object: Object, link: Optional[str] = None, loose: Optional[bool] = False) -> None: - """ - Attaches another object to this object. This is done by - saving the transformation between the given link, if there is one, and - the base pose of the other object. Additionally, the name of the link, to - which the object is attached, will be saved. - Furthermore, a constraint of pybullet will be created so the attachment - also works while simulation. - Loose attachments means that the attachment will only be one-directional. For example, if this object moves the - other, attached, object will also move but not the other way around. - - :param object: The other object that should be attached - :param link: The link of this object to which the other object should be - :param loose: If the attachment should be a loose attachment. - """ - link_id = self.get_link_id(link) if link else -1 - link_to_object = self._calculate_transform(object, link) - self.attachments[object] = [link_to_object, link, loose] - object.attachments[self] = [link_to_object.invert(), None, False] - - cid = p.createConstraint(self.id, link_id, object.id, -1, p.JOINT_FIXED, - [0, 1, 0], link_to_object.translation_as_list(), [0, 0, 0], - link_to_object.rotation_as_list(), - physicsClientId=self.world.client_id) - self.cids[object] = cid - object.cids[self] = cid - self.world.attachment_event(self, [self, object]) - - def detach(self, object: Object) -> None: - """ - Detaches another object from this object. This is done by - deleting the attachment from the attachments dictionary of both objects - and deleting the constraint of pybullet. - Afterward the detachment event of the corresponding BulletWorld will be fired. - - :param object: The object which should be detached - """ - del self.attachments[object] - del object.attachments[self] - - p.removeConstraint(self.cids[object], physicsClientId=self.world.client_id) - - del self.cids[object] - del object.cids[self] - self.world.detachment_event(self, [self, object]) - - def detach_all(self) -> None: - """ - Detach all objects attached to this object. - """ - attachments = self.attachments.copy() - for att in attachments.keys(): - self.detach(att) - - def get_position(self) -> Point: - """ - Returns the position of this Object as a list of xyz. - - :return: The current position of this object - """ - return self.get_pose().position - - def get_orientation(self) -> Quaternion: - """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. - - :return: A list of xyzw - """ - return self.get_pose().orientation - - def get_pose(self) -> Pose: - """ - Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. - - :return: The current pose of this object - """ - return self._current_pose - - def set_pose(self, pose: Pose, base: bool = False) -> None: - """ - Sets the Pose of the object. - - :param pose: New Pose for the object - :param base: If True places the object base instead of origin at the specified position and orientation - """ - pose_in_map = self.local_transformer.transform_pose(pose, "map") - position, orientation = pose_in_map.to_list() - if base: - position = np.array(position) + self.base_origin_shift - p.resetBasePositionAndOrientation(self.id, position, orientation, self.world.client_id) - self._current_pose = pose_in_map - self._update_link_poses() - self._set_attached_objects([self]) - - @property - def pose(self) -> Pose: - """ - Property that returns the current position of this Object. - - :return: The position as a list of xyz - """ - return self.get_pose() - - @pose.setter - def pose(self, value: Pose) -> None: - """ - Sets the Pose of the Object to the given value. Function for attribute use. - - :param value: New Pose of the Object - """ - self.set_pose(value) - - def move_base_to_origin_pos(self) -> None: - """ - Move the object such that its base will be at the current origin position. - This is useful when placing objects on surfaces where you want the object base in contact with the surface. - """ - self.set_pose(self.get_pose(), base=True) - - def _set_attached_objects(self, prev_object: List[Object]) -> None: - """ - Updates the positions of all attached objects. This is done - by calculating the new pose in world coordinate frame and setting the - base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. - - :param prev_object: A list of Objects that were already moved, these will be excluded to prevent loops in the update. - """ - for obj in self.attachments: - if obj in prev_object: - continue - if self.attachments[obj][2]: - # Updates the attachment transformation and contraint if the - # attachment is loos, instead of updating the position of all attached objects - link_to_object = self._calculate_transform(obj, self.attachments[obj][1]) - link_id = self.get_link_id(self.attachments[obj][1]) if self.attachments[obj][1] else -1 - self.attachments[obj][0] = link_to_object - obj.attachments[self][0] = link_to_object.invert() - p.removeConstraint(self.cids[obj], physicsClientId=self.world.client_id) - cid = p.createConstraint(self.id, link_id, obj.id, -1, p.JOINT_FIXED, [0, 0, 0], - link_to_object.translation_as_list(), - [0, 0, 0], link_to_object.rotation_as_list(), - physicsClientId=self.world.client_id) - self.cids[obj] = cid - obj.cids[self] = cid - else: - link_to_object = self.attachments[obj][0] - - world_to_object = self.local_transformer.transform_pose(link_to_object.to_pose(), "map") - p.resetBasePositionAndOrientation(obj.id, world_to_object.position_as_list(), - world_to_object.orientation_as_list(), - physicsClientId=self.world.client_id) - obj._current_pose = world_to_object - obj._set_attached_objects(prev_object + [self]) - - def _calculate_transform(self, obj: Object, link: str = None) -> Transform: - """ - Calculates the transformation between another object and the given - link of this object. If no link is provided then the base position will be used. - - :param obj: The other object for which the transformation should be calculated - :param link: The optional link name - :return: The transformation from the link (or base position) to the other objects base position - """ - transform = self.local_transformer.transform_to_object_frame(obj.pose, self, link) - - return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) - - def set_position(self, position: Union[Pose, Point], base=False) -> None: - """ - Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position - instead of the origin in the center of the Object. The given position can either be a Pose, in this case only the - position is used or a geometry_msgs.msg/Point which is the position part of a Pose. - - :param position: Target position as xyz. - :param base: If the bottom of the Object should be placed or the origin in the center. - """ - pose = Pose() - if type(position) == Pose: - target_position = position.position - pose.frame = position.frame - else: - target_position = position - - pose.pose.position = target_position - pose.pose.orientation = self.get_orientation() - self.set_pose(pose, base=base) - - def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: - """ - Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only - the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. - - :param orientation: Target orientation given as a list of xyzw. - """ - pose = Pose() - if type(orientation) == Pose: - target_orientation = orientation.orientation - pose.frame = orientation.frame - else: - target_orientation = orientation - - pose.pose.position = self.get_position() - pose.pose.orientation = target_orientation - self.set_pose(pose) - - def _joint_or_link_name_to_id(self, type: str) -> Dict[str, int]: - """ - Creates a dictionary which maps the link or joint name to the unique ids used by pybullet. - - :param type: Determines if the dictionary should be for joints or links - :return: A dictionary that maps joint or link names to unique ids - """ - nJoints = p.getNumJoints(self.id, self.world.client_id) - joint_name_to_id = {} - info = 1 if type == "joint" else 12 - for i in range(0, nJoints): - joint_info = p.getJointInfo(self.id, i, self.world.client_id) - joint_name_to_id[joint_info[info].decode('utf-8')] = joint_info[0] - return joint_name_to_id - - def get_joint_id(self, name: str) -> int: - """ - Returns the unique id for a joint name. As used by PyBullet. - - :param name: The joint name - :return: The unique id - """ - return self.joints[name] - - def get_link_id(self, name: str) -> int: - """ - Returns a unique id for a link name. As used by PyBullet. - - :param name: The link name - :return: The unique id - """ - return self.links[name] - - def get_link_by_id(self, id: int) -> str: - """ - Returns the name of a link for a given unique PyBullet id - - :param id: PyBullet id for link - :return: The link name - """ - return dict(zip(self.links.values(), self.links.keys()))[id] - - def get_joint_by_id(self, id: int) -> str: - """ - Returns the joint name for a unique PyBullet id - - :param id: The Pybullet id of for joint - :return: The joint name - """ - return dict(zip(self.joints.values(), self.joints.keys()))[id] - - def get_link_relative_to_other_link(self, source_frame: str, target_frame: str) -> Pose: - """ - Calculates the position of a link in the coordinate frame of another link. - - :param source_frame: The name of the source frame - :param target_frame: The name of the target frame - :return: The pose of the source frame in the target frame - """ - source_pose = self.get_link_pose(source_frame) - return self.local_transformer.transform_to_object_frame(source_pose, self, target_frame) - - def get_link_position(self, name: str) -> Point: - """ - Returns the position of a link of this Object. Position is returned as a list of xyz. - - :param name: The link name - :return: The link position as xyz - """ - return self.get_link_pose(name).position - - def get_link_orientation(self, name: str) -> Quaternion: - """ - Returns the orientation of a link of this Object. Orientation is returned as a quaternion. - - :param name: The name of the link - :return: The orientation of the link as a quaternion - """ - return self.get_link_pose(name).orientation - - def get_link_pose(self, name: str) -> Pose: - """ - Returns a Pose of the link corresponding to the given name. The returned Pose will be in world coordinate frame. - - :param name: Link name for which a Pose should returned - :return: The pose of the link - """ - if name in self.links.keys() and self.links[name] == -1: - return self.get_pose() - return self._current_link_poses[name] - # return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6]) - - def set_joint_position(self, joint_name: str, joint_pose: float) -> None: - """ - Sets the state of the given joint to the given joint pose. If the pose is outside the joint limits, as stated - in the URDF, an error will be printed. However, the joint will be set either way. - - :param joint_name: The name of the joint - :param joint_pose: The target pose for this joint - """ - # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly - up_lim, low_lim = p.getJointInfo(self.id, self.joints[joint_name], physicsClientId=self.world.client_id)[ - 8:10] - if low_lim > up_lim: - low_lim, up_lim = up_lim, low_lim - if not low_lim <= joint_pose <= up_lim: - logging.error( - f"The joint position has to be within the limits of the joint. The joint limits for {joint_name} are {low_lim} and {up_lim}") - logging.error(f"The given joint position was: {joint_pose}") - # Temporarily disabled because kdl outputs values exciting joint limits - # return - p.resetJointState(self.id, self.joints[joint_name], joint_pose, physicsClientId=self.world.client_id) - self._current_joints_positions[joint_name] = joint_pose - # self.local_transformer.update_transforms_for_object(self) - self._update_link_poses() - self._set_attached_objects([self]) - - def set_joint_positions(self, joint_poses: dict) -> None: - """ - Sets the current state of multiple joints at once, this method should be preferred when setting multiple joints - at once instead of running :func:`~Object.set_joint_state` in a loop. - - :param joint_poses: - :return: - """ - for joint_name, joint_pose in joint_poses.items(): - p.resetJointState(self.id, self.joints[joint_name], joint_pose, physicsClientId=self.world.client_id) - self._current_joints_positions[joint_name] = joint_pose - self._update_link_poses() - self._set_attached_objects([self]) - - def get_joint_position(self, joint_name: str) -> float: - """ - Returns the joint state for the given joint name. - - :param joint_name: The name of the joint - :return: The current pose of the joint - """ - return self._current_joints_positions[joint_name] - - def contact_points(self) -> List: - """ - Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ - - :return: A list of all contact points with other objects - """ - return p.getContactPoints(self.id) - - def contact_points_simulated(self) -> List: - """ - Returns a list of all contact points between this Object and other Objects after stepping the simulation once. - For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ - - :return: A list of contact points between this Object and other Objects - """ - s = self.world.save_state() - p.stepSimulation(self.world.client_id) - contact_points = self.contact_points() - self.world.restore_state(*s) - return contact_points - - def update_joints_from_topic(self, topic_name: str) -> None: - """ - Updates the joints of this object with positions obtained from a topic with the message type JointState. - Joint names on the topic have to correspond to the joints of this object otherwise an error message will be logged. - - :param topic_name: Name of the topic with the joint states - """ - msg = rospy.wait_for_message(topic_name, JointState) - joint_names = msg.name - joint_positions = msg.position - if set(joint_names).issubset(self.joints.keys()): - for i in range(len(joint_names)): - self.set_joint_position(joint_names[i], joint_positions[i]) - else: - add_joints = set(joint_names) - set(self.joints.keys()) - rospy.logerr(f"There are joints in the published joint state which are not in this model: /n \ - The following joint{'s' if len(add_joints) != 1 else ''}: {add_joints}") - - def update_pose_from_tf(self, frame: str) -> None: - """ - Updates the pose of this object from a TF message. - - :param frame: Name of the TF frame from which the position should be taken - """ - tf_listener = tf.TransformListener() - time.sleep(0.5) - position, orientation = tf_listener.lookupTransform(frame, "map", rospy.Time(0)) - position = [position[0][0] * -1, - position[0][1] * -1, - position[0][2]] - self.set_position(Pose(position, orientation)) - - def set_color(self, color: List[float], link: Optional[str] = "") -> None: - """ - Changes the color of this object, the color has to be given as a list - of RGBA values. Optionally a link name can can be provided, if no link - name is provided all links of this object will be colored. - - :param color: The color as RGBA values between 0 and 1 - :param link: The link name of the link which should be colored - """ - if link == "": - # Check if there is only one link, this is the case for primitive - # forms or if loaded from an .stl or .obj file - if self.links != {}: - for link_id in self.links.values(): - p.changeVisualShape(self.id, link_id, rgbaColor=color, physicsClientId=self.world.client_id) - else: - p.changeVisualShape(self.id, -1, rgbaColor=color, physicsClientId=self.world.client_id) - else: - p.changeVisualShape(self.id, self.links[link], rgbaColor=color, physicsClientId=self.world.client_id) - - def get_color(self, link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: - """ - This method returns the color of this object or a link of this object. If no link is given then the - return is either: - - 1. A list with the color as RGBA values, this is the case if the object only has one link (this - happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the color as value. The color is given as RGBA values. - Please keep in mind that not every link may have a color. This is dependent on the URDF from which the - object is spawned. - - If a link is specified then the return is a list with RGBA values representing the color of this link. - It may be that this link has no color, in this case the return is None as well as an error message. - - :param link: the link name for which the color should be returned. - :return: The color of the object or link, or a dictionary containing every colored link with its color - """ - visual_data = p.getVisualShapeData(self.id, physicsClientId=self.world.client_id) - swap = {v: k for k, v in self.links.items()} - links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) - colors = list(map(lambda x: x[7], visual_data)) - link_to_color = dict(zip(links, colors)) - - if link: - if link in link_to_color.keys(): - return link_to_color[link] - elif link not in self.links.keys(): - rospy.logerr(f"The link '{link}' is not part of this obejct") - return None - else: - rospy.logerr(f"The link '{link}' has no color") - return None - - if len(visual_data) == 1: - return list(visual_data[0][7]) - else: - return link_to_color - - def get_AABB(self, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: - """ - Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case - the axis aligned bounding box of the link will be returned. The return of this method are two points in - world coordinate frame which define a bounding box. - - :param link_name: The Optional name of a link of this object. - :return: Two lists of x,y,z which define the bounding box. - """ - if link_name: - return p.getAABB(self.id, self.links[link_name], self.world.client_id) - else: - return p.getAABB(self.id, physicsClientId=self.world.client_id) - - def get_base_origin(self, link_name: Optional[str] = None) -> Pose: - """ - Returns the origin of the base/bottom of an object/link - - :param link_name: The link name for which the bottom position should be returned - :return: The position of the bottom of this Object or link - """ - aabb = self.get_AABB(link_name=link_name) - base_width = np.absolute(aabb[0][0] - aabb[1][0]) - base_length = np.absolute(aabb[0][1] - aabb[1][1]) - return Pose([aabb[0][0] + base_width / 2, aabb[0][1] + base_length / 2, aabb[0][2]], - self.get_pose().orientation_as_list()) - - def get_joint_limits(self, joint: str) -> Tuple[float, float]: - """ - Returns the lower and upper limit of a joint, if the lower limit is higher - than the upper they are swapped to ensure the lower limit is always the smaller one. - - :param joint: The name of the joint for which the limits should be found. - :return: The lower and upper limit of the joint. - """ - if joint not in self.joints.keys(): - raise KeyError(f"The given Joint: {joint} is not part of this object") - lower, upper = p.getJointInfo(self.id, self.joints[joint], self.world.client_id)[8: 10] - if lower > upper: - lower, upper = upper, lower - return lower, upper - - def get_joint_axis(self, joint_name: str) -> Tuple[float]: - """ - Returns the axis along which a joint is moving. The given joint_name has to be part of this object. - - :param joint_name: Name of the joint for which the axis should be returned. - :return: The axis a vector of xyz - """ - return p.getJointInfo(self.id, self.joints[joint_name], self.world.client_id)[13] - - def get_joint_type(self, joint_name: str) -> JointType: - """ - Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - - :param joint_name: Joint name for which the type should be returned - :return: The type of the joint - """ - joint_type = p.getJointInfo(self.id, self.joints[joint_name], self.world.client_id)[2] - return JointType(joint_type) - - def find_joint_above(self, link_name: str, joint_type: JointType) -> str: - """ - Traverses the chain from 'link_name' to the URDF origin and returns the first joint that is of type 'joint_type'. - - :param link_name: Link name above which the joint should be found - :param joint_type: Joint type that should be searched for - :return: Name of the first joint which has the given type - """ - chain = self.urdf_object.get_chain(self.urdf_object.get_root(), link_name) - reversed_chain = reversed(chain) - container_joint = None - for element in reversed_chain: - if element in self.joints and self.get_joint_type(element) == joint_type: - container_joint = element - break - if not container_joint: - rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") - return container_joint - - 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. - - :return: A dictionary with all joints positions'. - """ - return self._current_joints_positions - - def get_link_tf_frame(self, link_name: str) -> str: - """ - Returns the name of the tf frame for the given link name. This method does not check if the given name is - actually a link of this object. - - :param link_name: Name of a link for which the tf frame should be returned - :return: A TF frame name for a specific link - """ - return self.tf_frame + "/" + link_name - - def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]: - """ - Extracts the geometry information for each collision of each link and links them to the respective link. - - :return: A dictionary with link name as key and geometry information as value - """ - link_to_geometry = {} - for link in self.links.keys(): - link_obj = self.urdf_object.link_map[link] - if not link_obj.collision: - link_to_geometry[link] = None - else: - link_to_geometry[link] = link_obj.collision.geometry - return link_to_geometry - - def _update_link_poses(self) -> None: - """ - Updates the cached poses and transforms for each link of this Object - """ - for link_name in self.links.keys(): - if link_name == self.urdf_object.get_root(): - self._current_link_poses[link_name] = self._current_pose - self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame) - else: - self._current_link_poses[link_name] = Pose(*p.getLinkState(self.id, self.links[link_name], - physicsClientId=self.world.client_id)[4:6]) - self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( - self.get_link_tf_frame(link_name)) - - def _init_current_positions_of_joints(self) -> None: - """ - Initialize the cached joint position for each joint. - """ - for joint_name in self.joints.keys(): - self._current_joints_positions[joint_name] = \ - p.getJointState(self.id, self.joints[joint_name], physicsClientId=self.world.client_id)[0] - - -def filter_contact_points(contact_points, exclude_ids) -> List: - """ - Returns a list of contact points where Objects that are in the 'exclude_ids' list are removed. - - :param contact_points: A list of contact points - :param exclude_ids: A list of unique ids of Objects that should be removed from the list - :return: A list containing 'contact_points' without Objects that are in 'exclude_ids' - """ - return list(filter(lambda cp: cp[2] not in exclude_ids, contact_points)) - - -def get_path_from_data_dir(file_name: str, data_directory: str) -> str: - """ - Returns the full path for a given file name in the given directory. If there is no file with the given filename - this method returns None. - - :param file_name: The filename of the searched file. - :param data_directory: The directory in which to search for the file. - :return: The full path in the filesystem or None if there is no file with the filename in the directory - """ - dir = pathlib.Path(data_directory) - for file in os.listdir(data_directory): - if file == file_name: - return data_directory + f"/{file_name}" - - -def _get_robot_name_from_urdf(urdf_string: str) -> str: - """ - Extracts the robot name from the 'robot_name' tag of a URDF. - - :param urdf_string: The URDF as string. - :return: The name of the robot described by the URDF. - """ - res = re.findall(r"robot\ *name\ *=\ *\"\ *[a-zA-Z_0-9]*\ *\"", urdf_string) - if len(res) == 1: - begin = res[0].find("\"") - end = res[0][begin + 1:].find("\"") - robot = res[0][begin + 1:begin + 1 + end].lower() - return robot - - -def _load_object(name: str, - path: str, - position: List[float], - orientation: List[float], - world: BulletWorld, - color: List[float], - ignoreCachedFiles: bool) -> Tuple[int, str]: - """ - Loads an object to the given BulletWorld with the given position and orientation. The color will only be - used when an .obj or .stl file is given. - If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created - and this URDf file will be loaded instead. - When spawning a URDf file a new file will be created in the cache directory, if there exists none. - This new file will have resolved mesh file paths, meaning there will be no references - to ROS packges instead there will be absolute file paths. - - :param name: The name of the object which should be spawned - :param path: The path to the source file or the name on the ROS parameter server - :param position: The position in which the object should be spawned - :param orientation: The orientation in which the object should be spawned - :param world: The BulletWorld to which the Object should be spawned - :param color: The color of the object, only used when .obj or .stl file is given - :param ignoreCachedFiles: Whether to ignore files in the cache directory. - :return: The unique id of the object and the path to the file used for spawning - """ - pa = pathlib.Path(path) - extension = pa.suffix - world, world_id = _world_and_id(world) - if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): - for dir in world.data_directory: - path = get_path_from_data_dir(path, dir) - if path: break - - if not path: - raise FileNotFoundError( - f"File {pa.name} could not be found in the resource directory {world.data_directory}") - # rospack = rospkg.RosPack() - # cach_dir = rospack.get_path('pycram') + '/resources/cached/' - cach_dir = world.data_directory[0] + '/cached/' - if not pathlib.Path(cach_dir).exists(): - os.mkdir(cach_dir) - - # if file is not yet cached corrcet the urdf and save if in the cache directory - if not _is_cached(path, name, cach_dir) or ignoreCachedFiles: - if extension == ".obj" or extension == ".stl": - path = _generate_urdf_file(name, path, color, cach_dir) - elif extension == ".urdf": - with open(path, mode="r") as f: - urdf_string = fix_missing_inertial(f.read()) - urdf_string = remove_error_tags(urdf_string) - urdf_string = fix_link_attributes(urdf_string) - try: - urdf_string = _correct_urdf_string(urdf_string) - except rospkg.ResourceNotFound as e: - rospy.logerr(f"Could not find resource package linked in this URDF") - raise e - path = cach_dir + pa.name - with open(path, mode="w") as f: - f.write(urdf_string) - else: # Using the urdf from the parameter server - urdf_string = rospy.get_param(path) - path = cach_dir + name + ".urdf" - with open(path, mode="w") as f: - f.write(_correct_urdf_string(urdf_string)) - # save correct path in case the file is already in the cache directory - elif extension == ".obj" or extension == ".stl": - path = cach_dir + pa.stem + ".urdf" - elif extension == ".urdf": - path = cach_dir + pa.name - else: - path = cach_dir + name + ".urdf" - - try: - obj = p.loadURDF(path, basePosition=position, baseOrientation=orientation, physicsClientId=world_id) - return obj, path - except p.error as e: - logging.error( - "The File could not be loaded. Plese note that the path has to be either a URDF, stl or obj file or the name of an URDF string on the parameter server.") - os.remove(path) - raise (e) - # print(f"{bcolors.BOLD}{bcolors.WARNING}The path has to be either a path to a URDf file, stl file, obj file or the name of a URDF on the parameter server.{bcolors.ENDC}") - - -def _is_cached(path: str, name: str, cach_dir: str) -> bool: - """ - Checks if the file in the given path is already cached or if - there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from - the parameter server is used. - - :param path: The path given by the user to the source file. - :param name: The name for this object. - :param cach_dir: The absolute path the cach directory in the pycram package. - :return: True if there already exists a chached file, False in any other case. - """ - file_name = pathlib.Path(path).name - p = pathlib.Path(cach_dir + file_name) - if p.exists(): - return True - # Returns filename without the filetype, e.g. returns "test" for "test.txt" - file_stem = pathlib.Path(path).stem - p = pathlib.Path(cach_dir + file_stem + ".urdf") - if p.exists(): - return True - return False - - -def _correct_urdf_string(urdf_string: str) -> str: - """ - Changes paths for files in the URDF from ROS paths to paths in the file system. Since PyBullet can't deal with ROS - package paths. - - :param urdf_name: The name of the URDf on the parameter server - :return: The URDF string with paths in the filesystem instead of ROS packages - """ - r = rospkg.RosPack() - new_urdf_string = "" - for line in urdf_string.split('\n'): - if "package://" in line: - s = line.split('//') - s1 = s[1].split('/') - path = r.get_path(s1[0]) - line = line.replace("package://" + s1[0], path) - new_urdf_string += line + '\n' - - return fix_missing_inertial(new_urdf_string) - - -def fix_missing_inertial(urdf_string: str) -> str: - """ - Insert inertial tags for every URDF link that has no inertia. - This is used to prevent PyBullet from dumping warnings in the terminal - - :param urdf_string: The URDF description as string - :returns: The new, corrected URDF description as string. - """ - - inertia_tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.Element("inertial")) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("mass", {"value": "0.1"})) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("inertia", {"ixx": "0.01", - "ixy": "0", - "ixz": "0", - "iyy": "0.01", - "iyz": "0", - "izz": "0.01"})) - - # create tree from string - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) - - for link_element in tree.iter("link"): - inertial = [*link_element.iter("inertial")] - if len(inertial) == 0: - link_element.append(inertia_tree.getroot()) - - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') - - -def remove_error_tags(urdf_string: str) -> str: - """ - Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the - URDF_parser - - :param urdf_string: String of the URDF from which the tags should be removed - :return: The URDF string with the tags removed - """ - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) - removing_tags = ["gazebo", "transmission"] - for tag_name in removing_tags: - all_tags = tree.findall(tag_name) - for tag in all_tags: - tree.getroot().remove(tag) - - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') - - -def fix_link_attributes(urdf_string: str) -> str: - """ - Removes the attribute 'type' from links since this is not parsable by the URDF parser. - - :param urdf_string: The string of the URDF from which the attributes should be removed - :return: The URDF string with the attributes removed - """ - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) - - for link in tree.iter("link"): - if "type" in link.attrib.keys(): - del link.attrib["type"] - - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') - - -def _generate_urdf_file(name: str, path: str, color: List[float], cach_dir: str) -> str: - """ - Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given color will be - used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with the name - as filename. - - :param name: The name of the object - :param path: The path to the .obj or .stl file - :param color: The color which should be used for the material tag - :param cach_dir The absolute file path to the cach directory in the pycram package - :return: The absolute path of the created file - """ - urdf_template = ' \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - ' - urdf_template = fix_missing_inertial(urdf_template) - rgb = " ".join(list(map(str, color))) - pathlib_obj = pathlib.Path(path) - path = str(pathlib_obj.resolve()) - content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) - with open(cach_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: - file.write(content) - return cach_dir + pathlib_obj.stem + ".urdf" - - -def _world_and_id(world: BulletWorld) -> Tuple[BulletWorld, int]: - """ - Selects the world to be used. If the given world is None the 'current_bullet_world' is used. - - :param world: The world which should be used or None if 'current_bullet_world' should be used - :return: The BulletWorld object and the id of this BulletWorld - """ - world = world if world is not None else BulletWorld.current_bullet_world - id = world.client_id if world is not None else BulletWorld.current_bullet_world.client_id - return world, id diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 67d4c6fc9..586758694 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -42,7 +42,7 @@ def __init__(self, resolution: float, self.height: int = height self.width: int = width local_transformer = LocalTransformer() - self.origin: Pose = local_transformer.transform_pose(origin, 'map') + self.origin: Pose = local_transformer.transform_pose_to_target_frame(origin, 'map') self.map: np.ndarray = map self.vis_ids: List[int] = [] diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 38ce79ab7..e902df6e4 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -690,7 +690,7 @@ def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: :return: The adjusted grasp pose """ lt = LocalTransformer() - pose_in_object = lt.transform_to_object_frame(pose, self.bullet_world_object) + pose_in_object = lt.transform_pose_to_object_base_frame(pose, self.bullet_world_object) special_knowledge = [] # Initialize as an empty list if self.type in SPECIAL_KNOWLEDGE: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 354aed83e..5f1a8c5cc 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -310,11 +310,11 @@ def perform(self) -> None: # oTm = Object Pose in Frame map oTm = object.get_pose() # Transform the object pose to the object frame, basically the origin of the object frame - mTo = object.local_transformer.transform_to_object_frame(oTm, object) + mTo = object.local_transformer.transform_pose_to_object_base_frame(oTm, object) # Adjust the pose according to the special knowledge of the object designator adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) # Transform the adjusted pose to the map frame - adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") + adjusted_oTm = object.local_transformer.transform_pose_to_target_frame(adjusted_pose, "map") # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper ori = multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y, adjusted_oTm.orientation.z, adjusted_oTm.orientation.w], @@ -330,19 +330,19 @@ def perform(self) -> None: # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" gripper_frame = robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm)) # First rotate the gripper, so the further calculations makes sense - tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + tmp_for_rotate_pose = object.local_transformer.transform_pose_to_target_frame(adjusted_oTm, gripper_frame) tmp_for_rotate_pose.pose.position.x = 0 tmp_for_rotate_pose.pose.position.y = 0 tmp_for_rotate_pose.pose.position.z = -0.1 - gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") + gripper_rotate_pose = object.local_transformer.transform_pose_to_target_frame(tmp_for_rotate_pose, "map") #Perform Gripper Rotate # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() - oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + oTg = object.local_transformer.transform_pose_to_target_frame(adjusted_oTm, gripper_frame) oTg.pose.position.x -= 0.1 # in x since this is how the gripper is oriented - prepose = object.local_transformer.transform_pose(oTg, "map") + prepose = object.local_transformer.transform_pose_to_target_frame(oTg, "map") # Perform the motion with the prepose and open gripper BulletWorld.current_bullet_world.add_vis_axis(prepose) @@ -434,8 +434,8 @@ def perform(self) -> None: local_tf = LocalTransformer() # Transformations such that the target position is the position of the object and not the tcp - tcp_to_object = local_tf.transform_pose(object_pose, - BulletWorld.robot.get_link_tf_frame( + tcp_to_object = local_tf.transform_pose_to_target_frame(object_pose, + BulletWorld.robot.get_link_tf_frame( robot_description.get_tool_frame(self.arm))) target_diff = self.target_location.to_transform("target").inverse_times( tcp_to_object.to_transform("object")).to_pose() @@ -884,8 +884,8 @@ def perform(self) -> Any: lt = LocalTransformer() gripper_name = robot_description.get_tool_frame(self.arm) - object_pose_in_gripper = lt.transform_pose(object_pose, - BulletWorld.robot.get_link_tf_frame(gripper_name)) + object_pose_in_gripper = lt.transform_pose_to_target_frame(object_pose, + BulletWorld.robot.get_link_tf_frame(gripper_name)) pre_grasp = object_pose_in_gripper.copy() pre_grasp.pose.position.x -= 0.1 diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index a42acd13a..8a5dc60bb 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -45,7 +45,7 @@ def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_ob :return: A moveit_msgs/PositionIKRequest message containing all the information from the parameter """ local_transformer = LocalTransformer() - target_pose = local_transformer.transform_pose(target_pose, robot_object.get_link_tf_frame(root_link)) + target_pose = local_transformer.transform_pose_to_target_frame(target_pose, robot_object.get_link_tf_frame(root_link)) robot_state = RobotState() joint_state = JointState() @@ -124,7 +124,7 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str # Get link after last joint in chain end_effector = robot_description.get_child(joints[-1]) - target_torso = local_transformer.transform_pose(target_pose, robot.get_link_tf_frame(base_link)) + target_torso = local_transformer.transform_pose_to_target_frame(target_pose, robot.get_link_tf_frame(base_link)) # target_torso = _transform_to_torso(pose, shadow_robot) diff = calculate_wrist_tool_offset(end_effector, gripper, robot) diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index 3a41c2074..d092dbe2a 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -129,7 +129,7 @@ def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescripti source = query_result.res[0].poseSource[i] lt = LocalTransformer() - pose = lt.transform_pose(pose, "map") + pose = lt.transform_pose_to_target_frame(pose, "map") pose_candidates[source] = pose diff --git a/src/pycram/helper.py b/src/pycram/helper.py index aad79a9d1..719fd040c 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -73,7 +73,7 @@ def _transform_to_torso(pose_and_rotation: Tuple[List[float], List[float]], robo def calculate_wrist_tool_offset(wrist_frame: str, tool_frame: str, robot: BulletWorldObject) -> Transform: local_transformer = LocalTransformer() tool_pose = robot.get_link_pose(tool_frame) - wrist_to_tool = local_transformer.transform_pose(tool_pose, robot.get_link_tf_frame(wrist_frame)) + wrist_to_tool = local_transformer.transform_pose_to_target_frame(tool_pose, robot.get_link_tf_frame(wrist_frame)) return wrist_to_tool.to_transform(robot.get_link_tf_frame(tool_frame)) diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 9c0ede811..256d49b3a 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -68,7 +68,7 @@ def update_objects_for_current_world(self) -> None: for obj in list(self.world.current_bullet_world.objects): self.update_transforms_for_object(obj, curr_time) - def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: + def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union[Pose, None]: """ Transforms a given pose to the target frame. @@ -91,6 +91,28 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: return Pose(*copy_pose.to_list(), frame=new_pose.header.frame_id) + def transform_pose_to_object_base_frame(self, pose: Pose, world_object: 'world.Object') -> Union[Pose, None]: + """ + Transforms the given pose to the base frame of the given BulletWorld object. + + :param pose: Pose that should be transformed + :param world_object: World Object with frame to which the pose should be transformed + :return: The new pose transformed to the base coordinate frame of the object + """ + return self.transform_to_object_frame(pose, world_object.tf_frame) + + def transform_pose_to_object_link_frame(self, pose: Pose, world_object: 'world.Object', link_name: str) -> Union[Pose, None]: + """ + Transforms the given pose to the link frame of the given BulletWorld object. + + :param pose: Pose that should be transformed + :param world_object: World Object with frame to which the pose should be transformed + :param link_name: Name of the link of the object to which the pose should be transformed + :return: The new pose transformed to the link coordinate frame of the object + """ + target_frame = world_object.get_link_tf_frame(link_name) + return self.transform_to_object_frame(pose, target_frame) + def transform_to_object_frame(self, pose: Pose, world_object: 'world.Object', link_name: str = None) -> Union[Pose, None]: """ @@ -106,7 +128,7 @@ def transform_to_object_frame(self, pose: Pose, target_frame = world_object.get_link_tf_frame(link_name) else: target_frame = world_object.tf_frame - return self.transform_pose(pose, target_frame) + return self.transform_pose_to_target_frame(pose, target_frame) def tf_transform(self, source_frame: str, target_frame: str, time: Optional[rospy.rostime.Time] = None) -> Transform: @@ -156,5 +178,5 @@ def transformPose(self, target_frame, ps) -> Pose: :param ps: Pose that should be transformed :return: Input pose in the target_frame """ - return self.transform_pose(ps, target_frame) + return self.transform_pose_to_target_frame(ps, target_frame) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 07800af68..c446b1463 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -97,7 +97,7 @@ def _execute(self, desig: PlaceMotion.Motion): # Transformations such that the target position is the position of the object and not the tcp object_pose = object.get_pose() local_tf = LocalTransformer() - tcp_to_object = local_tf.transform_pose(object_pose, robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) + tcp_to_object = local_tf.transform_pose_to_target_frame(object_pose, robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, arm) @@ -115,8 +115,8 @@ def _execute(self, desig: LookingMotion.Motion): robot = BulletWorld.robot local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_pan_link")) - pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_tilt_link")) + pose_in_pan = local_transformer.transform_pose_to_target_frame(target, robot.get_link_tf_frame("head_pan_link")) + pose_in_tilt = local_transformer.transform_pose_to_target_frame(target, robot.get_link_tf_frame("head_tilt_link")) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 @@ -295,8 +295,8 @@ def _execute(self, desig: LookingMotion.Motion): robot = BulletWorld.robot local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_pan_link")) - pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_tilt_link")) + pose_in_pan = local_transformer.transform_pose_to_target_frame(target, robot.get_link_tf_frame("head_pan_link")) + pose_in_tilt = local_transformer.transform_pose_to_target_frame(target, robot.get_link_tf_frame("head_tilt_link")) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 @@ -321,7 +321,7 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: obj_pose = query_result["ClusterPoseBBAnnotator"] lt = LocalTransformer() - obj_pose = lt.transform_pose(obj_pose, BulletWorld.robot.get_link_tf_frame("torso_lift_link")) + obj_pose = lt.transform_pose_to_target_frame(obj_pose, BulletWorld.robot.get_link_tf_frame("torso_lift_link")) obj_pose.orientation = [0, 0, 0, 1] obj_pose.position.x += 0.05 @@ -350,7 +350,7 @@ class Pr2MoveTCPReal(ProcessModule): def _execute(self, designator: MoveTCPMotion.Motion) -> Any: lt = LocalTransformer() - pose_in_map = lt.transform_pose(designator.target, "map") + pose_in_map = lt.transform_pose_to_target_frame(designator.target, "map") if designator.allow_gripper_collision: giskard.allow_gripper_collision(designator.arm) diff --git a/src/pycram/world.py b/src/pycram/world.py index 2e3c0f159..af9e34a3f 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -10,7 +10,7 @@ import xml.etree.ElementTree from queue import Queue import tf -from typing import List, Optional, Dict, Tuple, Callable +from typing import List, Optional, Dict, Tuple, Callable, Set from typing import Union import numpy as np @@ -32,24 +32,51 @@ from .pose import Pose, Transform from abc import ABC, abstractproperty, abstractmethod +from dataclasses import dataclass + + +@dataclass +class Constraint: + parent_obj_id: int + child_obj_id: int + parent_link_id: int + child_link_id: int + parent_to_child_transform: Transform + joint_type: int + joint_axis: List + + +@dataclass +class Attachment: + parent_obj: Object + child_obj: Object + parent_link: Optional[str] + loose: bool class World(ABC): """ The World Class represents the physics Simulation and belief state. """ - current_world: World = None """ Global reference to the currently used World, usually this is the graphical one. However, if you are inside a Use_shadow_world() environment the current_world points to the shadow world. In this way you can comfortably use the current_world, which should point towards the World used at the moment. """ - robot: Object = None + current_world: World = None + """ Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the URDF with the name of the URDF on the parameter server. """ + robot: Object = None + + """ + Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. + """ + data_directory: List[str] = {os.path.dirname(__file__) + "/../../resources"} + def __init__(self, mode: str, is_prospection_world: bool): """ Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the @@ -61,22 +88,40 @@ def __init__(self, mode: str, is_prospection_world: bool): """ self.objects: List[Object] = [] self.client_id: int = -1 - self.detachment_event: Event = Event() - self.attachment_event: Event = Event() - self.manipulation_event: Event = Event() self.mode: str = mode - if World.current_world is None: - World.current_world = self + self._initialize_events() + World.current_world = self if World.current_world is None else World.current_world self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} - self.data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] - self.prospection_world: World = World("DIRECT", True) if not is_prospection_world else None - self.world_sync: WorldSync = WorldSync(self, self.prospection_world) if not is_prospection_world else None - self.is_prospection_world: bool = is_prospection_world self.local_transformer = LocalTransformer() - if not is_prospection_world: - self.world_sync.start() - self.local_transformer.world = self - self.local_transformer.prospection_world = self.prospection_world + + self.is_prospection_world: bool = is_prospection_world + + if is_prospection_world: # then no need to add another prospection world + self.prospection_world = None + self.world_sync = None + else: # a normal world should have a synced prospection world + self._init_and_sync_prospection_world() + self._update_local_transformer_worlds() + + def _init_and_sync_prospection_world(self): + self._init_prospection_world() + self._sync_prospection_world() + + def _update_local_transformer_worlds(self): + self.local_transformer.world = self + self.local_transformer.prospection_world = self.prospection_world + + def _init_prospection_world(self): + self.prospection_world: World = World("DIRECT", True) + + def _sync_prospection_world(self): + self.world_sync: WorldSync = WorldSync(self, self.prospection_world) + self.world_sync.start() + + def _initialize_events(self): + self.detachment_event: Event = Event() + self.attachment_event: Event = Event() + self.manipulation_event: Event = Event() def get_objects_by_name(self, name: str) -> List[Object]: """ @@ -114,22 +159,33 @@ def remove_object(self, obj_id: int) -> None: """ pass - def add_constraint(self, - parent_obj_id: int, - child_obj_id: int, - parent_link_id: int, - joint_type: int, - parent_to_child_transform: Transform, - joint_axis: List, - child_link_id: int) -> int: + def add_constraint(self, constraint: Constraint) -> int: """ Add a constraint between two objects so that attachment they become attached """ - cid = p.createConstraint(parent_obj_id, parent_link_id, child_obj_id, child_link_id, joint_type, - joint_axis, parent_to_child_transform.translation_as_list(), [0, 0, 0], - parent_to_child_transform.rotation_as_list(), + cid = p.createConstraint(constraint.parent_obj_id, + constraint.parent_link_id, + constraint.child_obj_id, + constraint.child_link_id, + constraint.joint_type, + constraint.joint_axis, + constraint.parent_to_child_transform.translation_as_list(), + [0, 0, 0], + constraint.parent_to_child_transform.rotation_as_list(), physicsClientId=self.client_id) return cid + + def attach_object_base_to_parent_object_base(self, parent_obj: Object, + child_obj: Object, + loose: Optional[bool] = False) -> None: + pass # TODO: implement this function + + def attach_object_base_to_parent_object_link(self, parent_obj: Object, + child_obj: Object, + parent_link: str, + loose: Optional[bool] = False) -> None: + pass # TODO: implement this function + def attach_objects(self, parent_obj: Object, child_obj: Object, @@ -151,7 +207,7 @@ def attach_objects(self, :param loose: If the attachment should be a loose attachment. """ link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 - link_to_object = parent_obj._calculate_transform(child_obj, parent_link) + link_to_object = parent_obj._calculate_transform_of_object_base_to_this_object_link(child_obj, parent_link) parent_obj.attachments[child_obj] = [link_to_object, parent_link, loose] child_obj.attachments[parent_obj] = [link_to_object.invert(), None, False] @@ -213,7 +269,7 @@ def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: else: link_to_object = obj.attachments[att_obj][0] - world_to_object = obj.local_transformer.transform_pose(link_to_object.to_pose(), "map") + world_to_object = obj.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") p.resetBasePositionAndOrientation(att_obj.id, world_to_object.position_as_list(), world_to_object.orientation_as_list(), physicsClientId=self.client_id) @@ -620,7 +676,7 @@ def add_additional_resource_path(self, path: str) -> None: :param path: A path in the filesystem in which to search for files. """ - self.data_directory.append(path) + World.data_directory.append(path) def get_shadow_object(self, object: Object) -> Object: """ @@ -707,18 +763,20 @@ def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): self._gui_thread: Gui = Gui(self, mode) self._gui_thread.start() + # This disables file caching from PyBullet, since this would also cache # files that can not be loaded p.setPhysicsEngineParameter(enableFileCaching=0) + # Needed to let the other thread start the simulation, before Objects are spawned. time.sleep(0.1) self.vis_axis: List[Object] = [] # Some default settings self.set_gravity([0, 0, -9.8]) + if not is_prospection_world: plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - # atexit.register(self.exit) def get_objects_by_name(self, name: str) -> List[Object]: """ @@ -839,7 +897,7 @@ def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: else: link_to_object = obj.attachments[att_obj][0] - world_to_object = obj.local_transformer.transform_pose(link_to_object.to_pose(), "map") + world_to_object = obj.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") p.resetBasePositionAndOrientation(att_obj.id, world_to_object.position_as_list(), world_to_object.orientation_as_list(), physicsClientId=self.client_id) @@ -1239,15 +1297,6 @@ def register_collision_callback(self, objectA: Object, objectB: Object, """ self.coll_callbacks[(objectA, objectB)] = (callback_collision, callback_no_collision) - def add_additional_resource_path(self, path: str) -> None: - """ - Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an - Object is spawned only with a filename. - - :param path: A path in the filesystem in which to search for files. - """ - self.data_directory.append(path) - def get_shadow_object(self, object: Object) -> Object: """ Returns the corresponding object from the shadow world for the given object. If the given Object is already in @@ -1665,7 +1714,7 @@ def __init__(self, name: str, type: Union[str, ObjectType], path: str, self.name: str = name self.type: str = type self.color: List[float] = color - pose_in_map = self.local_transformer.transform_pose(pose, "map") + pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") position, orientation = pose_in_map.to_list() self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignoreCachedFiles) self.joints: Dict[str, int] = self._joint_or_link_name_to_id("joint") @@ -1793,7 +1842,7 @@ def set_pose(self, pose: Pose, base: bool = False) -> None: :param pose: New Pose for the object :param base: If True places the object base instead of origin at the specified position and orientation """ - pose_in_map = self.local_transformer.transform_pose(pose, "map") + pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") position, orientation = pose_in_map.to_list() if base: position = np.array(position) + self.base_origin_shift @@ -1839,6 +1888,14 @@ def _set_attached_objects(self, prev_object: List[Object]) -> None: """ self.world._set_attached_objects(self, prev_object) + def _calculate_transform_of_object_base_to_this_object_base(self, obj: Object): + transform = self.local_transformer.transform_pose_to_object_base_frame(obj.pose, self) + return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) + + def _calculate_transform_of_object_base_to_this_object_link(self, obj: Object, link: str): + transform = self.local_transformer.transform_pose_to_object_link_frame(obj.pose, self, link) + return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) + def _calculate_transform(self, obj: Object, link: str = None) -> Transform: """ Calculates the transformation between another object and the given @@ -2267,7 +2324,6 @@ def get_path_from_data_dir(file_name: str, data_directory: str) -> str: :param data_directory: The directory in which to search for the file. :return: The full path in the filesystem or None if there is no file with the filename in the directory """ - dir = pathlib.Path(data_directory) for file in os.listdir(data_directory): if file == file_name: return data_directory + f"/{file_name}" diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 23e0c8924..d655b8f8c 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -311,7 +311,7 @@ def blocking(pose_or_object: Union[Object, Pose], joints = robot_description.chains[arm].joints local_transformer = LocalTransformer() - target_map = local_transformer.transform_pose(input_pose, "map") + target_map = local_transformer.transform_pose_to_target_frame(input_pose, "map") if grasp: grasp_orientation = robot_description.grasps.get_orientation_for_grasp(grasp) target_map.orientation.x = grasp_orientation[0] diff --git a/test/local_transformer_tests.py b/test/local_transformer_tests.py index fc8c48f3e..b3f3b8bd1 100644 --- a/test/local_transformer_tests.py +++ b/test/local_transformer_tests.py @@ -26,7 +26,7 @@ def test_transform_pose(self): l.setTransform(Transform([1, 1, 1], [0, 0, 0, 1], "map", "test_frame")) p = Pose() - transformed_pose = l.transform_pose(p, "test_frame") + transformed_pose = l.transform_pose_to_target_frame(p, "test_frame") self.assertTrue(transformed_pose.position_as_list() == [-1, -1, -1]) self.assertTrue(transformed_pose.orientation_as_list() == [0, 0, 0, 1]) @@ -37,7 +37,7 @@ def test_transform_pose_position(self): l.setTransform(Transform([1, 1, 1], [0, 0, 0, 1], "map", "test_frame")) p = Pose() - transformed_pose = l.transform_pose(p, "test_frame") + transformed_pose = l.transform_pose_to_target_frame(p, "test_frame") self.assertTrue(transformed_pose.position == transformed_pose.pose.position) From a5ae78bdb11113ddc2882ebf76a1ba4a9c5b8328 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 6 Dec 2023 19:08:31 +0100 Subject: [PATCH 06/69] [WorldAbstractClass] InProgress , currently in _set_attached_objects method, and defining the Attachment class and making the necessary changes for that, also add_constraint is still in progress. --- examples/local_transformer.ipynb | 2 +- src/pycram/local_transformer.py | 4 +- src/pycram/pose.py | 4 + src/pycram/world.py | 243 ++++++++++++++++++++----------- 4 files changed, 163 insertions(+), 90 deletions(-) diff --git a/examples/local_transformer.ipynb b/examples/local_transformer.ipynb index 2d4d41c03..31017bebb 100644 --- a/examples/local_transformer.ipynb +++ b/examples/local_transformer.ipynb @@ -233,7 +233,7 @@ "l = LocalTransformer()\n", "test_pose = Pose([1, 1, 1], [0, 0, 0, 1], \"map\")\n", "\n", - "transformed_pose = l.transform_to_object_frame(test_pose, milk)\n", + "transformed_pose = l.transform_pose_to_object_frame(test_pose, milk)\n", "print(transformed_pose)\n", "\n", "new_pose = l.transform_pose(transformed_pose, \"map\")\n", diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 256d49b3a..23bcb3cdc 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -99,7 +99,7 @@ def transform_pose_to_object_base_frame(self, pose: Pose, world_object: 'world.O :param world_object: World Object with frame to which the pose should be transformed :return: The new pose transformed to the base coordinate frame of the object """ - return self.transform_to_object_frame(pose, world_object.tf_frame) + return self.transform_pose_to_target_frame(pose, world_object.tf_frame) def transform_pose_to_object_link_frame(self, pose: Pose, world_object: 'world.Object', link_name: str) -> Union[Pose, None]: """ @@ -111,7 +111,7 @@ def transform_pose_to_object_link_frame(self, pose: Pose, world_object: 'world.O :return: The new pose transformed to the link coordinate frame of the object """ target_frame = world_object.get_link_tf_frame(link_name) - return self.transform_to_object_frame(pose, target_frame) + return self.transform_pose_to_target_frame(pose, target_frame) def transform_to_object_frame(self, pose: Pose, world_object: 'world.Object', link_name: str = None) -> Union[Pose, None]: diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 852f20967..0b9feeaca 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -301,6 +301,10 @@ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional self.frame = frame + @classmethod + def from_pose_and_child_frame(cls, pose: Pose, child_frame_name: str) -> Transform: + return cls(pose.position_as_list(), pose.orientation_as_list(), pose.frame, child_frame_name) + @staticmethod def from_transform_stamped(transform_stamped: TransformStamped) -> Transform: """ diff --git a/src/pycram/world.py b/src/pycram/world.py index af9e34a3f..9ff813e65 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -38,20 +38,22 @@ @dataclass class Constraint: parent_obj_id: int - child_obj_id: int parent_link_id: int + child_obj_id: int child_link_id: int - parent_to_child_transform: Transform - joint_type: int - joint_axis: List + joint_type: JointType + joint_axis_in_child_link_frame: List[int] + joint_frame_position_wrt_parent_origin: List[float] + joint_frame_position_wrt_child_origin: List[float] + joint_frame_orientation_wrt_parent_origin: Optional[List[float]] = None + joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None @dataclass class Attachment: - parent_obj: Object - child_obj: Object - parent_link: Optional[str] - loose: bool + transform_from_child_base_to_parent_link: Transform + parent_link: Optional[str] = None + loose: Optional[bool] = False class World(ABC): @@ -123,6 +125,10 @@ def _initialize_events(self): self.attachment_event: Event = Event() self.manipulation_event: Event = Event() + @abstractmethod + def load_urdf_with_pose_and_get_world_object_id(self, path: str, pose: Pose) -> int: + pass + def get_objects_by_name(self, name: str) -> List[Object]: """ Returns a list of all Objects in this World with the same name as the given one. @@ -159,33 +165,73 @@ def remove_object(self, obj_id: int) -> None: """ pass + @abstractmethod def add_constraint(self, constraint: Constraint) -> int: """ - Add a constraint between two objects so that attachment they become attached + Add a constraint between two objects so that they become attached """ - cid = p.createConstraint(constraint.parent_obj_id, - constraint.parent_link_id, - constraint.child_obj_id, - constraint.child_link_id, - constraint.joint_type, - constraint.joint_axis, - constraint.parent_to_child_transform.translation_as_list(), - [0, 0, 0], - constraint.parent_to_child_transform.rotation_as_list(), - physicsClientId=self.client_id) - return cid + pass - def attach_object_base_to_parent_object_base(self, parent_obj: Object, - child_obj: Object, - loose: Optional[bool] = False) -> None: - pass # TODO: implement this function + @abstractmethod + def remove_constraint(self, constraint_id): + pass - def attach_object_base_to_parent_object_link(self, parent_obj: Object, - child_obj: Object, - parent_link: str, - loose: Optional[bool] = False) -> None: + def create_fixed_constraint_to_parent_link_at_child_origin(self, + parent_object: Object, + child_object: Object, + parent_link: str) -> Constraint: + parent_link_id = parent_object.get_link_id(parent_link) + child_base_wrt_parent_link = \ + parent_object._calculate_transform_from_other_object_base_to_this_object_link(child_object, parent_link) + constraint = Constraint(parent_object.id, + parent_link_id, + child_object.id, + child_link_id=-1, # -1 means use the base link + joint_type=JointType.FIXED, + joint_axis_in_child_link_frame=[0, 1, 0], + joint_frame_position_wrt_parent_origin=child_base_wrt_parent_link.translation_as_list(), + joint_frame_position_wrt_child_origin=[0, 0, 0], + joint_frame_orientation_wrt_parent_origin=child_base_wrt_parent_link.rotation_as_list(), + ) + return constraint + + def add_attachment_to_parent_and_child_attachments(self, + parent_object: Object, + child_object: Object, + new_attachment: Attachment): + + parent_object.attachments[child_object] = [child_base_wrt_parent_link, parent_link, loose] + child_object.attachments[parent_object] = [child_base_wrt_parent_link.invert(), None, False] + + def attach_child_object_base_to_parent_object_base(self, parent_obj: Object, + child_obj: Object, + loose: Optional[bool] = False) -> None: pass # TODO: implement this function + def attach_child_object_base_to_parent_object_link(self, parent_obj: Object, + child_obj: Object, + parent_link: str, + loose: Optional[bool] = False) -> None: + parent_link_id = parent_obj.get_link_id(parent_link) + child_base_wrt_parent_link = \ + parent_obj._calculate_transform_from_other_object_base_to_this_object_link(child_obj, parent_link) + parent_obj.attachments[child_obj] = [child_base_wrt_parent_link, parent_link, loose] + child_obj.attachments[parent_obj] = [child_base_wrt_parent_link.invert(), None, False] + constraint = Constraint(parent_obj.id, + parent_link_id, + child_obj.id, + child_link_id=-1, # -1 means use the base link + joint_type=JointType.FIXED, + joint_axis_in_child_link_frame=[0, 1, 0], + joint_frame_position_wrt_parent_origin=child_base_wrt_parent_link.translation_as_list(), + joint_frame_position_wrt_child_origin=[0, 0, 0], + joint_frame_orientation_wrt_parent_origin=child_base_wrt_parent_link.rotation_as_list(), + ) + constraint_id = self.add_constraint(constraint) + parent_obj.cids[child_obj] = constraint_id + child_obj.cids[parent_obj] = constraint_id + self.attachment_event(parent_obj, [parent_obj, child_obj]) + def attach_objects(self, parent_obj: Object, child_obj: Object, @@ -206,17 +252,24 @@ def attach_objects(self, :param parent_link: The link of the parent object to which the child object should be attached :param loose: If the attachment should be a loose attachment. """ - link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 - link_to_object = parent_obj._calculate_transform_of_object_base_to_this_object_link(child_obj, parent_link) - parent_obj.attachments[child_obj] = [link_to_object, parent_link, loose] - child_obj.attachments[parent_obj] = [link_to_object.invert(), None, False] - - cid = p.createConstraint(parent_obj.id, link_id, child_obj.id, -1, p.JOINT_FIXED, - [0, 1, 0], link_to_object.translation_as_list(), [0, 0, 0], - link_to_object.rotation_as_list(), - physicsClientId=self.client_id) - parent_obj.cids[child_obj] = cid - child_obj.cids[parent_obj] = cid + parent_link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 + child_base_wrt_parent_link =\ + parent_obj._calculate_transform_from_other_object_base_to_this_object_link(child_obj, parent_link) + parent_obj.attachments[child_obj] = [child_base_wrt_parent_link, parent_link, loose] + child_obj.attachments[parent_obj] = [child_base_wrt_parent_link.invert(), None, False] + constraint = Constraint(parent_obj.id, + parent_link_id, + child_obj.id, + child_link_id=-1, + joint_type=JointType.FIXED, + joint_axis_in_child_link_frame=[0, 1, 0], + joint_frame_position_wrt_parent_origin=child_base_wrt_parent_link.translation_as_list(), + joint_frame_position_wrt_child_origin=[0, 0, 0], + joint_frame_orientation_wrt_parent_origin=child_base_wrt_parent_link.rotation_as_list(), + ) + constraint_id = self.add_constraint(constraint) + parent_obj.cids[child_obj] = constraint_id + child_obj.cids[parent_obj] = constraint_id self.attachment_event(parent_obj, [parent_obj, child_obj]) def detach_objects(self, obj1: Object, obj2: Object) -> None: @@ -252,14 +305,17 @@ def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: for att_obj in obj.attachments: if att_obj in prev_object: continue - if obj.attachments[att_obj][2]: + if obj.attachments[att_obj].loose: # Updates the attachment transformation and constraint if the # attachment is loose, instead of updating the position of all attached objects - link_to_object = obj._calculate_transform(att_obj, obj.attachments[att_obj][1]) - link_id = obj.get_link_id(obj.attachments[att_obj][1]) if obj.attachments[att_obj][1] else -1 - obj.attachments[att_obj][0] = link_to_object - att_obj.attachments[obj][0] = link_to_object.invert() - p.removeConstraint(obj.cids[att_obj], physicsClientId=self.client_id) + link_to_object = ( + obj._calculate_transform_from_other_object_base_to_this_object_link(att_obj, + obj.attachments[att_obj].parent_link)) + link_id = obj.get_link_id(obj.attachments[att_obj].parent_link) if obj.attachments[att_obj].parent_link else -1 + obj.attachments[att_obj].transform_from_child_base_to_parent_link = link_to_object + att_obj.attachments[obj].transform_from_child_base_to_parent_link = link_to_object.invert() + self.remove_constraint(obj.cids[att_obj]) + # TODO: Ask Jonas what is the benefit of joint axis in a fixed joint. cid = p.createConstraint(obj.id, link_id, att_obj.id, -1, p.JOINT_FIXED, [0, 0, 0], link_to_object.translation_as_list(), [0, 0, 0], link_to_object.rotation_as_list(), @@ -729,22 +785,6 @@ def reset_bullet_world(self) -> None: obj.set_pose(obj.original_pose) class BulletWorld(World): - """ - The BulletWorld Class represents the physics Simulation and belief state. - """ - - current_bullet_world: BulletWorld = None - """ - Global reference to the currently used BulletWorld, usually this is the - graphical one. However, if you are inside a Use_shadow_world() environment the current_bullet_world points to the - shadow world. In this way you can comfortably use the current_bullet_world, which should point towards the BulletWorld - used at the moment. - """ - robot: Object = None - """ - Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the - URDF with the name of the URDF on the parameter server. - """ # Check is for sphinx autoAPI to be able to work in a CI workflow if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): @@ -778,6 +818,11 @@ def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): if not is_prospection_world: plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) + def load_urdf_with_pose_and_get_world_object_id(self, path: str, pose: Pose) -> int: + return p.loadURDF(path, + basePosition=pose.position_as_list(), + baseOrientation=pose.orientation_as_list(), physicsClientId=self.client_id) + def get_objects_by_name(self, name: str) -> List[Object]: """ Returns a list of all Objects in this BulletWorld with the same name as the given one. @@ -814,6 +859,26 @@ def remove_object(self, obj_id: int) -> None: p.removeBody(obj_id, self.client_id) + def add_constraint(self, constraint: Constraint) -> int: + """ + Add a constraint between two objects so that attachment they become attached + """ + constraint_id = p.createConstraint(constraint.parent_obj_id, + constraint.parent_link_id, + constraint.child_obj_id, + constraint.child_link_id, + constraint.joint_type, + constraint.joint_axis, + constraint.joint_frame_position_wrt_parent_origin, + constraint.joint_frame_position_wrt_child_origin, + constraint.joint_frame_orientation_wrt_parent_origin, + constraint.joint_frame_orientation_wrt_child_origin, + physicsClientId=self.client_id) + return constraint_id + + def remove_constraint(self, constraint_id): + p.removeConstraint(constraint_id, physicsClientId=self.client_id) + def attach_objects(self, parent_obj: Object, child_obj: Object, @@ -1686,30 +1751,30 @@ def run(self): class Object: """ - Represents a spawned Object in the BulletWorld. + Represents a spawned Object in the World. """ def __init__(self, name: str, type: Union[str, ObjectType], path: str, pose: Pose = None, - world: BulletWorld = None, + world: World = None, color: Optional[List[float]] = [1, 1, 1, 1], ignoreCachedFiles: Optional[bool] = False): """ - The constructor loads the urdf file into the given BulletWorld, if no BulletWorld is specified the - :py:attr:`~BulletWorld.current_bullet_world` will be used. It is also possible to load .obj and .stl file into the BulletWorld. + The constructor loads the urdf file into the given World, if no World is specified the + :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. The color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. :param name: The name of the object :param type: The type of the object :param path: The path to the source file, if only a filename is provided then the resourcer directories will be searched :param pose: The pose at which the Object should be spawned - :param world: The BulletWorld in which the object should be spawned, if no world is specified the :py:attr:`~BulletWorld.current_bullet_world` will be used + :param world: The World in which the object should be spawned, if no world is specified the :py:attr:`~World.current_world` will be used :param color: The color with which the object should be spawned. :param ignoreCachedFiles: If true the file will be spawned while ignoring cached files. """ if not pose: pose = Pose() - self.world: BulletWorld = world if world is not None else BulletWorld.current_bullet_world + self.world: World = world if world is not None else World.current_world self.local_transformer = LocalTransformer() self.name: str = name self.type: str = type @@ -1719,21 +1784,21 @@ def __init__(self, name: str, type: Union[str, ObjectType], path: str, self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignoreCachedFiles) self.joints: Dict[str, int] = self._joint_or_link_name_to_id("joint") self.links: Dict[str, int] = self._joint_or_link_name_to_id("link") - self.attachments: Dict[Object, List] = {} + self.attachments: Dict[Object, Attachment] = {} self.cids: Dict[Object, int] = {} self.original_pose = pose_in_map self.tf_frame = ("shadow/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) # This means "world" is not the shadow world since it has a reference to a shadow world - if self.world.prospection_world != None: + if self.world.prospection_world is not None: self.world.world_sync.add_obj_queue.put( [name, type, path, position, orientation, self.world.prospection_world, color, self]) with open(self.path) as f: self.urdf_object = URDF.from_xml_string(f.read()) - if self.urdf_object.name == robot_description.name and not BulletWorld.robot: - BulletWorld.robot = self + if self.urdf_object.name == robot_description.name and not World.robot: + World.robot = self self.links[self.urdf_object.get_root()] = -1 @@ -1758,13 +1823,13 @@ def __repr__(self): def remove(self) -> None: """ - Removes this object from the BulletWorld it currently resides in. + Removes this object from the World it currently resides in. For the object to be removed it has to be detached from all objects it is currently attached to. After this is done a call to PyBullet is done to remove this Object from the simulation. """ for obj in self.attachments.keys(): - self.world.detach(self, obj) + self.world.detach_objects(self, obj) self.world.objects.remove(self) # This means the current world of the object is not the shadow world, since it # has a reference to the shadow world @@ -1772,8 +1837,8 @@ def remove(self) -> None: self.world.world_sync.remove_obj_queue.put(self) self.world.world_sync.remove_obj_queue.join() self.world.remove_object(self.id) - if BulletWorld.robot == self: - BulletWorld.robot = None + if World.robot == self: + World.robot = None def attach(self, obj: Object, link: Optional[str] = None, loose: Optional[bool] = False) -> None: """ @@ -1797,7 +1862,7 @@ def detach(self, obj: Object) -> None: Detaches another object from this object. This is done by deleting the attachment from the attachments dictionary of both objects and deleting the constraint of pybullet. - Afterward the detachment event of the corresponding BulletWorld will be fired. + Afterward the detachment event of the corresponding World will be fired. :param object: The object which should be detached """ @@ -1809,7 +1874,7 @@ def detach_all(self) -> None: """ attachments = self.attachments.copy() for att in attachments.keys(): - self.world.detach(self, att) + self.world.detach_objects(self, att) def get_position(self) -> Pose: """ @@ -1888,13 +1953,17 @@ def _set_attached_objects(self, prev_object: List[Object]) -> None: """ self.world._set_attached_objects(self, prev_object) - def _calculate_transform_of_object_base_to_this_object_base(self, obj: Object): - transform = self.local_transformer.transform_pose_to_object_base_frame(obj.pose, self) - return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) + def _calculate_transform_from_other_object_base_to_this_object_base(self, other_object: Object): + pose_wrt_this_object_base = self.local_transformer.transform_pose_to_object_base_frame(other_object.pose, self) + return Transform.from_pose_and_child_frame(pose_wrt_this_object_base, other_object.tf_frame) - def _calculate_transform_of_object_base_to_this_object_link(self, obj: Object, link: str): - transform = self.local_transformer.transform_pose_to_object_link_frame(obj.pose, self, link) - return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) + def _calculate_transform_from_other_object_base_to_this_object_link(self, + other_object: Object, + this_object_link_name: str): + pose_wrt_this_object_link = self.local_transformer.transform_pose_to_object_link_frame(other_object.pose, + self, + this_object_link_name) + return Transform.from_pose_and_child_frame(pose_wrt_this_object_link, other_object.tf_frame) def _calculate_transform(self, obj: Object, link: str = None) -> Transform: """ @@ -2348,11 +2417,11 @@ def _load_object(name: str, path: str, position: List[float], orientation: List[float], - world: BulletWorld, + world: World, color: List[float], ignoreCachedFiles: bool) -> Tuple[int, str]: """ - Loads an object to the given BulletWorld with the given position and orientation. The color will only be + Loads an object to the given World with the given position and orientation. The color will only be used when an .obj or .stl file is given. If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created and this URDf file will be loaded instead. @@ -2364,7 +2433,7 @@ def _load_object(name: str, :param path: The path to the source file or the name on the ROS parameter server :param position: The position in which the object should be spawned :param orientation: The orientation in which the object should be spawned - :param world: The BulletWorld to which the Object should be spawned + :param world: The World to which the Object should be spawned :param color: The color of the object, only used when .obj or .stl file is given :param ignoreCachedFiles: Whether to ignore files in the cache directory. :return: The unique id of the object and the path to the file used for spawning @@ -2417,7 +2486,7 @@ def _load_object(name: str, path = cach_dir + name + ".urdf" try: - obj = p.loadURDF(path, basePosition=position, baseOrientation=orientation, physicsClientId=world_id) + obj = world.load_urdf_with_pose_and_get_world_object_id(path, Pose(position, orientation)) return obj, path except p.error as e: logging.error( From 5188eed31aaa681f02bf146e4dce7dd85a01fb71 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 7 Dec 2023 19:12:07 +0100 Subject: [PATCH 07/69] [WorldAbstractClass] InProgress , currently in refactoring attachments. --- src/pycram/world.py | 231 +++++++++++++++++++++++--------------------- 1 file changed, 122 insertions(+), 109 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 9ff813e65..9dfffabc5 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -49,35 +49,66 @@ class Constraint: joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None -@dataclass class Attachment: - transform_from_child_base_to_parent_link: Transform - parent_link: Optional[str] = None - loose: Optional[bool] = False + def __init__(self, + parent_object: Object, + child_object: Object, + parent_link: Optional[str] = None, + bidirectional: Optional[bool] = False): + """ + Creates an attachment between the parent object and the child object. + """ + self.parent_object = parent_object + self.child_object = child_object + self.parent_link = parent_link + self.child_link = None + self.bidirectional = bidirectional + self.child_base_to_parent_link_transform = self._calculate_transforms() + + def _calculate_transforms(self): + # TODO: need to handle inverse case where there is a child link but no parent link + if self.parent_link is None: + return self.parent_object._calculate_transform_from_other_object_base_to_this_object_base(self.child_object) + else: + return self.parent_object._calculate_transform_from_other_object_base_to_this_object_link(self.child_object, + self.parent_link) + + def update_transforms(self): + self.child_base_to_parent_link_transform = self._calculate_transforms() + + def get_inverse(self): + attachment = Attachment(self.child_object, self.parent_object, None, self.bidirectional) + attachment.child_link = self.parent_link + return attachment + + def add_to_objects_attachments_collection(self): + self.parent_object.attachments[self.child_object] = self + self.child_object.attachments[self.parent_object] = self.get_inverse() class World(ABC): """ The World Class represents the physics Simulation and belief state. """ + + current_world: World = None """ - Global reference to the currently used World, usually this is the - graphical one. However, if you are inside a Use_shadow_world() environment the current_world points to the - shadow world. In this way you can comfortably use the current_world, which should point towards the World - used at the moment. + Global reference to the currently used World, usually this is the + graphical one. However, if you are inside a Use_shadow_world() environment the current_world points to the + shadow world. In this way you can comfortably use the current_world, which should point towards the World + used at the moment. """ - current_world: World = None + robot: Object = None """ Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the URDF with the name of the URDF on the parameter server. """ - robot: Object = None + data_directory: List[str] = {os.path.dirname(__file__) + "/../../resources"} """ Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. """ - data_directory: List[str] = {os.path.dirname(__file__) + "/../../resources"} def __init__(self, mode: str, is_prospection_world: bool): """ @@ -176,67 +207,61 @@ def add_constraint(self, constraint: Constraint) -> int: def remove_constraint(self, constraint_id): pass - def create_fixed_constraint_to_parent_link_at_child_origin(self, - parent_object: Object, - child_object: Object, - parent_link: str) -> Constraint: - parent_link_id = parent_object.get_link_id(parent_link) - child_base_wrt_parent_link = \ - parent_object._calculate_transform_from_other_object_base_to_this_object_link(child_object, parent_link) - constraint = Constraint(parent_object.id, - parent_link_id, - child_object.id, + def add_fixed_constraint_between_parent_link_and_child_base(self, + parent_object: Object, + child_object: Object, + child_base_wrt_parent_link: Transform, + parent_link: Optional[str] = None) -> int: + """ + Creates a fixed joint constraint between the given parent link and the base of the child object, + the joint frame will be at the origin of the parent link and the child base, and would have the same orientation + as the child base frame. + + returns the constraint id + """ + + constraint = Constraint(parent_obj_id=parent_object.id, + parent_link_id=parent_object.get_link_id(parent_link) if parent_link else -1, + child_obj_id=child_object.id, child_link_id=-1, # -1 means use the base link joint_type=JointType.FIXED, - joint_axis_in_child_link_frame=[0, 1, 0], + joint_axis_in_child_link_frame=[0, 0, 0], joint_frame_position_wrt_parent_origin=child_base_wrt_parent_link.translation_as_list(), joint_frame_position_wrt_child_origin=[0, 0, 0], joint_frame_orientation_wrt_parent_origin=child_base_wrt_parent_link.rotation_as_list(), ) - return constraint - - def add_attachment_to_parent_and_child_attachments(self, - parent_object: Object, - child_object: Object, - new_attachment: Attachment): - - parent_object.attachments[child_object] = [child_base_wrt_parent_link, parent_link, loose] - child_object.attachments[parent_object] = [child_base_wrt_parent_link.invert(), None, False] + constraint_id = self.add_constraint(constraint) + return constraint_id def attach_child_object_base_to_parent_object_base(self, parent_obj: Object, child_obj: Object, loose: Optional[bool] = False) -> None: pass # TODO: implement this function - def attach_child_object_base_to_parent_object_link(self, parent_obj: Object, + def _create_attachment_and_constraint(self, + parent_object: Object, + child_object: Object, + parent_link: str, + loose: Optional[bool] = False) -> None: + + # Add the attachment to the attachment dictionary of both objects + attachment = Attachment(parent_object, child_object, parent_link, loose) + + # Create the constraint + constraint_id = self.add_fixed_constraint_between_parent_link_and_child_base(parent_object, + child_object, + attachment.transform_from_child_base_to_parent_link, + parent_link) + + # Update the attachment and constraint dictionary in both objects + parent_object.cids[child_object] = constraint_id + child_object.cids[parent_object] = constraint_id + + def attach_child_object_base_to_parent_object_link(self, + parent_obj: Object, child_obj: Object, - parent_link: str, + parent_link: Optional[str] = None, loose: Optional[bool] = False) -> None: - parent_link_id = parent_obj.get_link_id(parent_link) - child_base_wrt_parent_link = \ - parent_obj._calculate_transform_from_other_object_base_to_this_object_link(child_obj, parent_link) - parent_obj.attachments[child_obj] = [child_base_wrt_parent_link, parent_link, loose] - child_obj.attachments[parent_obj] = [child_base_wrt_parent_link.invert(), None, False] - constraint = Constraint(parent_obj.id, - parent_link_id, - child_obj.id, - child_link_id=-1, # -1 means use the base link - joint_type=JointType.FIXED, - joint_axis_in_child_link_frame=[0, 1, 0], - joint_frame_position_wrt_parent_origin=child_base_wrt_parent_link.translation_as_list(), - joint_frame_position_wrt_child_origin=[0, 0, 0], - joint_frame_orientation_wrt_parent_origin=child_base_wrt_parent_link.rotation_as_list(), - ) - constraint_id = self.add_constraint(constraint) - parent_obj.cids[child_obj] = constraint_id - child_obj.cids[parent_obj] = constraint_id - self.attachment_event(parent_obj, [parent_obj, child_obj]) - - def attach_objects(self, - parent_obj: Object, - child_obj: Object, - parent_link: Optional[str] = None, - loose: Optional[bool] = False) -> None: """ Attaches another object to this object. This is done by saving the transformation between the given link, if there is one, and @@ -252,24 +277,8 @@ def attach_objects(self, :param parent_link: The link of the parent object to which the child object should be attached :param loose: If the attachment should be a loose attachment. """ - parent_link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 - child_base_wrt_parent_link =\ - parent_obj._calculate_transform_from_other_object_base_to_this_object_link(child_obj, parent_link) - parent_obj.attachments[child_obj] = [child_base_wrt_parent_link, parent_link, loose] - child_obj.attachments[parent_obj] = [child_base_wrt_parent_link.invert(), None, False] - constraint = Constraint(parent_obj.id, - parent_link_id, - child_obj.id, - child_link_id=-1, - joint_type=JointType.FIXED, - joint_axis_in_child_link_frame=[0, 1, 0], - joint_frame_position_wrt_parent_origin=child_base_wrt_parent_link.translation_as_list(), - joint_frame_position_wrt_child_origin=[0, 0, 0], - joint_frame_orientation_wrt_parent_origin=child_base_wrt_parent_link.rotation_as_list(), - ) - constraint_id = self.add_constraint(constraint) - parent_obj.cids[child_obj] = constraint_id - child_obj.cids[parent_obj] = constraint_id + + self._create_attachment_and_constraint(parent_obj, child_obj, parent_link, loose) self.attachment_event(parent_obj, [parent_obj, child_obj]) def detach_objects(self, obj1: Object, obj2: Object) -> None: @@ -291,7 +300,7 @@ def detach_objects(self, obj1: Object, obj2: Object) -> None: del obj2.cids[obj1] obj1.world.detachment_event(obj1, [obj1, obj2]) - def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: + def _set_attached_objects(self, parent, prev_object: List[Object]) -> None: """ Updates the positions of all attached objects. This is done by calculating the new pose in world coordinate frame and setting the @@ -302,35 +311,39 @@ def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: :param prev_object: A list of Objects that were already moved, these will be excluded to prevent loops in the update. """ - for att_obj in obj.attachments: - if att_obj in prev_object: + for child in parent.attachments: + if child in prev_object: continue - if obj.attachments[att_obj].loose: + if parent.attachments[child].loose: + parent_link = parent.attachments[child].parent_link # Updates the attachment transformation and constraint if the # attachment is loose, instead of updating the position of all attached objects - link_to_object = ( - obj._calculate_transform_from_other_object_base_to_this_object_link(att_obj, - obj.attachments[att_obj].parent_link)) - link_id = obj.get_link_id(obj.attachments[att_obj].parent_link) if obj.attachments[att_obj].parent_link else -1 - obj.attachments[att_obj].transform_from_child_base_to_parent_link = link_to_object - att_obj.attachments[obj].transform_from_child_base_to_parent_link = link_to_object.invert() - self.remove_constraint(obj.cids[att_obj]) - # TODO: Ask Jonas what is the benefit of joint axis in a fixed joint. - cid = p.createConstraint(obj.id, link_id, att_obj.id, -1, p.JOINT_FIXED, [0, 0, 0], - link_to_object.translation_as_list(), - [0, 0, 0], link_to_object.rotation_as_list(), - physicsClientId=self.client_id) - obj.cids[att_obj] = cid - att_obj.cids[obj] = cid + + # Update the attachment transformation + # link_to_object = ( + # parent._calculate_transform_from_other_object_base_to_this_object_link(child, parent_link)) + # link_id = parent.get_link_id(parent.attachments[child].parent_link) if parent.attachments[child].parent_link else -1 + # parent.attachments[child].transform_from_child_base_to_parent_link = link_to_object + # child.attachments[parent].transform_from_child_base_to_parent_link = link_to_object.invert() + + # Update the constraint + self.remove_constraint(parent.cids[child]) + constraint_id = self.add_fixed_constraint_between_parent_link_and_child_base(parent, child, parent_link) + # cid = p.createConstraint(parent.id, link_id, child.id, -1, p.JOINT_FIXED, [0, 0, 0], + # link_to_object.translation_as_list(), + # [0, 0, 0], link_to_object.rotation_as_list(), + # physicsClientId=self.client_id) + parent.cids[child] = constraint_id + child.cids[parent] = constraint_id else: - link_to_object = obj.attachments[att_obj][0] + link_to_object = parent.attachments[child].transform_from_child_base_to_parent_link - world_to_object = obj.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") - p.resetBasePositionAndOrientation(att_obj.id, world_to_object.position_as_list(), + world_to_object = parent.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") + p.resetBasePositionAndOrientation(child.id, world_to_object.position_as_list(), world_to_object.orientation_as_list(), physicsClientId=self.client_id) - att_obj._current_pose = world_to_object - self._set_attached_objects(att_obj, prev_object + [obj]) + child._current_pose = world_to_object + self._set_attached_objects(child, prev_object + [parent]) def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: """ @@ -868,7 +881,7 @@ def add_constraint(self, constraint: Constraint) -> int: constraint.child_obj_id, constraint.child_link_id, constraint.joint_type, - constraint.joint_axis, + constraint.joint_axis_in_child_link_frame, constraint.joint_frame_position_wrt_parent_origin, constraint.joint_frame_position_wrt_child_origin, constraint.joint_frame_orientation_wrt_parent_origin, @@ -879,11 +892,11 @@ def add_constraint(self, constraint: Constraint) -> int: def remove_constraint(self, constraint_id): p.removeConstraint(constraint_id, physicsClientId=self.client_id) - def attach_objects(self, - parent_obj: Object, - child_obj: Object, - parent_link: Optional[str] = None, - loose: Optional[bool] = False) -> None: + def attach_child_object_base_to_parent_object_link(self, + parent_obj: Object, + child_obj: Object, + parent_link: Optional[str] = None, + loose: Optional[bool] = False) -> None: """ Attaches another object to this object. This is done by saving the transformation between the given link, if there is one, and @@ -1855,7 +1868,7 @@ def attach(self, obj: Object, link: Optional[str] = None, loose: Optional[bool] :param link: The link of this object to which the other object should be :param loose: If the attachment should be a loose attachment. """ - self.world.attach_objects(self, obj, link, loose) + self.world.attach_child_object_base_to_parent_object_link(self, obj, link, loose) def detach(self, obj: Object) -> None: """ @@ -2045,7 +2058,7 @@ def get_joint_id(self, name: str) -> int: def get_link_id(self, name: str) -> int: """ - Returns a unique id for a link name. As used by PyBullet. + Returns a unique id for a link name. If the name is None return -1. :param name: The link name :return: The unique id @@ -2054,9 +2067,9 @@ def get_link_id(self, name: str) -> int: def get_link_by_id(self, id: int) -> str: """ - Returns the name of a link for a given unique PyBullet id + Returns the name of a link for a given unique link id - :param id: PyBullet id for link + :param id: id for link :return: The link name """ return dict(zip(self.links.values(), self.links.keys()))[id] From 3b365a865e1206f93df28dec3b173892304f1351 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 8 Dec 2023 20:13:13 +0100 Subject: [PATCH 08/69] [WorldAbstractClass] Updated local transformer class to not depend on Object Class (still need to remove world dependency if needed), The attachment refactoring still needs to be completed. --- src/pycram/designator.py | 2 +- src/pycram/designators/action_designator.py | 2 +- src/pycram/local_transformer.py | 99 +++--------- .../process_modules/boxy_process_modules.py | 2 +- .../process_modules/donbot_process_modules.py | 2 +- src/pycram/world.py | 141 +++++------------- 6 files changed, 60 insertions(+), 188 deletions(-) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index e902df6e4..289ebe498 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -690,7 +690,7 @@ def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: :return: The adjusted grasp pose """ lt = LocalTransformer() - pose_in_object = lt.transform_pose_to_object_base_frame(pose, self.bullet_world_object) + pose_in_object = lt.transform_pose_to_target_frame(pose, self.bullet_world_object.tf_frame) special_knowledge = [] # Initialize as an empty list if self.type in SPECIAL_KNOWLEDGE: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 5f1a8c5cc..2e7e4f2db 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -310,7 +310,7 @@ def perform(self) -> None: # oTm = Object Pose in Frame map oTm = object.get_pose() # Transform the object pose to the object frame, basically the origin of the object frame - mTo = object.local_transformer.transform_pose_to_object_base_frame(oTm, object) + mTo = object.local_transformer.transform_pose_to_target_frame(oTm, object.tf_frame) # Adjust the pose according to the special knowledge of the object designator adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) # Transform the adjusted pose to the map frame diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 23bcb3cdc..5570f10af 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -1,23 +1,16 @@ import sys import logging -import tf - if 'world' in sys.modules: logging.warning("(publisher) Make sure that you are not loading this module from pycram.world.") -import rospkg import rospy -import atexit -from threading import Thread, currentThread -from tf import TransformerROS, transformations -from rospy import Duration, logerr, Rate, is_shutdown -from urdf_parser_py.urdf import URDF +from tf import TransformerROS +from rospy import Duration from geometry_msgs.msg import TransformStamped from .pose import Pose, Transform -from .robot_descriptions import robot_description -from typing import List, Optional, Tuple, Union, Callable +from typing import List, Optional, Union, Iterable class LocalTransformer(TransformerROS): @@ -47,7 +40,7 @@ def __init__(self): if self._initialized: return super().__init__(interpolate=True, cache_time=Duration(10)) - # TF tf_stampeds and static_tf_stampeds of the Robot in the BulletWorld: + # TF tf_stampeds and static_tf_stampeds of the Robot in the World: # These are initialized with the function init_transforms_from_urdf and are # used to update the local transformer with the function update_local_transformer_from_btr self.tf_stampeds: List[TransformStamped] = [] @@ -60,14 +53,6 @@ def __init__(self): # If the singelton was already initialized self._initialized = True - def update_objects_for_current_world(self) -> None: - """ - Updates transformations for all objects that are currently in :py:attr:`~pycram.world.World.current_world` - """ - curr_time = rospy.Time.now() - for obj in list(self.world.current_bullet_world.objects): - self.update_transforms_for_object(obj, curr_time) - def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union[Pose, None]: """ Transforms a given pose to the target frame. @@ -76,7 +61,7 @@ def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union :param target_frame: Name of the TF frame into which the Pose should be transformed :return: A transformed pose in the target frame """ - self.update_objects_for_current_world() + self.world.update_transforms_for_objects_in_current_world() copy_pose = pose.copy() copy_pose.header.stamp = rospy.Time(0) if not self.canTransform(target_frame, pose.frame, rospy.Time(0)): @@ -91,74 +76,28 @@ def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union return Pose(*copy_pose.to_list(), frame=new_pose.header.frame_id) - def transform_pose_to_object_base_frame(self, pose: Pose, world_object: 'world.Object') -> Union[Pose, None]: - """ - Transforms the given pose to the base frame of the given BulletWorld object. - - :param pose: Pose that should be transformed - :param world_object: World Object with frame to which the pose should be transformed - :return: The new pose transformed to the base coordinate frame of the object - """ - return self.transform_pose_to_target_frame(pose, world_object.tf_frame) - - def transform_pose_to_object_link_frame(self, pose: Pose, world_object: 'world.Object', link_name: str) -> Union[Pose, None]: - """ - Transforms the given pose to the link frame of the given BulletWorld object. - - :param pose: Pose that should be transformed - :param world_object: World Object with frame to which the pose should be transformed - :param link_name: Name of the link of the object to which the pose should be transformed - :return: The new pose transformed to the link coordinate frame of the object - """ - target_frame = world_object.get_link_tf_frame(link_name) - return self.transform_pose_to_target_frame(pose, target_frame) - - def transform_to_object_frame(self, pose: Pose, - world_object: 'world.Object', link_name: str = None) -> Union[Pose, None]: - """ - Transforms the given pose to the coordinate frame of the given BulletWorld object. If no link name is given the - base frame of the Object is used, otherwise the link frame is used as target for the transformation. - - :param pose: Pose that should be transformed - :param world_object: World Object in which frame the pose should be transformed - :param link_name: A link of the World Object which will be used as target coordinate frame instead - :return: The new pose the in coordinate frame of the object + def lookup_transform_from_source_to_target_frame(self, source_frame: str, target_frame: str, + time: Optional[rospy.rostime.Time] = None) -> Transform: """ - if link_name: - target_frame = world_object.get_link_tf_frame(link_name) - else: - target_frame = world_object.tf_frame - return self.transform_pose_to_target_frame(pose, target_frame) - - def tf_transform(self, source_frame: str, target_frame: str, - time: Optional[rospy.rostime.Time] = None) -> Transform: + Look up for the latest known transform that transforms a point from source frame to target frame. + If no time is given the last common time between the two frames is used. + :param time: Time at which the transform should be looked up """ - Returns the latest known transform between the 'source_frame' and 'target_frame'. If no time is given the last - common time between the two frames is used. - - :param source_frame: Source frame of the transform - :param target_frame: Target frame of the transform - :param time: Time at which the transform should be - :return: - """ - self.update_objects_for_current_world() + self.world.update_transforms_for_objects_in_current_world() tf_time = time if time else self.getLatestCommonTime(source_frame, target_frame) translation, rotation = self.lookupTransform(source_frame, target_frame, tf_time) return Transform(translation, rotation, source_frame, target_frame) - def update_transforms_for_object(self, world_object: 'world.Object', time: rospy.Time = None) -> None: + def update_transforms(self, transforms: Iterable[Transform], time: rospy.Time = None) -> None: """ - Updates local transforms for a Bullet Object, this includes the base as well as all links - - :param world_object: Object for which the Transforms should be updated - :param time: a specific time that should be used + Updates transforms by updating the time stamps of the header of each transform. If no time is given the current + time is used. """ time = time if time else rospy.Time.now() - for transform in world_object._current_link_transforms.values(): + for transform in transforms: transform.header.stamp = time self.setTransform(transform) - def get_all_frames(self) -> List[str]: """ Returns all know coordinate frames as a list with human-readable entries. @@ -171,12 +110,8 @@ def get_all_frames(self) -> List[str]: def transformPose(self, target_frame, ps) -> Pose: """ - Alias for :func:`~LocalTransformer.transform_pose` to avoid confusion since a similar method exists in the - super class. - - :param target_frame: Frame into which the pose should be transformer - :param ps: Pose that should be transformed - :return: Input pose in the target_frame + Alias for :func:`~LocalTransformer.transform_pose_to_target_frame` to avoid confusion since a similar method + exists in the super class. """ return self.transform_pose_to_target_frame(ps, target_frame) diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index f8c3e5d17..45a163e84 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -156,7 +156,7 @@ def _execute(self, desig): neck_base_frame = local_tf.projection_namespace + '/' + robot_description.chains["neck"].base_link if type(solutions['target']) is str: target = local_tf.projection_namespace + '/' + solutions['target'] - pose_in_neck_base = local_tf.tf_transform(neck_base_frame, target) + pose_in_neck_base = local_tf.lookup_transform_from_source_to_target_frame(neck_base_frame, target) elif helper_deprecated.is_list_pose(solutions['target']) or helper_deprecated.is_list_position(solutions['target']): pose = helper_deprecated.ensure_pose(solutions['target']) pose_in_neck_base = local_tf.tf_pose_transform(local_tf.map_frame, neck_base_frame, pose) diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 44db2771f..81e12caa1 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -135,7 +135,7 @@ def _execute(self, desig): target = local_transformer.projection_namespace + '/' + solutions['target'] if \ local_transformer.projection_namespace else \ solutions['target'] - pose_in_neck_base = local_transformer.tf_transform(neck_base_frame, target) + pose_in_neck_base = local_transformer.lookup_transform_from_source_to_target_frame(neck_base_frame, target) elif helper_deprecated.is_list_pose(solutions['target']) or helper_deprecated.is_list_position(solutions['target']): pose = helper_deprecated.ensure_pose(solutions['target']) pose_in_neck_base = local_transformer.tf_pose_transform(local_transformer.map_frame, neck_base_frame, pose) diff --git a/src/pycram/world.py b/src/pycram/world.py index 9dfffabc5..531923e1d 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -53,7 +53,8 @@ class Attachment: def __init__(self, parent_object: Object, child_object: Object, - parent_link: Optional[str] = None, + parent_link: str, + child_link: str, bidirectional: Optional[bool] = False): """ Creates an attachment between the parent object and the child object. @@ -61,16 +62,12 @@ def __init__(self, self.parent_object = parent_object self.child_object = child_object self.parent_link = parent_link - self.child_link = None + self.child_link = child_link self.bidirectional = bidirectional self.child_base_to_parent_link_transform = self._calculate_transforms() def _calculate_transforms(self): - # TODO: need to handle inverse case where there is a child link but no parent link - if self.parent_link is None: - return self.parent_object._calculate_transform_from_other_object_base_to_this_object_base(self.child_object) - else: - return self.parent_object._calculate_transform_from_other_object_base_to_this_object_link(self.child_object, + return self.parent_object._calculate_transform_from_other_object_base_to_this_object_link(self.child_object, self.parent_link) def update_transforms(self): @@ -797,6 +794,15 @@ def reset_bullet_world(self) -> None: obj.set_positions_of_all_joints(dict(zip(joint_names, joint_poses))) obj.set_pose(obj.original_pose) + def update_transforms_for_objects_in_current_world(self) -> None: + """ + Updates transformations for all objects that are currently in :py:attr:`~pycram.world.World.current_world`. + """ + curr_time = rospy.Time.now() + for obj in list(self.current_world.objects): + obj._update_link_transforms(curr_time) + + class BulletWorld(World): # Check is for sphinx autoAPI to be able to work in a CI workflow @@ -892,39 +898,6 @@ def add_constraint(self, constraint: Constraint) -> int: def remove_constraint(self, constraint_id): p.removeConstraint(constraint_id, physicsClientId=self.client_id) - def attach_child_object_base_to_parent_object_link(self, - parent_obj: Object, - child_obj: Object, - parent_link: Optional[str] = None, - loose: Optional[bool] = False) -> None: - """ - Attaches another object to this object. This is done by - saving the transformation between the given link, if there is one, and - the base pose of the other object. Additionally, the name of the link, to - which the object is attached, will be saved. - Furthermore, a constraint of pybullet will be created so the attachment - also works while simulation. - Loose attachments means that the attachment will only be one-directional. - For example, if the parent object moves, the child object will also move, but not the other way around. - - :param parent_obj: The parent object (jf loose, then this would not move when child moves) - :param child_obj: The child object - :param parent_link: The link of the parent object to which the child object should be attached - :param loose: If the attachment should be a loose attachment. - """ - link_id = parent_obj.get_link_id(parent_link) if parent_link else -1 - link_to_object = parent_obj._calculate_transform(child_obj, parent_link) - parent_obj.attachments[child_obj] = [link_to_object, parent_link, loose] - child_obj.attachments[parent_obj] = [link_to_object.invert(), None, False] - - cid = p.createConstraint(parent_obj.id, link_id, child_obj.id, -1, p.JOINT_FIXED, - [0, 1, 0], link_to_object.translation_as_list(), [0, 0, 0], - link_to_object.rotation_as_list(), - physicsClientId=self.client_id) - parent_obj.cids[child_obj] = cid - child_obj.cids[parent_obj] = cid - self.attachment_event(parent_obj, [parent_obj, child_obj]) - def detach_objects(self, obj1: Object, obj2: Object) -> None: """ Detaches obj2 from obj1. This is done by @@ -944,44 +917,6 @@ def detach_objects(self, obj1: Object, obj2: Object) -> None: del obj2.cids[obj1] obj1.world.detachment_event(obj1, [obj1, obj2]) - def _set_attached_objects(self, obj, prev_object: List[Object]) -> None: - """ - Updates the positions of all attached objects. This is done - by calculating the new pose in world coordinate frame and setting the - base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. - - :param prev_object: A list of Objects that were already moved, - these will be excluded to prevent loops in the update. - """ - for att_obj in obj.attachments: - if att_obj in prev_object: - continue - if obj.attachments[att_obj][2]: - # Updates the attachment transformation and constraint if the - # attachment is loose, instead of updating the position of all attached objects - link_to_object = obj._calculate_transform(att_obj, obj.attachments[att_obj][1]) - link_id = obj.get_link_id(obj.attachments[att_obj][1]) if obj.attachments[att_obj][1] else -1 - obj.attachments[att_obj][0] = link_to_object - att_obj.attachments[obj][0] = link_to_object.invert() - p.removeConstraint(obj.cids[att_obj], physicsClientId=self.client_id) - cid = p.createConstraint(obj.id, link_id, att_obj.id, -1, p.JOINT_FIXED, [0, 0, 0], - link_to_object.translation_as_list(), - [0, 0, 0], link_to_object.rotation_as_list(), - physicsClientId=self.client_id) - obj.cids[att_obj] = cid - att_obj.cids[obj] = cid - else: - link_to_object = obj.attachments[att_obj][0] - - world_to_object = obj.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") - p.resetBasePositionAndOrientation(att_obj.id, world_to_object.position_as_list(), - world_to_object.orientation_as_list(), - physicsClientId=self.client_id) - att_obj._current_pose = world_to_object - self._set_attached_objects(att_obj, prev_object + [obj]) - def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: """ Get the joint limits of an articulated object @@ -1816,14 +1751,14 @@ def __init__(self, name: str, type: Union[str, ObjectType], path: str, self.links[self.urdf_object.get_root()] = -1 self._current_pose = pose_in_map - self._current_link_poses = {} - self._current_link_transforms = {} + self._current_link_poses: Dict[str, Pose] = {} + self._current_link_transforms: Dict[str, Transform] = {} self._current_joints_positions = {} self._init_current_positions_of_joint() self._update_link_poses() self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) - self.local_transformer.update_transforms_for_object(self) + self._update_link_transforms() self.link_to_geometry = self._get_geometry_for_link() self.world.objects.append(self) @@ -1967,29 +1902,22 @@ def _set_attached_objects(self, prev_object: List[Object]) -> None: self.world._set_attached_objects(self, prev_object) def _calculate_transform_from_other_object_base_to_this_object_base(self, other_object: Object): - pose_wrt_this_object_base = self.local_transformer.transform_pose_to_object_base_frame(other_object.pose, self) + pose_wrt_this_object_base = self.local_transformer.transform_pose_to_target_frame(other_object.pose, + self.tf_frame) return Transform.from_pose_and_child_frame(pose_wrt_this_object_base, other_object.tf_frame) def _calculate_transform_from_other_object_base_to_this_object_link(self, other_object: Object, this_object_link_name: str): - pose_wrt_this_object_link = self.local_transformer.transform_pose_to_object_link_frame(other_object.pose, - self, - this_object_link_name) + pose_wrt_this_object_link = self.transform_pose_to_link_frame(other_object.pose, this_object_link_name) return Transform.from_pose_and_child_frame(pose_wrt_this_object_link, other_object.tf_frame) - def _calculate_transform(self, obj: Object, link: str = None) -> Transform: + def transform_pose_to_link_frame(self, pose: Pose, link_name: str) -> Union[Pose, None]: """ - Calculates the transformation between another object and the given - link of this object. If no link is provided then the base position will be used. - - :param obj: The other object for which the transformation should be calculated - :param link: The optional link name - :return: The transformation from the link (or base position) to the other objects base position + :return: The new pose transformed to be relative to the link coordinate frame. """ - transform = self.local_transformer.transform_to_object_frame(obj.pose, self, link) - - return Transform(transform.position_as_list(), transform.orientation_as_list(), transform.frame, obj.tf_frame) + target_frame = self.get_link_tf_frame(link_name) + return self.local_transformer.transform_pose_to_target_frame(pose, target_frame) def set_position(self, position: Union[Pose, Point], base=False) -> None: """ @@ -2083,16 +2011,22 @@ def get_joint_by_id(self, joint_id: int) -> str: """ return dict(zip(self.joints.values(), self.joints.keys()))[joint_id] - def get_link_relative_to_other_link(self, source_frame: str, target_frame: str) -> Pose: + def get_link_pose_relative_to_other_link(self, child_link_name: str, parent_link_name: str) -> Pose: """ - Calculates the position of a link in the coordinate frame of another link. - - :param source_frame: The name of the source frame - :param target_frame: The name of the target frame + Calculates the pose of a link (child_link) in the coordinate frame of another link (parent_link). :return: The pose of the source frame in the target frame """ - source_pose = self.get_link_pose(source_frame) - return self.local_transformer.transform_to_object_frame(source_pose, self, target_frame) + child_link_pose = self.get_link_pose(child_link_name) + parent_link_frame = self.get_link_tf_frame(parent_link_name) + return self.local_transformer.transform_pose_to_target_frame(child_link_pose, parent_link_frame) + + def get_transform_between_two_links(self, source_link_name: str, target_link_name: str) -> Transform: + """ + Calculates the transform from source link frame to target link frame. + """ + source_tf_frame = self.get_link_tf_frame(source_link_name) + target_tf_frame = self.get_link_tf_frame(target_link_name) + return self.local_transformer.lookup_transform_from_source_to_target_frame(source_tf_frame, target_tf_frame) def get_link_position(self, name: str) -> Point: """ @@ -2365,6 +2299,9 @@ def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]: link_to_geometry[link] = link_obj.collision.geometry return link_to_geometry + def _update_link_transforms(self, transform_time: Optional[rospy.Time] = None): + self.local_transformer.update_transforms(self._current_link_transforms.values(), transform_time) + def _update_link_poses(self) -> None: """ Updates the cached poses and transforms for each link of this Object From bac3a4b5d23d03103f7fe523e0f5ad84bdc522e0 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sat, 9 Dec 2023 20:24:59 +0100 Subject: [PATCH 09/69] [RefactoringAttachments] Improved attachment class to handle bidirectional and unidirectional attachments. --- src/pycram/local_transformer.py | 8 +- src/pycram/world.py | 307 +++++++++++++++++--------------- 2 files changed, 169 insertions(+), 146 deletions(-) diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 5570f10af..05d149277 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -43,11 +43,13 @@ def __init__(self): # TF tf_stampeds and static_tf_stampeds of the Robot in the World: # These are initialized with the function init_transforms_from_urdf and are # used to update the local transformer with the function update_local_transformer_from_btr + # TODO: Ask Jonas if this is still needed self.tf_stampeds: List[TransformStamped] = [] self.static_tf_stampeds: List[TransformStamped] = [] # Since this file can't import world.py this holds the reference to the current_bullet_world self.world = None + # TODO: Ask Jonas if this is still needed self.prospection_world = None # If the singelton was already initialized @@ -55,7 +57,7 @@ def __init__(self): def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union[Pose, None]: """ - Transforms a given pose to the target frame. + Transforms a given pose to the target frame after updating the transforms for all objects in the current world. :param pose: Pose that should be transformed :param target_frame: Name of the TF frame into which the Pose should be transformed @@ -79,8 +81,8 @@ def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union def lookup_transform_from_source_to_target_frame(self, source_frame: str, target_frame: str, time: Optional[rospy.rostime.Time] = None) -> Transform: """ - Look up for the latest known transform that transforms a point from source frame to target frame. - If no time is given the last common time between the two frames is used. + Update the transforms for all world objects then Look up for the latest known transform that transforms a point + from source frame to target frame. If no time is given the last common time between the two frames is used. :param time: Time at which the transform should be looked up """ self.world.update_transforms_for_objects_in_current_world() diff --git a/src/pycram/world.py b/src/pycram/world.py index 531923e1d..a587bc09d 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -35,53 +35,92 @@ from dataclasses import dataclass -@dataclass -class Constraint: - parent_obj_id: int - parent_link_id: int - child_obj_id: int - child_link_id: int - joint_type: JointType - joint_axis_in_child_link_frame: List[int] - joint_frame_position_wrt_parent_origin: List[float] - joint_frame_position_wrt_child_origin: List[float] - joint_frame_orientation_wrt_parent_origin: Optional[List[float]] = None - joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None - - class Attachment: def __init__(self, parent_object: Object, child_object: Object, - parent_link: str, - child_link: str, + constraint_id: int, + parent_link_id: Optional[int] = -1, # -1 means base link + child_link_id: Optional[int] = -1, bidirectional: Optional[bool] = False): """ Creates an attachment between the parent object and the child object. """ self.parent_object = parent_object self.child_object = child_object - self.parent_link = parent_link - self.child_link = child_link + self.constraint_id = constraint_id + self.parent_link_id = parent_link_id + self.child_link_id = child_link_id self.bidirectional = bidirectional - self.child_base_to_parent_link_transform = self._calculate_transforms() + self.child_to_parent_transform = self.calculate_transforms() + self._loose = False and not self.bidirectional - def _calculate_transforms(self): - return self.parent_object._calculate_transform_from_other_object_base_to_this_object_link(self.child_object, - self.parent_link) + def update_attachment(self): + self.update_transforms() + self.update_constraint() + self.update_objects_constraints_collection() + self.update_objects_attachments_collection() def update_transforms(self): - self.child_base_to_parent_link_transform = self._calculate_transforms() + self.child_to_parent_transform = self.calculate_transforms() - def get_inverse(self): - attachment = Attachment(self.child_object, self.parent_object, None, self.bidirectional) - attachment.child_link = self.parent_link - return attachment + def update_constraint(self): + self.parent_object.world.remove_constraint(self.constraint_id) + self.constraint_id = self.parent_object.world.add_fixed_constraint(self.parent_object.id, + self.child_object.id, + self.child_to_parent_transform, + self.parent_link_id, + self.child_link_id) - def add_to_objects_attachments_collection(self): + def update_objects_constraints_collection(self): + self.parent_object.cids[self.child_object] = self.constraint_id + self.child_object.cids[self.parent_object] = self.constraint_id + + def update_objects_attachments_collection(self): self.parent_object.attachments[self.child_object] = self self.child_object.attachments[self.parent_object] = self.get_inverse() + def get_inverse(self): + attachment = Attachment(self.child_object, self.parent_object, self.constraint_id, self.child_link_id, + self.parent_link_id, self.bidirectional) + attachment.loose = False if self.loose else True + return attachment + + def calculate_transforms(self): + return self.parent_object.get_transform_between_two_links(self.parent_link_id, self.child_link_id) + + @property + def loose(self) -> bool: + """ + If true, then the child object will not move when parent moves. + """ + return self._loose + + @loose.setter + def loose(self, loose: bool): + self._loose = loose and not self.bidirectional + + @property + def is_reversed(self) -> bool: + """ + If true means that when child moves, parent moves not the other way around. + """ + return self.loose + + +@dataclass +class Constraint: + parent_obj_id: int + parent_link_id: int + child_obj_id: int + child_link_id: int + joint_type: JointType + joint_axis_in_child_link_frame: List[int] + joint_frame_position_wrt_parent_origin: List[float] + joint_frame_position_wrt_child_origin: List[float] + joint_frame_orientation_wrt_parent_origin: Optional[List[float]] = None + joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None + class World(ABC): """ @@ -119,7 +158,7 @@ def __init__(self, mode: str, is_prospection_world: bool): self.objects: List[Object] = [] self.client_id: int = -1 self.mode: str = mode - self._initialize_events() + self._init_events() World.current_world = self if World.current_world is None else World.current_world self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} self.local_transformer = LocalTransformer() @@ -133,6 +172,11 @@ def __init__(self, mode: str, is_prospection_world: bool): self._init_and_sync_prospection_world() self._update_local_transformer_worlds() + def _init_events(self): + self.detachment_event: Event = Event() + self.attachment_event: Event = Event() + self.manipulation_event: Event = Event() + def _init_and_sync_prospection_world(self): self._init_prospection_world() self._sync_prospection_world() @@ -148,13 +192,8 @@ def _sync_prospection_world(self): self.world_sync: WorldSync = WorldSync(self, self.prospection_world) self.world_sync.start() - def _initialize_events(self): - self.detachment_event: Event = Event() - self.attachment_event: Event = Event() - self.manipulation_event: Event = Event() - @abstractmethod - def load_urdf_with_pose_and_get_world_object_id(self, path: str, pose: Pose) -> int: + def load_urdf_at_pose_and_get_object_id(self, path: str, pose: Pose) -> int: pass def get_objects_by_name(self, name: str) -> List[Object]: @@ -193,67 +232,37 @@ def remove_object(self, obj_id: int) -> None: """ pass - @abstractmethod - def add_constraint(self, constraint: Constraint) -> int: - """ - Add a constraint between two objects so that they become attached - """ - pass - - @abstractmethod - def remove_constraint(self, constraint_id): - pass - - def add_fixed_constraint_between_parent_link_and_child_base(self, - parent_object: Object, - child_object: Object, - child_base_wrt_parent_link: Transform, - parent_link: Optional[str] = None) -> int: + def _set_attached_objects(self, parent, prev_object: List[Object]) -> None: """ - Creates a fixed joint constraint between the given parent link and the base of the child object, - the joint frame will be at the origin of the parent link and the child base, and would have the same orientation - as the child base frame. + Updates the positions of all attached objects. This is done + by calculating the new pose in world coordinate frame and setting the + base pose of the attached objects to this new pose. + After this the _set_attached_objects method of all attached objects + will be called. - returns the constraint id + :param prev_object: A list of Objects that were already moved, + these will be excluded to prevent loops in the update. """ + for child in parent.attachments: + if child in prev_object: + continue + if not parent.attachments[child].bidirectional: + parent.attachments[child].update_attachment() + else: + link_to_object = parent.attachments[child].child_to_parent_transform - constraint = Constraint(parent_obj_id=parent_object.id, - parent_link_id=parent_object.get_link_id(parent_link) if parent_link else -1, - child_obj_id=child_object.id, - child_link_id=-1, # -1 means use the base link - joint_type=JointType.FIXED, - joint_axis_in_child_link_frame=[0, 0, 0], - joint_frame_position_wrt_parent_origin=child_base_wrt_parent_link.translation_as_list(), - joint_frame_position_wrt_child_origin=[0, 0, 0], - joint_frame_orientation_wrt_parent_origin=child_base_wrt_parent_link.rotation_as_list(), - ) - constraint_id = self.add_constraint(constraint) - return constraint_id + world_to_object = parent.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") + p.resetBasePositionAndOrientation(child.id, world_to_object.position_as_list(), + world_to_object.orientation_as_list(), + physicsClientId=self.client_id) + child._current_pose = world_to_object + self._set_attached_objects(child, prev_object + [parent]) def attach_child_object_base_to_parent_object_base(self, parent_obj: Object, child_obj: Object, loose: Optional[bool] = False) -> None: pass # TODO: implement this function - def _create_attachment_and_constraint(self, - parent_object: Object, - child_object: Object, - parent_link: str, - loose: Optional[bool] = False) -> None: - - # Add the attachment to the attachment dictionary of both objects - attachment = Attachment(parent_object, child_object, parent_link, loose) - - # Create the constraint - constraint_id = self.add_fixed_constraint_between_parent_link_and_child_base(parent_object, - child_object, - attachment.transform_from_child_base_to_parent_link, - parent_link) - - # Update the attachment and constraint dictionary in both objects - parent_object.cids[child_object] = constraint_id - child_object.cids[parent_object] = constraint_id - def attach_child_object_base_to_parent_object_link(self, parent_obj: Object, child_obj: Object, @@ -278,6 +287,64 @@ def attach_child_object_base_to_parent_object_link(self, self._create_attachment_and_constraint(parent_obj, child_obj, parent_link, loose) self.attachment_event(parent_obj, [parent_obj, child_obj]) + def _create_attachment_and_constraint(self, + parent_object: Object, + child_object: Object, + parent_link_id: Optional[int] = -1, + child_link_id: Optional[int] = -1, + loose: Optional[bool] = False) -> None: + + # Add the attachment to the attachment dictionary of both objects + attachment = Attachment(parent_object, child_object, parent_link_id, loose) + + # Create the constraint + constraint_id = self.add_fixed_constraint(parent_object.id, + child_object.id, + attachment.child_to_parent_transform, + parent_object.get_link_id(parent_link_id)) + + # Update the attachment and constraint dictionary in both objects + parent_object.cids[child_object] = constraint_id + child_object.cids[parent_object] = constraint_id + + def add_fixed_constraint(self, + parent_object_id: int, + child_object_id: int, + child_to_parent_transform: Transform, + parent_link_id: Optional[int] = -1, + child_link_id: Optional[int] = -1) -> int: + """ + Creates a fixed joint constraint between the given parent and child links, + the joint frame will be at the origin of the child link, and would have the same orientation + as the child base frame. if no link is given, the base link will be used (id = -1). + + returns the constraint id + """ + # -1 in link id means use the base link of the object + constraint = Constraint(parent_obj_id=parent_object_id, + parent_link_id=parent_link_id, + child_obj_id=child_object_id, + child_link_id=child_link_id, + joint_type=JointType.FIXED, + joint_axis_in_child_link_frame=[0, 0, 0], + joint_frame_position_wrt_parent_origin=child_to_parent_transform.translation_as_list(), + joint_frame_position_wrt_child_origin=[0, 0, 0], + joint_frame_orientation_wrt_parent_origin=child_to_parent_transform.rotation_as_list(), + ) + constraint_id = self.add_constraint(constraint) + return constraint_id + + @abstractmethod + def add_constraint(self, constraint: Constraint) -> int: + """ + Add a constraint between two objects so that they become attached + """ + pass + + @abstractmethod + def remove_constraint(self, constraint_id): + pass + def detach_objects(self, obj1: Object, obj2: Object) -> None: """ Detaches obj2 from obj1. This is done by @@ -297,51 +364,6 @@ def detach_objects(self, obj1: Object, obj2: Object) -> None: del obj2.cids[obj1] obj1.world.detachment_event(obj1, [obj1, obj2]) - def _set_attached_objects(self, parent, prev_object: List[Object]) -> None: - """ - Updates the positions of all attached objects. This is done - by calculating the new pose in world coordinate frame and setting the - base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. - - :param prev_object: A list of Objects that were already moved, - these will be excluded to prevent loops in the update. - """ - for child in parent.attachments: - if child in prev_object: - continue - if parent.attachments[child].loose: - parent_link = parent.attachments[child].parent_link - # Updates the attachment transformation and constraint if the - # attachment is loose, instead of updating the position of all attached objects - - # Update the attachment transformation - # link_to_object = ( - # parent._calculate_transform_from_other_object_base_to_this_object_link(child, parent_link)) - # link_id = parent.get_link_id(parent.attachments[child].parent_link) if parent.attachments[child].parent_link else -1 - # parent.attachments[child].transform_from_child_base_to_parent_link = link_to_object - # child.attachments[parent].transform_from_child_base_to_parent_link = link_to_object.invert() - - # Update the constraint - self.remove_constraint(parent.cids[child]) - constraint_id = self.add_fixed_constraint_between_parent_link_and_child_base(parent, child, parent_link) - # cid = p.createConstraint(parent.id, link_id, child.id, -1, p.JOINT_FIXED, [0, 0, 0], - # link_to_object.translation_as_list(), - # [0, 0, 0], link_to_object.rotation_as_list(), - # physicsClientId=self.client_id) - parent.cids[child] = constraint_id - child.cids[parent] = constraint_id - else: - link_to_object = parent.attachments[child].transform_from_child_base_to_parent_link - - world_to_object = parent.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") - p.resetBasePositionAndOrientation(child.id, world_to_object.position_as_list(), - world_to_object.orientation_as_list(), - physicsClientId=self.client_id) - child._current_pose = world_to_object - self._set_attached_objects(child, prev_object + [parent]) - def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: """ Get the joint limits of an articulated object @@ -837,7 +859,7 @@ def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): if not is_prospection_world: plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def load_urdf_with_pose_and_get_world_object_id(self, path: str, pose: Pose) -> int: + def load_urdf_at_pose_and_get_object_id(self, path: str, pose: Pose) -> int: return p.loadURDF(path, basePosition=pose.position_as_list(), baseOrientation=pose.orientation_as_list(), physicsClientId=self.client_id) @@ -1906,12 +1928,6 @@ def _calculate_transform_from_other_object_base_to_this_object_base(self, other_ self.tf_frame) return Transform.from_pose_and_child_frame(pose_wrt_this_object_base, other_object.tf_frame) - def _calculate_transform_from_other_object_base_to_this_object_link(self, - other_object: Object, - this_object_link_name: str): - pose_wrt_this_object_link = self.transform_pose_to_link_frame(other_object.pose, this_object_link_name) - return Transform.from_pose_and_child_frame(pose_wrt_this_object_link, other_object.tf_frame) - def transform_pose_to_link_frame(self, pose: Pose, link_name: str) -> Union[Pose, None]: """ :return: The new pose transformed to be relative to the link coordinate frame. @@ -2020,12 +2036,12 @@ def get_link_pose_relative_to_other_link(self, child_link_name: str, parent_link parent_link_frame = self.get_link_tf_frame(parent_link_name) return self.local_transformer.transform_pose_to_target_frame(child_link_pose, parent_link_frame) - def get_transform_between_two_links(self, source_link_name: str, target_link_name: str) -> Transform: + def get_transform_between_two_links(self, source_link_id: int, target_link_id: int) -> Transform: """ Calculates the transform from source link frame to target link frame. """ - source_tf_frame = self.get_link_tf_frame(source_link_name) - target_tf_frame = self.get_link_tf_frame(target_link_name) + source_tf_frame = self.get_link_tf_frame_by_id(source_link_id) + target_tf_frame = self.get_link_tf_frame_by_id(target_link_id) return self.local_transformer.lookup_transform_from_source_to_target_frame(source_tf_frame, target_tf_frame) def get_link_position(self, name: str) -> Point: @@ -2056,7 +2072,9 @@ def get_link_pose(self, name: str) -> Pose: if name in self.links.keys() and self.links[name] == -1: return self.get_pose() return self._current_link_poses[name] - # return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6]) + + def get_link_pose_by_id(self, link_id: int) -> Pose: + return self.get_link_pose(self.get_link_by_id(link_id)) def set_joint_position(self, joint_name: str, joint_pose: float) -> None: """ @@ -2284,6 +2302,9 @@ def get_link_tf_frame(self, link_name: str) -> str: """ return self.tf_frame + "/" + link_name + def get_link_tf_frame_by_id(self, link_id: int) -> str: + return self.get_link_tf_frame(self.get_link_by_id(link_id)) + def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]: """ Extracts the geometry information for each collision of each link and links them to the respective link. @@ -2436,7 +2457,7 @@ def _load_object(name: str, path = cach_dir + name + ".urdf" try: - obj = world.load_urdf_with_pose_and_get_world_object_id(path, Pose(position, orientation)) + obj = world.load_urdf_at_pose_and_get_object_id(path, Pose(position, orientation)) return obj, path except p.error as e: logging.error( From 5b61daa7f631c339e710c66ce018e2b6c3769534 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sat, 9 Dec 2023 23:30:51 +0100 Subject: [PATCH 10/69] [RefactoringAttachments] Improved attachment class, needs to better implement the inverse attachment (changes are not synced) --- src/pycram/world.py | 122 +++++++++++++++++++------------------------- 1 file changed, 53 insertions(+), 69 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index a587bc09d..1afe260c4 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -39,50 +39,55 @@ class Attachment: def __init__(self, parent_object: Object, child_object: Object, - constraint_id: int, parent_link_id: Optional[int] = -1, # -1 means base link child_link_id: Optional[int] = -1, - bidirectional: Optional[bool] = False): + bidirectional: Optional[bool] = False, + constraint_id: Optional[int] = None): """ Creates an attachment between the parent object and the child object. """ self.parent_object = parent_object self.child_object = child_object - self.constraint_id = constraint_id self.parent_link_id = parent_link_id self.child_link_id = child_link_id self.bidirectional = bidirectional self.child_to_parent_transform = self.calculate_transforms() self._loose = False and not self.bidirectional + self.constraint_id = self.update_constraint() if constraint_id is None else constraint_id def update_attachment(self): self.update_transforms() self.update_constraint() - self.update_objects_constraints_collection() self.update_objects_attachments_collection() def update_transforms(self): self.child_to_parent_transform = self.calculate_transforms() def update_constraint(self): - self.parent_object.world.remove_constraint(self.constraint_id) - self.constraint_id = self.parent_object.world.add_fixed_constraint(self.parent_object.id, - self.child_object.id, - self.child_to_parent_transform, - self.parent_link_id, - self.child_link_id) - - def update_objects_constraints_collection(self): - self.parent_object.cids[self.child_object] = self.constraint_id - self.child_object.cids[self.parent_object] = self.constraint_id + if self.constraint_id is not None: + self.parent_object.world.remove_constraint(self.constraint_id) + self.constraint_id = self.add_fixed_constraint() + self.update_objects_constraints_collection() def update_objects_attachments_collection(self): self.parent_object.attachments[self.child_object] = self self.child_object.attachments[self.parent_object] = self.get_inverse() + def add_fixed_constraint(self): + constraint_id = self.parent_object.world.add_fixed_constraint(self.parent_object.id, + self.child_object.id, + self.child_to_parent_transform, + self.parent_link_id, + self.child_link_id) + return constraint_id + + def update_objects_constraints_collection(self): + self.parent_object.cids[self.child_object] = self.constraint_id + self.child_object.cids[self.parent_object] = self.constraint_id + def get_inverse(self): - attachment = Attachment(self.child_object, self.parent_object, self.constraint_id, self.child_link_id, - self.parent_link_id, self.bidirectional) + attachment = Attachment(self.child_object, self.parent_object, self.child_link_id, + self.parent_link_id, self.bidirectional, self.constraint_id) attachment.loose = False if self.loose else True return attachment @@ -258,54 +263,22 @@ def _set_attached_objects(self, parent, prev_object: List[Object]) -> None: child._current_pose = world_to_object self._set_attached_objects(child, prev_object + [parent]) - def attach_child_object_base_to_parent_object_base(self, parent_obj: Object, - child_obj: Object, - loose: Optional[bool] = False) -> None: - pass # TODO: implement this function - - def attach_child_object_base_to_parent_object_link(self, - parent_obj: Object, - child_obj: Object, - parent_link: Optional[str] = None, - loose: Optional[bool] = False) -> None: + def attach_objects(self, + parent_object: Object, + child_object: Object, + parent_link_id: Optional[int] = -1, + child_link_id: Optional[int] = -1, + bidirectional: Optional[bool] = True) -> None: """ - Attaches another object to this object. This is done by - saving the transformation between the given link, if there is one, and - the base pose of the other object. Additionally, the name of the link, to - which the object is attached, will be saved. - Furthermore, a constraint will be created so the attachment - also works while simulation. - Loose attachments means that the attachment will only be one-directional. - For example, if the parent object moves, the child object will also move, but not the other way around. + Attaches two objects together by saving the current transformation between the links coordinate frames. + Furthermore, a constraint will be created so the attachment also works while in simulation. - :param parent_obj: The parent object (jf loose, then this would not move when child moves) - :param child_obj: The child object - :param parent_link: The link of the parent object to which the child object should be attached - :param loose: If the attachment should be a loose attachment. + :param bidirectional: If the parent should also follow the child. """ - self._create_attachment_and_constraint(parent_obj, child_obj, parent_link, loose) - self.attachment_event(parent_obj, [parent_obj, child_obj]) - - def _create_attachment_and_constraint(self, - parent_object: Object, - child_object: Object, - parent_link_id: Optional[int] = -1, - child_link_id: Optional[int] = -1, - loose: Optional[bool] = False) -> None: - # Add the attachment to the attachment dictionary of both objects - attachment = Attachment(parent_object, child_object, parent_link_id, loose) - - # Create the constraint - constraint_id = self.add_fixed_constraint(parent_object.id, - child_object.id, - attachment.child_to_parent_transform, - parent_object.get_link_id(parent_link_id)) - - # Update the attachment and constraint dictionary in both objects - parent_object.cids[child_object] = constraint_id - child_object.cids[parent_object] = constraint_id + Attachment(parent_object, child_object, parent_link_id, child_link_id, bidirectional) + self.attachment_event(parent_object, [parent_object, child_object]) def add_fixed_constraint(self, parent_object_id: int, @@ -315,8 +288,8 @@ def add_fixed_constraint(self, child_link_id: Optional[int] = -1) -> int: """ Creates a fixed joint constraint between the given parent and child links, - the joint frame will be at the origin of the child link, and would have the same orientation - as the child base frame. if no link is given, the base link will be used (id = -1). + the joint frame will be at the origin of the child link frame, and would have the same orientation + as the child link frame. if no link is given, the base link will be used (id = -1). returns the constraint id """ @@ -1810,7 +1783,11 @@ def remove(self) -> None: if World.robot == self: World.robot = None - def attach(self, obj: Object, link: Optional[str] = None, loose: Optional[bool] = False) -> None: + def attach(self, + child_object: Object, + parent_link: Optional[str] = None, + child_link: Optional[str] = None, + bidirectional: Optional[bool] = True) -> None: """ Attaches another object to this object. This is done by saving the transformation between the given link, if there is one, and @@ -1821,22 +1798,27 @@ def attach(self, obj: Object, link: Optional[str] = None, loose: Optional[bool] Loose attachments means that the attachment will only be one-directional. For example, if this object moves the other, attached, object will also move but not the other way around. - :param object: The other object that should be attached - :param link: The link of this object to which the other object should be - :param loose: If the attachment should be a loose attachment. + :param child_object: The other object that should be attached. + :param parent_link: The link name of this object. + :param child_link: The link name of the other object. + :param bidirectional: If the attachment should be a loose attachment. """ - self.world.attach_child_object_base_to_parent_object_link(self, obj, link, loose) + self.world.attach_objects(self, + child_object, + self.get_link_id(parent_link), + child_object.get_link_id(child_link), + bidirectional) - def detach(self, obj: Object) -> None: + def detach(self, child_object: Object) -> None: """ Detaches another object from this object. This is done by deleting the attachment from the attachments dictionary of both objects and deleting the constraint of pybullet. Afterward the detachment event of the corresponding World will be fired. - :param object: The object which should be detached + :param child_object: The object which should be detached """ - self.world.detach_objects(self, obj) + self.world.detach_objects(self, child_object) def detach_all(self) -> None: """ @@ -2007,6 +1989,8 @@ def get_link_id(self, name: str) -> int: :param name: The link name :return: The unique id """ + if name is None: + return -1 return self.links[name] def get_link_by_id(self, id: int) -> str: From 7fb3524c204d6bc259b3486e23f44293e00a7844 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 10 Dec 2023 12:57:07 +0100 Subject: [PATCH 11/69] [AbstractWorld] Added a WorldState dataclass, Implemented saving and resotring of state, Implemented exiting the world and other methods. --- src/pycram/costmaps.py | 2 +- src/pycram/world.py | 703 +++++++++++++++++++++++------------------ 2 files changed, 395 insertions(+), 310 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 586758694..3d6f63451 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -732,7 +732,7 @@ def get_aabb_for_link(self) -> Tuple[List[float], List[float]]: link_orientation_trans = link_orientation.to_transform(self.object.get_link_tf_frame(self.link)) inverse_orientation = link_orientation_trans.invert() shadow_obj.set_orientation(inverse_orientation.to_pose()) - return shadow_obj.get_AABB(self.link) + return shadow_obj.get_link_AABB(self.link) cmap = colors.ListedColormap(['white', 'black', 'green', 'red', 'blue']) diff --git a/src/pycram/world.py b/src/pycram/world.py index 1afe260c4..6e3d75b1c 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -127,6 +127,13 @@ class Constraint: joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None +@dataclass +class WorldState: + state_id: int + attachments: Dict[Object, Dict[Object, Attachment]] + constraint_ids: Dict[Object, Dict[Object, int]] + + class World(ABC): """ The World Class represents the physics Simulation and belief state. @@ -151,7 +158,7 @@ class World(ABC): Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. """ - def __init__(self, mode: str, is_prospection_world: bool): + def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: float): """ Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -177,6 +184,10 @@ def __init__(self, mode: str, is_prospection_world: bool): self._init_and_sync_prospection_world() self._update_local_transformer_worlds() + self.simulation_time_step = simulation_time_step + self.simulation_frequency = int(1/self.simulation_time_step) + self._saved_states: Dict[int, WorldState] = {} # Different states of the world indexed by int state id. + def _init_events(self): self.detachment_event: Event = Event() self.attachment_event: Event = Event() @@ -257,9 +268,9 @@ def _set_attached_objects(self, parent, prev_object: List[Object]) -> None: link_to_object = parent.attachments[child].child_to_parent_transform world_to_object = parent.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") - p.resetBasePositionAndOrientation(child.id, world_to_object.position_as_list(), - world_to_object.orientation_as_list(), - physicsClientId=self.client_id) + self.reset_object_base_pose(child, + world_to_object.position_as_list(), + world_to_object.orientation_as_list()) child._current_pose = world_to_object self._set_attached_objects(child, prev_object + [parent]) @@ -314,10 +325,6 @@ def add_constraint(self, constraint: Constraint) -> int: """ pass - @abstractmethod - def remove_constraint(self, constraint_id): - pass - def detach_objects(self, obj1: Object, obj2: Object) -> None: """ Detaches obj2 from obj1. This is done by @@ -331,23 +338,50 @@ def detach_objects(self, obj1: Object, obj2: Object) -> None: del obj1.attachments[obj2] del obj2.attachments[obj1] - p.removeConstraint(obj1.cids[obj2], physicsClientId=self.client_id) + self.remove_constraint(obj1.cids[obj2]) del obj1.cids[obj2] del obj2.cids[obj1] + obj1.world.detachment_event(obj1, [obj1, obj2]) + @abstractmethod + def remove_constraint(self, constraint_id): + pass + def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: """ Get the joint limits of an articulated object - :param obj: The object - :param joint_name: The name of the joint + :param obj: The object. + :param joint_name: The name of the joint. :return: A tuple containing the upper and the lower limits of the joint. """ - up_lim, low_lim = p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8:10] - return up_lim, low_lim + return self.get_object_joint_upper_limit(obj, joint_name), self.get_object_joint_lower_limit(obj, joint_name) + + @abstractmethod + def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: + """ + Get the joint upper limit of an articulated object + + :param obj: The object. + :param joint_name: The name of the joint. + :return: The joint upper limit as a float. + """ + pass + @abstractmethod + def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: + """ + Get the joint lower limit of an articulated object + + :param obj: The object. + :param joint_name: The name of the joint. + :return: The joint lower limit as a float. + """ + pass + + @abstractmethod def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: """ Returns the axis along which a joint is moving. The given joint_name has to be part of this object. @@ -356,8 +390,9 @@ def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: :param joint_name: Name of the joint for which the axis should be returned. :return: The axis a vector of xyz """ - return p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[13] + pass + @abstractmethod def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: """ Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. @@ -366,9 +401,9 @@ def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: :param joint_name: Joint name for which the type should be returned :return: The type of the joint """ - joint_type = p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[2] - return JointType(joint_type) + pass + @abstractmethod def get_object_joint_position(self, obj: Object, joint_name: str) -> float: """ Get the state of a joint of an articulated object @@ -376,8 +411,9 @@ def get_object_joint_position(self, obj: Object, joint_name: str) -> float: :param obj: The object :param joint_name: The name of the joint """ - return p.getJointState(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[0] + pass + @abstractmethod def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: """ Get the pose of a link of an articulated object with respect to the world frame. @@ -386,18 +422,51 @@ def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List] :param obj: The object :param link_name: The name of the link """ - return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] + pass + def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: + """ + Simulates Physics in the World for a given amount of seconds. Usually this simulation is faster than real + time. By setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal + to real time. + + :param seconds: The amount of seconds that should be simulated. + :param real_time: If the simulation should happen in real time or faster. + """ + for i in range(0, int(seconds * self.simulation_frequency)): + self.step() + for objects, callback in self.coll_callbacks.items(): + contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1]) + if contact_points != (): + callback[0]() + elif callback[1] is not None: # Call no collision callback + callback[1]() + if real_time: + # Simulation runs at 240 Hz + time.sleep(self.simulation_time_step) + + @abstractmethod def get_object_contact_points(self, obj: Object) -> List: """ - Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ + Returns a list of contact points of this Object with other Objects. :param obj: The object. :return: A list of all contact points with other objects """ - return p.getContactPoints(obj.id) + pass + + @abstractmethod + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + """ + Returns a list of contact points between obj1 and obj2. + + :param obj1: The first object. + :param obj2: The second object. + :return: A list of all contact points between the two objects. + """ + pass + @abstractmethod def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: """ Get the ID of a joint in an articulated object. @@ -405,8 +474,9 @@ def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: :param obj: The object :param joint_idx: The index of the joint (would indicate order). """ - return p.getJointInfo(obj.id, joint_idx, self.client_id)[0] + pass + @abstractmethod def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: """ Get the name of a joint in an articulated object. @@ -414,8 +484,9 @@ def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: :param obj: The object :param joint_idx: The index of the joint (would indicate order). """ - return p.getJointInfo(obj.id, joint_idx, self.client_id)[1].decode('utf-8') + pass + @abstractmethod def get_object_link_name(self, obj: Object, link_idx: int) -> str: """ Get the name of a link in an articulated object. @@ -423,16 +494,18 @@ def get_object_link_name(self, obj: Object, link_idx: int) -> str: :param obj: The object :param link_idx: The index of the link (would indicate order). """ - return p.getJointInfo(obj.id, link_idx, self.client_id)[12].decode('utf-8') + pass + @abstractmethod def get_object_number_of_joints(self, obj: Object) -> int: """ Get the number of joints of an articulated object :param obj: The object """ - return p.getNumJoints(obj.id, self.client_id) + pass + @abstractmethod def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ Reset the joint position instantly without physics simulation @@ -441,8 +514,9 @@ def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) :param joint_name: The name of the joint :param joint_pose: The new joint pose """ - p.resetJointState(obj.id, obj.joints[joint_name], joint_pose, physicsClientId=self.client_id) + pass + @abstractmethod def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): """ Reset the world position and orientation of the base of the object instantaneously, @@ -452,13 +526,14 @@ def reset_object_base_pose(self, obj: Object, position: List[float], orientation :param position: The new position of the object as a vector of x,y,z :param orientation: The new orientation of the object as a quaternion of x,y,z,w """ - p.resetBasePositionAndOrientation(obj.id, position, orientation, self.client_id) + pass + @abstractmethod def step(self): """ Step the world simulation using forward dynamics """ - p.stepSimulation(self.client_id) + pass def set_object_color(self, obj: Object, color: List[float], link: Optional[str] = ""): """ @@ -475,11 +550,23 @@ def set_object_color(self, obj: Object, color: List[float], link: Optional[str] # forms or if loaded from an .stl or .obj file if obj.links != {}: for link_id in obj.links.values(): - p.changeVisualShape(obj.id, link_id, rgbaColor=color, physicsClientId=self.client_id) + self.set_object_link_color(obj, link_id, color) else: - p.changeVisualShape(obj.id, -1, rgbaColor=color, physicsClientId=self.client_id) + self.set_object_link_color(obj, -1, color) else: - p.changeVisualShape(obj.id, obj.links[link], rgbaColor=color, physicsClientId=self.client_id) + self.set_object_link_color(obj, obj.links[link], color) + + @abstractmethod + def set_object_link_color(self, obj: Object, link_id: int, rgba_color: List[float]): + """ + Changes the color of a link of this object, the color has to be given as a 4 element list + of RGBA values. + + :param obj: The object which should be colored + :param link_id: The link id of the link which should be colored + :param rgba_color: The color as RGBA values between 0 and 1 + """ + pass def get_object_color(self, obj: Object, @@ -501,15 +588,11 @@ def get_object_color(self, :param link: the link name for which the color should be returned. :return: The color of the object or link, or a dictionary containing every colored link with its color """ - visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) - swap = {v: k for k, v in obj.links.items()} - links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) - colors = list(map(lambda x: x[7], visual_data)) - link_to_color = dict(zip(links, colors)) + link_to_color_dict = self.get_object_colors(obj) if link: - if link in link_to_color.keys(): - return link_to_color[link] + if link in link_to_color_dict.keys(): + return link_to_color_dict[link] elif link not in obj.links.keys(): rospy.logerr(f"The link '{link}' is not part of this obejct") return None @@ -517,25 +600,43 @@ def get_object_color(self, rospy.logerr(f"The link '{link}' has no color") return None - if len(visual_data) == 1: - return list(visual_data[0][7]) + if len(link_to_color_dict) == 1: + return list(link_to_color_dict.values())[0] else: - return link_to_color + return link_to_color_dict - def get_object_AABB(self, obj: Object, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: + @abstractmethod + def get_object_colors(self, obj: Object) -> Dict[str, List[float]]: """ - Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case - the axis aligned bounding box of the link will be returned. The return of this method are two points in + Get the RGBA colors of each link in the object as a dictionary from link name to color. + + :param obj: The object + :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. + """ + pass + + @abstractmethod + def get_object_AABB(self, obj: Object) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. :param obj: The object for which the bounding box should be returned. - :param link_name: The Optional name of a link of this object. :return: Two lists of x,y,z which define the bounding box. """ - if link_name: - return p.getAABB(obj.id, obj.links[link_name], self.client_id) - else: - return p.getAABB(obj.id, physicsClientId=self.client_id) + pass + + @abstractmethod + def get_object_link_AABB(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of the link. The return of this method are two points in + world coordinate frame which define a bounding box. + + :param obj: The object for which the bounding box should be returned. + :param link_name: The name of a link of this object. + :return: Two lists of x,y,z which define the bounding box. + """ + pass def get_attachment_event(self) -> Event: """ @@ -561,102 +662,156 @@ def get_manipulation_event(self) -> Event: """ return self.manipulation_event + @abstractmethod def set_realtime(self, real_time: bool) -> None: """ - Enables the real time simulation of Physic in the BulletWorld. By default this is disabled and Physic is only + Enables the real time simulation of Physic in the BulletWorld. By default, this is disabled and Physics is only simulated to reason about it. - :param real_time: Whether the BulletWorld should simulate Physic in real time. + :param real_time: Whether the World should simulate Physics in real time. """ - p.setRealTimeSimulation(1 if real_time else 0, self.client_id) + pass - def set_gravity(self, velocity: List[float]) -> None: + @abstractmethod + def set_gravity(self, gravity_vector: List[float]) -> None: """ - Sets the gravity that is used in the BullteWorld, by default the is the gravity on earth ([0, 0, -9.8]). Gravity - is given as a vector in x,y,z. Gravity is only applied while simulating Physic. + Sets the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). + Gravity is given as a vector in x,y,z. Gravity is only applied while simulating Physic. - :param velocity: The gravity vector that should be used in the BulletWorld. + :param gravity_vector: The gravity vector that should be used in the World. """ - p.setGravity(velocity[0], velocity[1], velocity[2], physicsClientId=self.client_id) + pass - def set_robot(self, robot: Object) -> None: + @classmethod + @abstractmethod + def set_robot(cls, robot: Object) -> None: """ Sets the global variable for the robot Object. This should be set on spawning the robot. :param robot: The Object reference to the Object representing the robot. """ - BulletWorld.robot = robot + pass - def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: + def exit(self, wait_time_before_exit_in_secs: Optional[float] = None) -> None: + """ + Closes the World as well as the shadow world, also collects any other thread that is running. """ - Simulates Physic in the BulletWorld for a given amount of seconds. Usually this simulation is faster than real - time, meaning you can simulate for example 10 seconds of Physic in the BulletWorld in 1 second real time. By - setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real - time. + if wait_time_before_exit_in_secs is not None: + time.sleep(wait_time_before_exit_in_secs) + self.exit_prospection_world_if_exists() + self.disconnect_from_physics_server() + self.reset_current_world() + self.reset_robot() + self.join_threads() - :param seconds: The amount of seconds that should be simulated. - :param real_time: If the simulation should happen in real time or faster. + def exit_prospection_world_if_exists(self): + if self.prospection_world: + self.terminate_world_sync() + self.prospection_world.exit() + + @abstractmethod + def disconnect_from_physics_server(self): """ - for i in range(0, int(seconds * 240)): - p.stepSimulation(self.client_id) - for objects, callback in self.coll_callbacks.items(): - contact_points = p.getContactPoints(objects[0].id, objects[1].id, physicsClientId=self.client_id) - # contact_points = p.getClosestPoints(objects[0].id, objects[1].id, 0.02) - # print(contact_points[0][5]) - if contact_points != (): - callback[0]() - elif callback[1] != None: # Call no collision callback - callback[1]() - if real_time: - # Simulation runs at 240 Hz - time.sleep(0.004167) + Disconnects the world from the physics server. + """ + pass + + def reset_current_world(self): + if self.current_world == self: + self.current_world = None - def exit(self) -> None: + def reset_robot(self): + self.set_robot(None) + + @abstractmethod + def join_threads(self): """ - Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the - preferred method to close the BulletWorld. + Join any running threads. Useful for example when exiting the world. """ - # True if this is NOT the shadow world since it has a reference to the - # Shadow world - time.sleep(0.1) - if self.prospection_world: - self.world_sync.terminate = True - self.world_sync.join() - self.prospection_world.exit() - p.disconnect(self.client_id) - if self._gui_thread: - self._gui_thread.join() - if BulletWorld.current_bullet_world == self: - BulletWorld.current_bullet_world = None - BulletWorld.robot = None + pass - def save_state(self) -> Tuple[int, Dict]: + def terminate_world_sync(self): + self.world_sync.terminate = True + self.world_sync.join() + + def save_state(self) -> int: """ - Returns the id of the saved state of the BulletWorld. The saved state contains the position, orientation and joint - position of every Object in the BulletWorld. + Returns the id of the saved state of the World. The saved state contains the position, orientation and joint + position of every Object in the World, the objects attachments and the constraint ids. :return: A unique id of the state """ - objects2attached = {} - # ToDo find out what this is for and where it is used + state_id = self.save_physics_simulator_state() + self._saved_states[state_id] = WorldState(state_id, + self.get_objects_attachments(), + self.get_objects_constraint_ids()) + return state_id + + def restore_state(self, state_id) -> None: + """ + Restores the state of the World according to the given state using the unique state id. This includes position, + orientation, and joint states. However, restore can not respawn objects if there are objects that were deleted + between creation of the state and restoring, they will be skipped. + + :param state_id: The unique id representing the state, as returned by :func:`~save_state` + """ + self.restore_physics_simulator_state(state_id) + self.restore_attachments_and_constraints_from_saved_world_state(state_id) + + @abstractmethod + def save_physics_simulator_state(self) -> int: + """ + Saves the state of the physics simulator and returns the unique id of the state. + """ + pass + + def get_objects_attachments(self) -> Dict[Object, Dict[Object, Attachment]]: + """ + Get The attachments collections that is stored in each object. + """ + attachments = {} for o in self.objects: - objects2attached[o] = (o.attachments.copy(), o.cids.copy()) - return p.saveState(self.client_id), objects2attached + attachments[o] = o.attachments.copy() + return attachments - def restore_state(self, state, objects2attached: Optional[Dict] = None) -> None: + def get_objects_constraint_ids(self) -> Dict[Object, Dict[Object, int]]: """ - Restores the state of the BulletWorld according to the given state id. This includes position, orientation and - joint states. However, restore can not respawn objects if there are objects that were deleted between creation of - the state and restoring they will be skiped. + Get the constraint ids collection that is stored in each object. + """ + constraint_ids = {} + for o in self.objects: + constraint_ids[o] = o.cids.copy() + return constraint_ids - :param state: The unique id representing the state, as returned by :func:`~save_state` - :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~world.Object.attachments` + @abstractmethod + def restore_physics_simulator_state(self, state_id): """ - p.restoreState(state, physicsClientId=self.client_id) - objects2attached = {} if objects2attached is None else objects2attached + Restores the objects and environment state in the physics simulator according to + the given state using the unique state id. + """ + pass + + def restore_attachments_and_constraints_from_saved_world_state(self, state_id: int): + """ + Restores the attachments and constraints of the objects in the World. This is done by setting the attachments, + and the cids attributes of each object in the World to the given attachments and constraint_ids. + """ + self.restore_attachments_from_saved_world_state(state_id) + self.restore_constraints_from_saved_world_state(state_id) + + def restore_attachments_from_saved_world_state(self, state_id: int): + attachments = self._saved_states[state_id].attachments for obj in self.objects: try: - obj.attachments, obj.cids = objects2attached[obj] + obj.attachments = attachments[obj] + except KeyError: + continue + + def restore_constraints_from_saved_world_state(self, state_id: int): + constraint_ids = self._saved_states[state_id].constraint_ids + for obj in self.objects: + try: + obj.cids = constraint_ids[obj] except KeyError: continue @@ -799,6 +954,30 @@ def update_transforms_for_objects_in_current_world(self) -> None: class BulletWorld(World): + """ + This class represents a BulletWorld, which is a simulation environment that uses the Bullet Physics Engine. This + class is the main interface to the Bullet Physics Engine and should be used to spawn Objects, simulate Physic and + manipulate the Bullet World. + """ + + current_world: World = None + """ + Global reference to the currently used World, usually this is the + graphical one. However, if you are inside a Use_shadow_world() environment the current_world points to the + shadow world. In this way you can comfortably use the current_world, which should point towards the World + used at the moment. + """ + + robot: Object = None + """ + Global reference to the spawned Object that represents the robot. The robot is identified by checking the name + in the URDF with the name of the URDF on the parameter server. + """ + + data_directory: List[str] = {os.path.dirname(__file__) + "/../../resources"} + """ + Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. + """ # Check is for sphinx autoAPI to be able to work in a CI workflow if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): @@ -813,7 +992,7 @@ def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. """ - super().__init__(mode=mode, is_prospection_world=is_prospection_world) + super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_time_step=0.004167) # 240 Hz self._gui_thread: Gui = Gui(self, mode) self._gui_thread.start() @@ -837,33 +1016,6 @@ def load_urdf_at_pose_and_get_object_id(self, path: str, pose: Pose) -> int: basePosition=pose.position_as_list(), baseOrientation=pose.orientation_as_list(), physicsClientId=self.client_id) - def get_objects_by_name(self, name: str) -> List[Object]: - """ - Returns a list of all Objects in this BulletWorld with the same name as the given one. - - :param name: The name of the returned Objects. - :return: A list of all Objects with the name 'name'. - """ - return list(filter(lambda obj: obj.name == name, self.objects)) - - def get_objects_by_type(self, obj_type: str) -> List[Object]: - """ - Returns a list of all Objects which have the type 'obj_type'. - - :param obj_type: The type of the returned Objects. - :return: A list of all Objects that have the type 'obj_type'. - """ - return list(filter(lambda obj: obj.type == obj_type, self.objects)) - - def get_object_by_id(self, id: int) -> Object: - """ - Returns the single Object that has the unique id. - - :param id: The unique id for which the Object should be returned. - :return: The Object with the id 'id'. - """ - return list(filter(lambda obj: obj.id == id, self.objects))[0] - def remove_object(self, obj_id: int) -> None: """ Remove an object by its ID. @@ -893,35 +1045,25 @@ def add_constraint(self, constraint: Constraint) -> int: def remove_constraint(self, constraint_id): p.removeConstraint(constraint_id, physicsClientId=self.client_id) - def detach_objects(self, obj1: Object, obj2: Object) -> None: + def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: """ - Detaches obj2 from obj1. This is done by - deleting the attachment from the attachments dictionary of both objects - and deleting the constraint of pybullet. - Afterward the detachment event of the corresponding BulletWorld will be fired. + Get the joint upper limit of an articulated object - :param obj1: The object from which an object should be detached. - :param obj2: The object which should be detached. + :param obj: The object. + :param joint_name: The name of the joint. + :return: The joint upper limit as a float. """ - del obj1.attachments[obj2] - del obj2.attachments[obj1] - - p.removeConstraint(obj1.cids[obj2], physicsClientId=self.client_id) + return p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8] - del obj1.cids[obj2] - del obj2.cids[obj1] - obj1.world.detachment_event(obj1, [obj1, obj2]) - - def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: + def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: """ - Get the joint limits of an articulated object + Get the joint lower limit of an articulated object - :param obj: The object - :param joint_name: The name of the joint - :return: A tuple containing the upper and the lower limits of the joint. + :param obj: The object. + :param joint_name: The name of the joint. + :return: The joint lower limit as a float. """ - up_lim, low_lim = p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8:10] - return up_lim, low_lim + return p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[9] def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: """ @@ -973,6 +1115,16 @@ def get_object_contact_points(self, obj: Object) -> List: """ return p.getContactPoints(obj.id) + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + """ + Returns a list of contact points between obj1 and obj2. + + :param obj1: The first object. + :param obj2: The second object. + :return: A list of all contact points between the two objects. + """ + return p.getContactPoints(obj1.id, obj2.id) + def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: """ Get the ID of a joint in an articulated object. @@ -1035,205 +1187,130 @@ def step(self): """ p.stepSimulation(self.client_id) - def set_object_color(self, obj: Object, color: List[float], link: Optional[str] = ""): + def set_object_link_color(self, obj: Object, link_id: int, rgba_color: List[float]): """ - Changes the color of this object, the color has to be given as a list - of RGBA values. Optionally a link name can can be provided, if no link - name is provided all links of this object will be colored. + Changes the color of a link of this object, the color has to be given as a 4 element list + of RGBA values. :param obj: The object which should be colored - :param color: The color as RGBA values between 0 and 1 - :param link: The link name of the link which should be colored + :param link_id: The link id of the link which should be colored + :param rgba_color: The color as RGBA values between 0 and 1 """ - if link == "": - # Check if there is only one link, this is the case for primitive - # forms or if loaded from an .stl or .obj file - if obj.links != {}: - for link_id in obj.links.values(): - p.changeVisualShape(obj.id, link_id, rgbaColor=color, physicsClientId=self.client_id) - else: - p.changeVisualShape(obj.id, -1, rgbaColor=color, physicsClientId=self.client_id) - else: - p.changeVisualShape(obj.id, obj.links[link], rgbaColor=color, physicsClientId=self.client_id) + p.changeVisualShape(obj.id, link_id, rgbaColor=rgba_color, physicsClientId=self.client_id) - def get_object_color(self, - obj: Object, - link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: + def get_object_colors(self, obj: Object) -> Dict[str, List[float]]: """ - This method returns the color of this object or a link of this object. If no link is given then the - return is either: - - 1. A list with the color as RGBA values, this is the case if the object only has one link (this - happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the color as value. The color is given as RGBA values. - Please keep in mind that not every link may have a color. This is dependent on the URDF from which the - object is spawned. - - If a link is specified then the return is a list with RGBA values representing the color of this link. - It may be that this link has no color, in this case the return is None as well as an error message. + Get the RGBA colors of each link in the object as a dictionary from link name to color. - :param obj: The object for which the color should be returned. - :param link: the link name for which the color should be returned. - :return: The color of the object or link, or a dictionary containing every colored link with its color + :param obj: The object + :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. """ visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) swap = {v: k for k, v in obj.links.items()} links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) colors = list(map(lambda x: x[7], visual_data)) link_to_color = dict(zip(links, colors)) + return link_to_color - if link: - if link in link_to_color.keys(): - return link_to_color[link] - elif link not in obj.links.keys(): - rospy.logerr(f"The link '{link}' is not part of this obejct") - return None - else: - rospy.logerr(f"The link '{link}' has no color") - return None - - if len(visual_data) == 1: - return list(visual_data[0][7]) - else: - return link_to_color - - def get_object_AABB(self, obj: Object, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: + def get_object_AABB(self, obj: Object) -> Tuple[List[float], List[float]]: """ - Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case - the axis aligned bounding box of the link will be returned. The return of this method are two points in + Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. :param obj: The object for which the bounding box should be returned. - :param link_name: The Optional name of a link of this object. :return: Two lists of x,y,z which define the bounding box. """ - if link_name: - return p.getAABB(obj.id, obj.links[link_name], self.client_id) - else: - return p.getAABB(obj.id, physicsClientId=self.client_id) - - def get_attachment_event(self) -> Event: - """ - Returns the event reference that is fired if an attachment occurs. - - :return: The reference to the attachment event - """ - return self.attachment_event - - def get_detachment_event(self) -> Event: - """ - Returns the event reference that is fired if a detachment occurs. + return p.getAABB(obj.id, physicsClientId=self.client_id) - :return: The event reference for the detachment event. + def get_object_link_AABB(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: """ - return self.detachment_event - - def get_manipulation_event(self) -> Event: - """ - Returns the event reference that is fired if any manipulation occurs. + Returns the axis aligned bounding box of the link. The return of this method are two points in + world coordinate frame which define a bounding box. - :return: The event reference for the manipulation event. + :param obj: The object for which the bounding box should be returned. + :param link_name: The name of a link of this object. + :return: Two lists of x,y,z which define the bounding box. """ - return self.manipulation_event + return p.getAABB(obj.id, obj.links[link_name], self.client_id) def set_realtime(self, real_time: bool) -> None: """ - Enables the real time simulation of Physic in the BulletWorld. By default this is disabled and Physic is only + Enables the real time simulation of Physic in the BulletWorld. By default, this is disabled and Physics is only simulated to reason about it. - :param real_time: Whether the BulletWorld should simulate Physic in real time. + :param real_time: Whether the World should simulate Physics in real time. """ p.setRealTimeSimulation(1 if real_time else 0, self.client_id) - def set_gravity(self, velocity: List[float]) -> None: + def set_gravity(self, gravity_vector: List[float]) -> None: """ - Sets the gravity that is used in the BullteWorld, by default the is the gravity on earth ([0, 0, -9.8]). Gravity - is given as a vector in x,y,z. Gravity is only applied while simulating Physic. + Sets the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). + Gravity is given as a vector in x,y,z. Gravity is only applied while simulating Physic. - :param velocity: The gravity vector that should be used in the BulletWorld. + :param gravity_vector: The gravity vector that should be used in the World. """ - p.setGravity(velocity[0], velocity[1], velocity[2], physicsClientId=self.client_id) + p.setGravity(gravity_vector[0], gravity_vector[1], gravity_vector[2], physicsClientId=self.client_id) - def set_robot(self, robot: Object) -> None: + @classmethod + def set_robot(cls, robot: Object) -> None: """ Sets the global variable for the robot Object. This should be set on spawning the robot. :param robot: The Object reference to the Object representing the robot. """ - BulletWorld.robot = robot - - def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: - """ - Simulates Physic in the BulletWorld for a given amount of seconds. Usually this simulation is faster than real - time, meaning you can simulate for example 10 seconds of Physic in the BulletWorld in 1 second real time. By - setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real - time. - - :param seconds: The amount of seconds that should be simulated. - :param real_time: If the simulation should happen in real time or faster. - """ - for i in range(0, int(seconds * 240)): - p.stepSimulation(self.client_id) - for objects, callback in self.coll_callbacks.items(): - contact_points = p.getContactPoints(objects[0].id, objects[1].id, physicsClientId=self.client_id) - # contact_points = p.getClosestPoints(objects[0].id, objects[1].id, 0.02) - # print(contact_points[0][5]) - if contact_points != (): - callback[0]() - elif callback[1] != None: # Call no collision callback - callback[1]() - if real_time: - # Simulation runs at 240 Hz - time.sleep(0.004167) + cls.robot = robot - def exit(self) -> None: + def exit(self, wait_time_before_exit_in_secs: Optional[float] = 0.1) -> None: """ Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the preferred method to close the BulletWorld. """ - # True if this is NOT the shadow world since it has a reference to the - # Shadow world - time.sleep(0.1) + super().exit(wait_time_before_exit_in_secs) + + def exit_prospection_world_if_exists(self): if self.prospection_world: - self.world_sync.terminate = True - self.world_sync.join() + self.terminate_world_sync() self.prospection_world.exit() + + def disconnect_from_physics_server(self): + """ + Disconnects the world from the physics server. + """ p.disconnect(self.client_id) + + def reset_current_world(self): + if self.current_world == self: + self.current_world = None + + def reset_robot(self): + self.set_robot(None) + + def join_threads(self): + """ + Join any running threads. Useful for example when exiting the world. + """ + self.join_gui_thread_if_exists() + + def join_gui_thread_if_exists(self): if self._gui_thread: self._gui_thread.join() - if BulletWorld.current_bullet_world == self: - BulletWorld.current_bullet_world = None - BulletWorld.robot = None - def save_state(self) -> Tuple[int, Dict]: - """ - Returns the id of the saved state of the BulletWorld. The saved state contains the position, orientation and joint - position of every Object in the BulletWorld. + def terminate_world_sync(self): + self.world_sync.terminate = True + self.world_sync.join() - :return: A unique id of the state + def save_physics_simulator_state(self) -> int: """ - objects2attached = {} - # ToDo find out what this is for and where it is used - for o in self.objects: - objects2attached[o] = (o.attachments.copy(), o.cids.copy()) - return p.saveState(self.client_id), objects2attached - - def restore_state(self, state, objects2attached: Optional[Dict] = None) -> None: + Saves the state of the physics simulator and returns the unique id of the state. """ - Restores the state of the BulletWorld according to the given state id. This includes position, orientation and - joint states. However, restore can not respawn objects if there are objects that were deleted between creation of - the state and restoring they will be skiped. + return p.saveState(self.client_id) - :param state: The unique id representing the state, as returned by :func:`~save_state` - :param objects2attached: A dictionary of attachments, as saved in :py:attr:`~world.Object.attachments` + def restore_physics_simulator_state(self, state_id): """ - p.restoreState(state, physicsClientId=self.client_id) - objects2attached = {} if objects2attached is None else objects2attached - for obj in self.objects: - try: - obj.attachments, obj.cids = objects2attached[obj] - except KeyError: - continue + Restores the objects and environment state in the physics simulator according to + the given state using the unique state id. + """ + p.restoreState(state_id, self.client_id) def copy(self) -> BulletWorld: """ @@ -2192,16 +2269,24 @@ def get_color(self, link: Optional[str] = None) -> Union[List[float], Dict[str, """ return self.world.get_object_color(self, link) - def get_AABB(self, link_name: Optional[str] = None) -> Tuple[List[float], List[float]]: + def get_AABB(self) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of this object. The return of this method are two points in + world coordinate frame which define a bounding box. + + :return: Two lists of x,y,z which define the bounding box. + """ + return self.world.get_object_AABB(self) + + def get_link_AABB(self, link_name: str) -> Tuple[List[float], List[float]]: """ - Returns the axis aligned bounding box of this object, optionally a link name can be provided in this case - the axis aligned bounding box of the link will be returned. The return of this method are two points in + Returns the axis aligned bounding box of the given link name. The return of this method are two points in world coordinate frame which define a bounding box. - :param link_name: The Optional name of a link of this object. + :param link_name: The name of a link of this object. :return: Two lists of x,y,z which define the bounding box. """ - return self.world.get_object_AABB(self, link_name) + return self.world.get_object_link_AABB(self, link_name) def get_base_origin(self, link_name: Optional[str] = None) -> Pose: """ @@ -2210,7 +2295,7 @@ def get_base_origin(self, link_name: Optional[str] = None) -> Pose: :param link_name: The link name for which the bottom position should be returned :return: The position of the bottom of this Object or link """ - aabb = self.get_AABB(link_name=link_name) + aabb = self.get_link_AABB(link_name=link_name) base_width = np.absolute(aabb[0][0] - aabb[1][0]) base_length = np.absolute(aabb[0][1] - aabb[1][1]) return Pose([aabb[0][0] + base_width / 2, aabb[0][1] + base_length / 2, aabb[0][2]], From 0f1133ba0d3363c044caee28096bb59bffb924da Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Dec 2023 01:22:09 +0100 Subject: [PATCH 12/69] [AbstractWorld] Finished defining AbstractWorld, and correcting all classes defined in the world.py file. --- src/pycram/costmaps.py | 32 +- src/pycram/designator.py | 2 +- src/pycram/designators/action_designator.py | 8 +- src/pycram/designators/location_designator.py | 14 +- src/pycram/external_interfaces/giskard.py | 6 +- src/pycram/external_interfaces/robokudo.py | 2 +- src/pycram/pose_generator_and_validator.py | 6 +- .../process_modules/boxy_process_modules.py | 4 +- .../process_modules/donbot_process_modules.py | 4 +- .../process_modules/hsr_process_modules.py | 4 +- .../process_modules/pr2_process_modules.py | 6 +- .../resolver/location/giskard_location.py | 6 +- src/pycram/ros/force_torque_sensor.py | 2 +- src/pycram/ros/joint_state_publisher.py | 2 +- src/pycram/ros/tf_broadcaster.py | 2 +- src/pycram/ros/viz_marker_publisher.py | 2 +- src/pycram/task.py | 4 +- src/pycram/world.py | 336 ++++++------------ src/pycram/world_reasoning.py | 70 ++-- test/test_bullet_world.py | 4 +- test/test_database_resolver.py | 2 +- test/test_jpt_resolver.py | 2 +- test/test_task_tree.py | 2 +- 23 files changed, 196 insertions(+), 326 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 3d6f63451..86017cdbd 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -8,7 +8,7 @@ from matplotlib import colors import psutil import time -from .world import BulletWorld, Use_shadow_world, Object +from .world import BulletWorld, UseProspectionWorld, Object from .world_reasoning import _get_images_for_target from nav_msgs.msg import OccupancyGrid, MapMetaData from typing import Tuple, List, Union, Optional @@ -385,17 +385,17 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: i = 0 j = 0 for n in self._chunks(np.array(rays), 16380): - with Use_shadow_world(): + with UseProspectionWorld(BulletWorld): r_t = p.rayTestBatch(n[:, 0], n[:, 1], numThreads=0, - physicsClientId=BulletWorld.current_bullet_world.client_id) + physicsClientId=BulletWorld.current_world.client_id) while r_t is None: r_t = p.rayTestBatch(n[:, 0], n[:, 1], numThreads=0, - physicsClientId=BulletWorld.current_bullet_world.client_id) + physicsClientId=BulletWorld.current_world.client_id) j += len(n) if BulletWorld.robot: - shadow_robot = BulletWorld.current_bullet_world.get_shadow_object(BulletWorld.robot) + shadow_robot = BulletWorld.current_world.get_prospection_object(BulletWorld.robot) attached_objs = BulletWorld.robot.attachments.keys() - attached_objs_shadow_id = [BulletWorld.current_bullet_world.get_shadow_object(x).id for x in + attached_objs_shadow_id = [BulletWorld.current_world.get_prospection_object(x).id for x in attached_objs] res[i:j] = [ 1 if ray[0] == -1 or ray[0] == shadow_robot.id or ray[0] in attached_objs_shadow_id else 0 for @@ -470,7 +470,7 @@ def __init__(self, min_height: float, if (11 * size ** 2 + size ** 3) * 2 > psutil.virtual_memory().available: raise OSError("Not enough free RAM to calculate a costmap of this size") - self.world = world if world else BulletWorld.current_bullet_world + self.world = world if world else BulletWorld.current_world self.map = np.zeros((size, size)) self.size = size self.resolution = resolution @@ -494,26 +494,26 @@ def _create_images(self) -> List[np.ndarray]: images = [] camera_pose = self.origin - with Use_shadow_world(): + with UseProspectionWorld(BulletWorld): origin_copy = self.origin.copy() origin_copy.position.y += 1 images.append( - _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_bullet_world, size=self.size)[1]) + _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_world, size=self.size)[1]) origin_copy = self.origin.copy() origin_copy.position.x -= 1 images.append( - _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_bullet_world, size=self.size)[1]) + _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_world, size=self.size)[1]) origin_copy = self.origin.copy() origin_copy.position.y -= 1 images.append( - _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_bullet_world, size=self.size)[1]) + _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_world, size=self.size)[1]) origin_copy = self.origin.copy() origin_copy.position.x += 1 images.append( - _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_bullet_world, size=self.size)[1]) + _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_world, size=self.size)[1]) for i in range(0, 4): images[i] = self._depth_buffer_to_meter(images[i]) @@ -695,7 +695,7 @@ def __init__(self, object, urdf_link_name, size=100, resolution=0.02, world=None :param resolution: Resolution of the final costmap :param world: The BulletWorld from which the costmap should be created """ - self.world: BulletWorld = world if world else BulletWorld.current_bullet_world + self.world: BulletWorld = world if world else BulletWorld.current_world self.object: Object = object self.link: str = urdf_link_name self.resolution: float = resolution @@ -725,14 +725,14 @@ def get_aabb_for_link(self) -> Tuple[List[float], List[float]]: :return: Two points in world coordinate space, which span a rectangle """ - shadow_obj = BulletWorld.current_bullet_world.get_shadow_object(self.object) - with Use_shadow_world(): + shadow_obj = BulletWorld.current_world.get_prospection_object(self.object) + with UseProspectionWorld(BulletWorld): shadow_obj.set_orientation(Pose(orientation=[0, 0, 0, 1])) link_orientation = shadow_obj.get_link_pose(self.link) link_orientation_trans = link_orientation.to_transform(self.object.get_link_tf_frame(self.link)) inverse_orientation = link_orientation_trans.invert() shadow_obj.set_orientation(inverse_orientation.to_pose()) - return shadow_obj.get_link_AABB(self.link) + return shadow_obj.get_link_aabb(self.link) cmap = colors.ListedColormap(['white', 'black', 'green', 'red', 'blue']) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 289ebe498..3329fba27 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -753,7 +753,7 @@ def __iter__(self) -> Iterable[Object]: :yield: A resolved object designator """ # for every bullet world object - for obj in BulletWorld.current_bullet_world.objects: + for obj in BulletWorld.current_world.objects: # skip if name does not match specification if self.names and obj.name not in self.names: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 2e7e4f2db..b92363bf1 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -345,12 +345,12 @@ def perform(self) -> None: prepose = object.local_transformer.transform_pose_to_target_frame(oTg, "map") # Perform the motion with the prepose and open gripper - BulletWorld.current_bullet_world.add_vis_axis(prepose) + BulletWorld.current_world.add_vis_axis(prepose) MoveTCPMotion(prepose, self.arm).resolve().perform() MoveGripperMotion(motion="open", gripper=self.arm).resolve().perform() # Perform the motion with the adjusted pose -> actual grasp and close gripper - BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) + BulletWorld.current_world.add_vis_axis(adjusted_oTm) MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() adjusted_oTm.pose.position.z += 0.03 MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform() @@ -358,11 +358,11 @@ def perform(self) -> None: robot.attach(object, tool_frame) # Lift object - BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) + BulletWorld.current_world.add_vis_axis(adjusted_oTm) MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() # Remove the vis axis from the world - BulletWorld.current_bullet_world.remove_vis_axis() + BulletWorld.current_world.remove_vis_axis() def to_sql(self) -> ORMPickUpAction: return ORMPickUpAction(self.arm, self.grasp) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 7b62fb27c..99c2a4b0e 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -3,7 +3,7 @@ from typing import List, Tuple, Union, Iterable, Optional, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart -from ..world import Object, BulletWorld, Use_shadow_world +from ..world import Object, BulletWorld, UseProspectionWorld from ..world_reasoning import link_pose_for_joint_config from ..designator import Designator, DesignatorError, LocationDesignatorDescription from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap @@ -180,16 +180,16 @@ def __iter__(self): if self.visible_for or self.reachable_for: robot_object = self.visible_for.bullet_world_object if self.visible_for else self.reachable_for.bullet_world_object - test_robot = BulletWorld.current_bullet_world.get_shadow_object(robot_object) + test_robot = BulletWorld.current_world.get_prospection_object(robot_object) - with Use_shadow_world(): + with UseProspectionWorld(BulletWorld): for maybe_pose in pose_generator(final_map, number_of_samples=600): res = True arms = None if self.visible_for: res = res and visibility_validator(maybe_pose, test_robot, target_pose, - BulletWorld.current_bullet_world) + BulletWorld.current_world) if self.reachable_for: hand_links = [] for name, chain in robot_description.chains.items(): @@ -256,7 +256,7 @@ def __iter__(self) -> Location: final_map = occupancy + gaussian - test_robot = BulletWorld.current_bullet_world.get_shadow_object(self.robot) + test_robot = BulletWorld.current_world.get_prospection_object(self.robot) # Find a Joint of type prismatic which is above the handle in the URDF tree container_joint = self.handle.bullet_world_object.find_joint_above(self.handle.name, JointType.PRISMATIC) @@ -275,7 +275,7 @@ def __iter__(self) -> Location: container_joint: self.handle.bullet_world_object.get_joint_limits(container_joint)[1] / 1.5}, self.handle.name) - with Use_shadow_world(): + with UseProspectionWorld(BulletWorld): for maybe_pose in pose_generator(final_map, number_of_samples=600, orientation_generator=lambda p, o: generate_orientation(p, half_pose)): @@ -338,7 +338,7 @@ def __iter__(self): sem_costmap = SemanticCostmap(self.part_of.bullet_world_object, self.urdf_link_name) height_offset = 0 if self.for_object: - min, max = self.for_object.bullet_world_object.get_AABB() + min, max = self.for_object.bullet_world_object.get_aabb() height_offset = (max[2] - min[2]) / 2 for maybe_pose in pose_generator(sem_costmap): maybe_pose.position.z += height_offset diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 98f15d55a..7c4c5f4c6 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -69,7 +69,7 @@ def initial_adding_objects() -> None: Adds object that are loaded in the BulletWorld to the Giskard belief state, if they are not present at the moment. """ groups = giskard_wrapper.get_group_names() - for obj in BulletWorld.current_bullet_world.objects: + for obj in BulletWorld.current_world.objects: if obj == BulletWorld.robot: continue name = obj.name + "_" + str(obj.id) @@ -84,7 +84,7 @@ def removing_of_objects() -> None: """ groups = giskard_wrapper.get_group_names() object_names = list( - map(lambda obj: object_names.name + "_" + str(obj.id), BulletWorld.current_bullet_world.objects)) + map(lambda obj: object_names.name + "_" + str(obj.id), BulletWorld.current_world.objects)) diff = list(set(groups) - set(object_names)) for grp in diff: giskard_wrapper.remove_group(grp) @@ -99,7 +99,7 @@ def sync_worlds() -> None: """ add_gripper_groups() bullet_object_names = set() - for obj in BulletWorld.current_bullet_world.objects: + for obj in BulletWorld.current_world.objects: if obj.name != robot_description.name and len(obj.links) != 1: bullet_object_names.add(obj.name + "_" + str(obj.id)) diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index d092dbe2a..c6e897af8 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -125,7 +125,7 @@ def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescripti for i in range(0, len(query_result.res[0].pose)): pose = Pose.from_pose_stamped(query_result.res[0].pose[i]) - pose.frame = BulletWorld.current_bullet_world.robot.get_link_tf_frame(pose.frame) + pose.frame = BulletWorld.current_world.robot.get_link_tf_frame(pose.frame) source = query_result.res[0].poseSource[i] lt = LocalTransformer() diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index a836034d1..ae3f9994d 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -3,7 +3,7 @@ import rospy import pybullet as p -from .world import Object, BulletWorld, Use_shadow_world +from .world import Object, BulletWorld, UseProspectionWorld from .world_reasoning import contact from .costmaps import Costmap from .pose import Pose, Transform @@ -157,7 +157,7 @@ def reachability_validator(pose: Pose, _apply_ik(robot, resp, left_joints) - for obj in BulletWorld.current_bullet_world.objects: + for obj in BulletWorld.current_world.objects: if obj.name == "floor": continue in_contact, contact_links = contact(robot, obj, return_links=True) @@ -181,7 +181,7 @@ def reachability_validator(pose: Pose, _apply_ik(robot, resp, right_joints) - for obj in BulletWorld.current_bullet_world.objects: + for obj in BulletWorld.current_world.objects: if obj.name == "floor": continue in_contact, contact_links = contact(robot, obj, return_links=True) diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 45a163e84..e1314e41c 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -225,7 +225,7 @@ def _execute(self, desig): cam_frame_name = solution['cam_frame'] front_facing_axis = solution['front_facing_axis'] - objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type) + objects = BulletWorld.current_world.get_objects_by_type(object_type) for obj in objects: if btr.visible(obj, robot.get_link_position_and_orientation(cam_frame_name), front_facing_axis, 0.5): return obj @@ -283,7 +283,7 @@ def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "world-state-detecting": obj_type = solution['object_type'] - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] + return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_world.objects))[0] class BoxyManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 81e12caa1..d893d8048 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -188,7 +188,7 @@ def _execute(self, desig): cam_frame_name = solution['cam_frame'] front_facing_axis = solution['front_facing_axis'] - objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type) + objects = BulletWorld.current_world.get_objects_by_type(object_type) for obj in objects: if btr.visible(obj, robot.get_link_position_and_orientation(cam_frame_name), front_facing_axis, 0.5): return obj @@ -240,7 +240,7 @@ def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "world-state-detecting": obj_type = solution['object_type'] - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] + return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_world.objects))[0] class DonbotManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 594f355f1..09b715e1c 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -161,7 +161,7 @@ def _execute(self, desig): cam_frame_name = solution['cam_frame'] front_facing_axis = solution['front_facing_axis'] - objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type) + objects = BulletWorld.current_world.get_objects_by_type(object_type) for obj in objects: if btr.visible(obj, robot.get_link_position_and_orientation(cam_frame_name), front_facing_axis, 0.5): return obj @@ -213,7 +213,7 @@ def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "world-state-detecting": obj_type = solution['object_type'] - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] + return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_world.objects))[0] class HSRManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index c446b1463..3432533d0 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -156,7 +156,7 @@ def _execute(self, desig: DetectingMotion.Motion): # should be [0, 0, 1] front_facing_axis = robot_description.front_facing_axis - objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type) + objects = BulletWorld.current_world.get_objects_by_type(object_type) for obj in objects: if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): return obj @@ -205,7 +205,7 @@ class Pr2WorldStateDetecting(ProcessModule): def _execute(self, desig: WorldStateDetectingMotion.Motion): obj_type = desig.object_type - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] + return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_world.objects))[0] class Pr2Open(ProcessModule): @@ -325,7 +325,7 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: obj_pose.orientation = [0, 0, 0, 1] obj_pose.position.x += 0.05 - bullet_obj = BulletWorld.current_bullet_world.get_objects_by_type(designator.object_type) + bullet_obj = BulletWorld.current_world.get_objects_by_type(designator.object_type) if bullet_obj: bullet_obj[0].set_pose(obj_pose) return bullet_obj[0] diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index cd2c76430..2a2840f36 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -1,6 +1,6 @@ from pycram.external_interfaces.giskard import achieve_cartesian_goal from pycram.designators.location_designator import CostmapLocation -from pycram.world import Use_shadow_world, BulletWorld +from pycram.world import UseProspectionWorld, BulletWorld from pycram.helper import _apply_ik from pycram.pose import Pose from pycram.robot_descriptions import robot_description @@ -28,8 +28,8 @@ def __iter__(self) -> CostmapLocation.Location: pose_left, end_config_left = self._get_reachable_pose_for_arm(self.target, robot_description.get_tool_frame("left")) - test_robot = BulletWorld.current_bullet_world.get_shadow_object(BulletWorld.robot) - with Use_shadow_world(): + test_robot = BulletWorld.current_world.get_prospection_object(BulletWorld.robot) + with UseProspectionWorld(BulletWorld): valid, arms = reachability_validator(pose_right, test_robot, self.target, {}) if valid: yield CostmapLocation.Location(pose_right, arms) diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros/force_torque_sensor.py index 314a9239a..c67c2bca2 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros/force_torque_sensor.py @@ -25,7 +25,7 @@ def __init__(self, joint_name, fts_topic="/pycram/fts", interval=0.1): :param fts_topic: Name of the ROS topic to which should be published :param interval: Interval at which the messages should be published, in seconds """ - self.world = BulletWorld.current_bullet_world + self.world = BulletWorld.current_world self.fts_joint_idx = None self.joint_name = joint_name if joint_name in self.world.robot.joints.keys(): diff --git a/src/pycram/ros/joint_state_publisher.py b/src/pycram/ros/joint_state_publisher.py index f771b5565..4f72e3a61 100644 --- a/src/pycram/ros/joint_state_publisher.py +++ b/src/pycram/ros/joint_state_publisher.py @@ -21,7 +21,7 @@ def __init__(self, joint_state_topic="/pycram/joint_state", interval=0.1): :param joint_state_topic: Topic name to which the joint states should be published :param interval: Interval at which the joint states should be published, in seconds """ - self.world = BulletWorld.current_bullet_world + self.world = BulletWorld.current_world self.joint_state_pub = rospy.Publisher(joint_state_topic, JointState, queue_size=10) self.interval = interval diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index ec0ab9cc3..907119275 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -21,7 +21,7 @@ def __init__(self, projection_namespace="simulated", odom_frame="odom", interval :param odom_frame: Name of the statically published odom frame :param interval: Interval at which the TFs should be published, in seconds """ - self.world = BulletWorld.current_bullet_world + self.world = BulletWorld.current_world self.tf_static_publisher = rospy.Publisher("/tf_static", TFMessage, queue_size=10) self.tf_publisher = rospy.Publisher("/tf", TFMessage, queue_size=10) diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 90278afd4..f4a815df3 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -56,7 +56,7 @@ def _make_marker_array(self) -> MarkerArray: :return: An Array of Visualization Marker """ marker_array = MarkerArray() - for obj in BulletWorld.current_bullet_world.objects: + for obj in BulletWorld.current_world.objects: if obj.name == "floor": continue for link in obj.links.keys(): diff --git a/src/pycram/task.py b/src/pycram/task.py index 74df58952..7ec966918 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -254,7 +254,7 @@ def __enter__(self): def simulation(): return None self.suspended_tree = task_tree - self.world_state, self.objects2attached = BulletWorld.current_bullet_world.save_state() + self.world_state, self.objects2attached = BulletWorld.current_world.save_state() self.simulated_root = TaskTreeNode(code=Code(simulation)) task_tree = self.simulated_root pybullet.addUserDebugText("Simulating...", [0, 0, 1.75], textColorRGB=[0, 0, 0], @@ -267,7 +267,7 @@ def __exit__(self, exc_type, exc_val, exc_tb): """ global task_tree task_tree = self.suspended_tree - BulletWorld.current_bullet_world.restore_state(self.world_state, self.objects2attached) + BulletWorld.current_world.restore_state(self.world_state, self.objects2attached) pybullet.removeAllUserDebugItems() diff --git a/src/pycram/world.py b/src/pycram/world.py index 6e3d75b1c..c78b1eeb5 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -10,7 +10,7 @@ import xml.etree.ElementTree from queue import Queue import tf -from typing import List, Optional, Dict, Tuple, Callable, Set +from typing import List, Optional, Dict, Tuple, Callable, Set, Type, TypeVar from typing import Union import numpy as np @@ -158,6 +158,8 @@ class World(ABC): Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. """ + simulation_time_step: float = None + def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: float): """ Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the @@ -184,7 +186,7 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: self._init_and_sync_prospection_world() self._update_local_transformer_worlds() - self.simulation_time_step = simulation_time_step + World.simulation_time_step = simulation_time_step self.simulation_frequency = int(1/self.simulation_time_step) self._saved_states: Dict[int, WorldState] = {} # Different states of the world indexed by int state id. @@ -815,7 +817,7 @@ def restore_constraints_from_saved_world_state(self, state_id: int): except KeyError: continue - def copy(self) -> BulletWorld: + def _copy(self) -> World: """ Copies this Bullet World into another and returns it. The other BulletWorld will be in Direct mode. The shadow world should always be preferred instead of creating a new BulletWorld. @@ -831,103 +833,66 @@ def copy(self) -> BulletWorld: o.set_joint_position(joint, obj.get_joint_position(joint)) return world - def add_vis_axis(self, pose: Pose, - length: Optional[float] = 0.2) -> None: - """ - Creates a Visual object which represents the coordinate frame at the given - position and orientation. There can be an unlimited amount of vis axis objects. - - :param pose: The pose at which the axis should be spawned - :param length: Optional parameter to configure the length of the axes - """ - - position, orientation = pose.to_list() - - vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], - rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) - vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], - rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) - vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], - rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) - - obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], - basePosition=position, baseOrientation=orientation, - linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkParentIndices=[0, 0, 0], - linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], - linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], - linkCollisionShapeIndices=[-1, -1, -1]) - - self.vis_axis.append(obj) - - def remove_vis_axis(self) -> None: - """ - Removes all spawned vis axis objects that are currently in this BulletWorld. - """ - for id in self.vis_axis: - p.removeBody(id) - self.vis_axis = [] - - def register_collision_callback(self, objectA: Object, objectB: Object, - callback_collision: Callable, - callback_no_collision: Optional[Callable] = None) -> None: + def register_two_objects_collision_callbacks(self, object_a: Object, object_b: Object, + on_collision_callback: Callable, + on_collision_removal_callback: Optional[Callable] = None) -> None: """ Registers callback methods for contact between two Objects. There can be a callback for when the two Objects get in contact and, optionally, for when they are not in contact anymore. - :param objectA: An object in the BulletWorld - :param objectB: Another object in the BulletWorld - :param callback_collision: A function that should be called if the objects are in contact - :param callback_no_collision: A function that should be called if the objects are not in contact + :param object_a: An object in the World + :param object_b: Another object in the World + :param on_collision_callback: A function that should be called if the objects are in contact + :param on_collision_removal_callback: A function that should be called if the objects are not in contact """ - self.coll_callbacks[(objectA, objectB)] = (callback_collision, callback_no_collision) + self.coll_callbacks[(object_a, object_b)] = (on_collision_callback, on_collision_removal_callback) - def add_additional_resource_path(self, path: str) -> None: + @classmethod + def add_additional_resource_path(cls, path: str) -> None: """ Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an Object is spawned only with a filename. :param path: A path in the filesystem in which to search for files. """ - World.data_directory.append(path) + cls.data_directory.append(path) - def get_shadow_object(self, object: Object) -> Object: + def get_prospection_object(self, obj: Object) -> Object: """ - Returns the corresponding object from the shadow world for the given object. If the given Object is already in - the shadow world it is returned. + Returns the corresponding object from the prospection world for a given object in the main world. + If the given Object is already in the prospection world, it is returned. - :param object: The object for which the shadow worlds object should be returned. - :return: The corresponding object in the shadow world. + :param obj: The object for which the corresponding object in the prospection World should be found. + :return: The corresponding object in the prospection world. """ try: - return self.world_sync.object_mapping[object] + return self.world_sync.object_mapping[obj] except KeyError: - shadow_world = self if self.is_prospection_world else self.prospection_world - if object in shadow_world.objects: - return object + prospection_world = self if self.is_prospection_world else self.prospection_world + if obj in prospection_world.objects: + return obj else: raise ValueError( - f"There is no shadow object for the given object: {object}, this could be the case if the object isn't anymore in the main (graphical) BulletWorld or if the given object is already a shadow object. ") + f"There is no prospection object for the given object: {obj}, this could be the case if" + f" the object isn't anymore in the main (graphical) World" + f" or if the given object is already a prospection object. ") - def get_bullet_object_for_shadow(self, object: Object) -> Object: + def get_object_from_prospection(self, prospection_object: Object) -> Object: """ - Returns the corresponding object from the main Bullet World for a given + Returns the corresponding object from the main World for a given object in the shadow world. If the given object is not in the shadow world an error will be raised. - :param object: The object for which the corresponding object in the main Bullet World should be found - :return: The object in the main Bullet World + :param prospection_object: The object for which the corresponding object in the main World should be found. + :return: The object in the main World. """ - map = self.world_sync.object_mapping + object_map = self.world_sync.object_mapping try: - return list(map.keys())[list(map.values()).index(object)] + return list(object_map.keys())[list(object_map.values()).index(prospection_object)] except ValueError: raise ValueError("The given object is not in the shadow world.") - def reset_bullet_world(self) -> None: + def reset_world(self) -> None: """ Resets the BulletWorld to the state it was first spawned in. All attached objects will be detached, all joints will be set to the @@ -960,7 +925,7 @@ class is the main interface to the Bullet Physics Engine and should be used to s manipulate the Bullet World. """ - current_world: World = None + current_world: BulletWorld = None """ Global reference to the currently used World, usually this is the graphical one. However, if you are inside a Use_shadow_world() environment the current_world points to the @@ -1267,24 +1232,12 @@ def exit(self, wait_time_before_exit_in_secs: Optional[float] = 0.1) -> None: """ super().exit(wait_time_before_exit_in_secs) - def exit_prospection_world_if_exists(self): - if self.prospection_world: - self.terminate_world_sync() - self.prospection_world.exit() - def disconnect_from_physics_server(self): """ Disconnects the world from the physics server. """ p.disconnect(self.client_id) - def reset_current_world(self): - if self.current_world == self: - self.current_world = None - - def reset_robot(self): - self.set_robot(None) - def join_threads(self): """ Join any running threads. Useful for example when exiting the world. @@ -1295,10 +1248,6 @@ def join_gui_thread_if_exists(self): if self._gui_thread: self._gui_thread.join() - def terminate_world_sync(self): - self.world_sync.terminate = True - self.world_sync.join() - def save_physics_simulator_state(self) -> int: """ Saves the state of the physics simulator and returns the unique id of the state. @@ -1312,22 +1261,6 @@ def restore_physics_simulator_state(self, state_id): """ p.restoreState(state_id, self.client_id) - def copy(self) -> BulletWorld: - """ - Copies this Bullet World into another and returns it. The other BulletWorld - will be in Direct mode. The shadow world should always be preferred instead of creating a new BulletWorld. - This method should only be used if necessary since there can be unforeseen problems. - - :return: The reference to the new BulletWorld - """ - world = BulletWorld("DIRECT") - for obj in self.objects: - o = Object(obj.name, obj.type, obj.path, Pose(obj.get_position(), obj.get_orientation()), - world, obj.color) - for joint in obj.joints: - o.set_joint_position(joint, obj.get_joint_position(joint)) - return world - def add_vis_axis(self, pose: Pose, length: Optional[float] = 0.2) -> None: """ @@ -1368,113 +1301,49 @@ def remove_vis_axis(self) -> None: p.removeBody(id) self.vis_axis = [] - def register_collision_callback(self, objectA: Object, objectB: Object, - callback_collision: Callable, - callback_no_collision: Optional[Callable] = None) -> None: - """ - Registers callback methods for contact between two Objects. There can be a callback for when the two Objects - get in contact and, optionally, for when they are not in contact anymore. - - :param objectA: An object in the BulletWorld - :param objectB: Another object in the BulletWorld - :param callback_collision: A function that should be called if the objects are in contact - :param callback_no_collision: A function that should be called if the objects are not in contact - """ - self.coll_callbacks[(objectA, objectB)] = (callback_collision, callback_no_collision) - - def get_shadow_object(self, object: Object) -> Object: - """ - Returns the corresponding object from the shadow world for the given object. If the given Object is already in - the shadow world it is returned. - - :param object: The object for which the shadow worlds object should be returned. - :return: The corresponding object in the shadow world. - """ - try: - return self.world_sync.object_mapping[object] - except KeyError: - shadow_world = self if self.is_prospection_world else self.prospection_world - if object in shadow_world.objects: - return object - else: - raise ValueError( - f"There is no shadow object for the given object: {object}, this could be the case if the object isn't anymore in the main (graphical) BulletWorld or if the given object is already a shadow object. ") - - def get_bullet_object_for_shadow(self, object: Object) -> Object: - """ - Returns the corresponding object from the main Bullet World for a given - object in the shadow world. If the given object is not in the shadow - world an error will be raised. - - :param object: The object for which the corresponding object in the main Bullet World should be found - :return: The object in the main Bullet World - """ - map = self.world_sync.object_mapping - try: - return list(map.keys())[list(map.values()).index(object)] - except ValueError: - raise ValueError("The given object is not in the shadow world.") - def reset_bullet_world(self) -> None: - """ - Resets the BulletWorld to the state it was first spawned in. - All attached objects will be detached, all joints will be set to the - default position of 0 and all objects will be set to the position and - orientation in which they were spawned. - """ - for obj in self.objects: - if obj.attachments: - attached_objects = list(obj.attachments.keys()) - for att_obj in attached_objects: - obj.detach(att_obj) - joint_names = list(obj.joints.keys()) - joint_poses = [0 for j in joint_names] - obj.set_positions_of_all_joints(dict(zip(joint_names, joint_poses))) - obj.set_pose(obj.original_pose) - - -class Use_shadow_world(): +class UseProspectionWorld: """ - An environment for using the shadow world, while in this environment the :py:attr:`~BulletWorld.current_bullet_world` - variable will point to the shadow world. + An environment for using the prospection world, while in this environment the :py:attr:`~World.current_world` + variable will point to the prospection world. Example: - with Use_shadow_world(): + with UseProspectionWorld(): NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() """ - def __init__(self): - self.prev_world: BulletWorld = None + def __init__(self, world_impl: Type[World]): + """ + :param world_impl: A class that implements a World (for example BulletWorld). + """ + self.world_impl = world_impl + self.prev_world: type(world_impl) = None def __enter__(self): - if not BulletWorld.current_bullet_world.is_prospection_world: - time.sleep(20 / 240) + if not self.world_impl.current_world.is_prospection_world: + time.sleep(20 * self.world_impl.simulation_time_step) # blocks until the adding queue is ready - BulletWorld.current_bullet_world.world_sync.add_obj_queue.join() - # **This is currently not used since the sleep(20/240) seems to be enough, but on weaker hardware this might - # not be a feasible solution** - # while not BulletWorld.current_bullet_world.world_sync.equal_states: - # time.sleep(0.1) + self.world_impl.current_world.world_sync.add_obj_queue.join() - self.prev_world = BulletWorld.current_bullet_world - BulletWorld.current_bullet_world.world_sync.pause_sync = True - BulletWorld.current_bullet_world = BulletWorld.current_bullet_world.prospection_world + self.prev_world = self.world_impl.current_world + self.world_impl.current_world.world_sync.pause_sync = True + self.world_impl.current_world = self.world_impl.current_world.prospection_world def __exit__(self, *args): - if not self.prev_world == None: - BulletWorld.current_bullet_world = self.prev_world - BulletWorld.current_bullet_world.world_sync.pause_sync = False + if self.prev_world is not None: + self.world_impl.current_world = self.prev_world + self.world_impl.current_world.world_sync.pause_sync = False class WorldSync(threading.Thread): """ - Synchronizes the state between the BulletWorld and its shadow world. - Meaning the cartesian and joint position of everything in the shadow world will be - synchronized with the main BulletWorld. + Synchronizes the state between the World and its prospection world. + Meaning the cartesian and joint position of everything in the prospection world will be + synchronized with the main World. Adding and removing objects is done via queues, such that loading times of objects - in the shadow world does not affect the BulletWorld. + in the prospection world does not affect the World. The class provides the possibility to pause the synchronization, this can be used - if reasoning should be done in the shadow world. + if reasoning should be done in the prospection world. """ def __init__(self, world: World, prospection_world: World): @@ -1487,7 +1356,7 @@ def __init__(self, world: World, prospection_world: World): self.add_obj_queue: Queue = Queue() self.remove_obj_queue: Queue = Queue() self.pause_sync: bool = False - # Maps bullet to shadow world objects + # Maps bullet to prospection world objects self.object_mapping: Dict[Object, Object] = {} self.equal_states = False @@ -1496,8 +1365,8 @@ def run(self): Main method of the synchronization, this thread runs in a loop until the terminate flag is set. While this loop runs it continuously checks the cartesian and joint position of - every object in the BulletWorld and updates the corresponding object in the - shadow world. When there are entries in the adding or removing queue the corresponding objects will be added + every object in the World and updates the corresponding object in the + prospection world. When there are entries in the adding or removing queue the corresponding objects will be added or removed in the same iteration. """ while not self.terminate: @@ -1507,28 +1376,28 @@ def run(self): obj = self.add_obj_queue.get() # [name, type, path, position, orientation, self.world.prospection_world, color, bulletworld object] o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) - # Maps the BulletWorld object to the shadow world object + # Maps the World object to the prospection world object self.object_mapping[obj[7]] = o self.add_obj_queue.task_done() for i in range(self.remove_obj_queue.qsize()): obj = self.remove_obj_queue.get() - # Get shadow world object reference from object mapping - shadow_obj = self.object_mapping[obj] - shadow_obj.remove() + # Get prospection world object reference from object mapping + prospection_obj = self.object_mapping[obj] + prospection_obj.remove() del self.object_mapping[obj] self.remove_obj_queue.task_done() - for bulletworld_obj, shadow_obj in self.object_mapping.items(): + for bulletworld_obj, prospection_obj in self.object_mapping.items(): b_pose = bulletworld_obj.get_pose() - s_pose = shadow_obj.get_pose() + s_pose = prospection_obj.get_pose() if b_pose.dist(s_pose) != 0.0: - shadow_obj.set_pose(bulletworld_obj.get_pose()) + prospection_obj.set_pose(bulletworld_obj.get_pose()) # Manage joint positions if len(bulletworld_obj.joints) > 2: for joint_name in bulletworld_obj.joints.keys(): - if shadow_obj.get_joint_position(joint_name) != bulletworld_obj.get_joint_position(joint_name): - shadow_obj.set_positions_of_all_joints(bulletworld_obj.get_positions_of_all_joints()) + if prospection_obj.get_joint_position(joint_name) != bulletworld_obj.get_joint_position(joint_name): + prospection_obj.set_positions_of_all_joints(bulletworld_obj.get_positions_of_all_joints()) break self.check_for_pause() @@ -1547,25 +1416,25 @@ def check_for_pause(self) -> None: def check_for_equal(self) -> None: """ - Checks if both BulletWorlds have the same state, meaning all objects are in the same position. + Checks if both Worlds have the same state, meaning all objects are in the same position. This is currently not used, but might be used in the future if synchronization issues worsen. """ eql = True - for obj, shadow_obj in self.object_mapping.items(): - eql = eql and obj.get_pose() == shadow_obj.get_pose() + for obj, prospection_obj in self.object_mapping.items(): + eql = eql and obj.get_pose() == prospection_obj.get_pose() self.equal_states = eql class Gui(threading.Thread): """ - For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~BulletWorld.exit` + For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~World.exit` Also contains the code for controlling the camera. """ - def __init__(self, world, type): + def __init__(self, world: World, mode: str): threading.Thread.__init__(self) - self.world: BulletWorld = world - self.type: str = type + self.world = world + self.mode: str = mode def run(self): """ @@ -1573,7 +1442,7 @@ def run(self): if it is still active. If it is the thread will be suspended for 1/80 seconds, if it is not the method and thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. """ - if self.type != "GUI": + if self.mode != "GUI": self.world.client_id = p.connect(p.DIRECT) else: self.world.client_id = p.connect(p.GUI) @@ -1808,9 +1677,9 @@ def __init__(self, name: str, type: Union[str, ObjectType], path: str, self.cids: Dict[Object, int] = {} self.original_pose = pose_in_map - self.tf_frame = ("shadow/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) + self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) - # This means "world" is not the shadow world since it has a reference to a shadow world + # This means "world" is not the prospection world since it has a reference to a prospection world if self.world.prospection_world is not None: self.world.world_sync.add_obj_queue.put( [name, type, path, position, orientation, self.world.prospection_world, color, self]) @@ -1851,8 +1720,8 @@ def remove(self) -> None: for obj in self.attachments.keys(): self.world.detach_objects(self, obj) self.world.objects.remove(self) - # This means the current world of the object is not the shadow world, since it - # has a reference to the shadow world + # This means the current world of the object is not the prospection world, since it + # has a reference to the prospection world if self.world.prospection_world is not None: self.world.world_sync.remove_obj_queue.put(self) self.world.world_sync.remove_obj_queue.join() @@ -1905,7 +1774,7 @@ def detach_all(self) -> None: for att in attachments.keys(): self.world.detach_objects(self, att) - def get_position(self) -> Pose: + def get_position(self) -> Pose.position: """ Returns the position of this Object as a list of xyz. @@ -1913,7 +1782,7 @@ def get_position(self) -> Pose: """ return self.get_pose().position - def get_orientation(self) -> Quaternion: + def get_orientation(self) -> Pose.orientation: """ Returns the orientation of this object as a list of xyzw, representing a quaternion. @@ -2105,7 +1974,7 @@ def get_transform_between_two_links(self, source_link_id: int, target_link_id: i target_tf_frame = self.get_link_tf_frame_by_id(target_link_id) return self.local_transformer.lookup_transform_from_source_to_target_frame(source_tf_frame, target_tf_frame) - def get_link_position(self, name: str) -> Point: + def get_link_position(self, name: str) -> Pose.position: """ Returns the position of a link of this Object. Position is returned as a list of xyz. @@ -2269,7 +2138,7 @@ def get_color(self, link: Optional[str] = None) -> Union[List[float], Dict[str, """ return self.world.get_object_color(self, link) - def get_AABB(self) -> Tuple[List[float], List[float]]: + def get_aabb(self) -> Tuple[List[float], List[float]]: """ Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. @@ -2278,7 +2147,7 @@ def get_AABB(self) -> Tuple[List[float], List[float]]: """ return self.world.get_object_AABB(self) - def get_link_AABB(self, link_name: str) -> Tuple[List[float], List[float]]: + def get_link_aabb(self, link_name: str) -> Tuple[List[float], List[float]]: """ Returns the axis aligned bounding box of the given link name. The return of this method are two points in world coordinate frame which define a bounding box. @@ -2295,7 +2164,7 @@ def get_base_origin(self, link_name: Optional[str] = None) -> Pose: :param link_name: The link name for which the bottom position should be returned :return: The position of the bottom of this Object or link """ - aabb = self.get_link_AABB(link_name=link_name) + aabb = self.get_link_aabb(link_name=link_name) base_width = np.absolute(aabb[0][0] - aabb[1][0]) base_length = np.absolute(aabb[0][1] - aabb[1][1]) return Pose([aabb[0][0] + base_width / 2, aabb[0][1] + base_length / 2, aabb[0][2]], @@ -2374,7 +2243,7 @@ def get_link_tf_frame(self, link_name: str) -> str: def get_link_tf_frame_by_id(self, link_id: int) -> str: return self.get_link_tf_frame(self.get_link_by_id(link_id)) - def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]: + def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.GeometricType]: """ Extracts the geometry information for each collision of each link and links them to the respective link. @@ -2450,7 +2319,8 @@ def _get_robot_name_from_urdf(urdf_string: str) -> str: begin = res[0].find("\"") end = res[0][begin + 1:].find("\"") robot = res[0][begin + 1:begin + 1 + end].lower() - return robot + return robot + raise Exception("Robot Name not Found") def _load_object(name: str, @@ -2459,7 +2329,7 @@ def _load_object(name: str, orientation: List[float], world: World, color: List[float], - ignoreCachedFiles: bool) -> Tuple[int, str]: + ignore_cached_files: bool) -> Tuple[int, str]: """ Loads an object to the given World with the given position and orientation. The color will only be used when an .obj or .stl file is given. @@ -2475,7 +2345,7 @@ def _load_object(name: str, :param orientation: The orientation in which the object should be spawned :param world: The World to which the Object should be spawned :param color: The color of the object, only used when .obj or .stl file is given - :param ignoreCachedFiles: Whether to ignore files in the cache directory. + :param ignore_cached_files: Whether to ignore files in the cache directory. :return: The unique id of the object and the path to the file used for spawning """ pa = pathlib.Path(path) @@ -2496,7 +2366,7 @@ def _load_object(name: str, os.mkdir(cach_dir) # if file is not yet cached corrcet the urdf and save if in the cache directory - if not _is_cached(path, name, cach_dir) or ignoreCachedFiles: + if not _is_cached(path, name, cach_dir) or ignore_cached_files: if extension == ".obj" or extension == ".stl": path = _generate_urdf_file(name, path, color, cach_dir) elif extension == ".urdf": @@ -2564,7 +2434,7 @@ def _correct_urdf_string(urdf_string: str) -> str: Changes paths for files in the URDF from ROS paths to paths in the file system. Since PyBullet can't deal with ROS package paths. - :param urdf_name: The name of the URDf on the parameter server + :param urdf_string: The name of the URDf on the parameter server :return: The URDF string with paths in the filesystem instead of ROS packages """ r = rospkg.RosPack() @@ -2684,13 +2554,13 @@ def _generate_urdf_file(name: str, path: str, color: List[float], cach_dir: str) return cach_dir + pathlib_obj.stem + ".urdf" -def _world_and_id(world: BulletWorld) -> Tuple[BulletWorld, int]: +def _world_and_id(world: World) -> Tuple[World, int]: """ - Selects the world to be used. If the given world is None the 'current_bullet_world' is used. + Selects the world to be used. If the given world is None the 'current_world' is used. - :param world: The world which should be used or None if 'current_bullet_world' should be used - :return: The BulletWorld object and the id of this BulletWorld + :param world: The world which should be used or None if 'current_world' should be used + :return: The World object and the id of this World """ - world = world if world is not None else BulletWorld.current_bullet_world - id = world.client_id if world is not None else BulletWorld.current_bullet_world.client_id + world = world if world is not None else World.current_world + id = world.client_id if world is not None else World.current_world.client_id return world, id diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index d655b8f8c..a849f8a10 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -3,7 +3,7 @@ import numpy as np import rospy -from .world import _world_and_id, Object, Use_shadow_world, BulletWorld +from .world import _world_and_id, Object, UseProspectionWorld, BulletWorld from .external_interfaces.ik import request_ik from .local_transformer import LocalTransformer from .plan_failures import IKError @@ -97,14 +97,14 @@ def stable(object: Object, :return: True if the given object is stable in the world False else """ world, world_id = _world_and_id(world) - shadow_obj = BulletWorld.current_bullet_world.get_shadow_object(object) - with Use_shadow_world(): + shadow_obj = BulletWorld.current_world.get_prospection_object(object) + with UseProspectionWorld(BulletWorld): coords_prev = shadow_obj.pose.position_as_list() - state = p.saveState(physicsClientId=BulletWorld.current_bullet_world.client_id) - p.setGravity(0, 0, -9.8, BulletWorld.current_bullet_world.client_id) + state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) + p.setGravity(0, 0, -9.8, BulletWorld.current_world.client_id) # one Step is approximately 1/240 seconds - BulletWorld.current_bullet_world.simulate(2) + BulletWorld.current_world.simulate(2) # coords_past = p.getBasePositionAndOrientation(object.id, physicsClientId=world_id)[0] coords_past = shadow_obj.pose.position_as_list() @@ -128,12 +128,12 @@ def contact(object1: Object, :return: True if the two objects are in contact False else. If links should be returned a list of links in contact """ - with Use_shadow_world(): - shadow_obj1 = BulletWorld.current_bullet_world.get_shadow_object(object1) - shadow_obj2 = BulletWorld.current_bullet_world.get_shadow_object(object2) - p.performCollisionDetection(BulletWorld.current_bullet_world.client_id) + with UseProspectionWorld(BulletWorld): + shadow_obj1 = BulletWorld.current_world.get_prospection_object(object1) + shadow_obj2 = BulletWorld.current_world.get_prospection_object(object2) + p.performCollisionDetection(BulletWorld.current_world.client_id) con_points = p.getContactPoints(shadow_obj1.id, shadow_obj2.id, - physicsClientId=BulletWorld.current_bullet_world.client_id) + physicsClientId=BulletWorld.current_world.client_id) if return_links: contact_links = [] @@ -163,12 +163,12 @@ def visible(object: Object, :return: True if the object is visible from the camera_position False if not """ front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis - with Use_shadow_world(): - shadow_obj = BulletWorld.current_bullet_world.get_shadow_object(object) + with UseProspectionWorld(BulletWorld): + shadow_obj = BulletWorld.current_world.get_prospection_object(object) if BulletWorld.robot: - shadow_robot = BulletWorld.current_bullet_world.get_shadow_object(BulletWorld.robot) - state = p.saveState(physicsClientId=BulletWorld.current_bullet_world.client_id) - for obj in BulletWorld.current_bullet_world.objects: + shadow_robot = BulletWorld.current_world.get_prospection_object(BulletWorld.robot) + state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) + for obj in BulletWorld.current_world.objects: if obj == shadow_obj or BulletWorld.robot and obj == shadow_robot: continue else: @@ -180,15 +180,15 @@ def visible(object: Object, # target_point = p.multiplyTransforms(world_to_cam.translation_as_list(), world_to_cam.rotation_as_list(), cam_to_point.translation_as_list(), [0, 0, 0, 1]) # print(target_point) - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_bullet_world)[2] + seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] flat_list = list(itertools.chain.from_iterable(seg_mask)) max_pixel = sum(list(map(lambda x: 1 if x == shadow_obj.id else 0, flat_list))) - p.restoreState(state, physicsClientId=BulletWorld.current_bullet_world.client_id) + p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) if max_pixel == 0: # Object is not visible return False - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_bullet_world)[2] + seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] flat_list = list(itertools.chain.from_iterable(seg_mask)) real_pixel = sum(list(map(lambda x: 1 if x == shadow_obj.id else 0, flat_list))) @@ -215,9 +215,9 @@ def occluding(object: Object, world, world_id = _world_and_id(world) # occ_world = world.copy() # state = p.saveState(physicsClientId=occ_world.client_id) - with Use_shadow_world(): - state = p.saveState(physicsClientId=BulletWorld.current_bullet_world.client_id) - for obj in BulletWorld.current_bullet_world.objects: + with UseProspectionWorld(BulletWorld): + state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) + for obj in BulletWorld.current_world.objects: if obj.name == BulletWorld.robot.name: continue elif object.get_pose() == obj.get_pose(): @@ -229,22 +229,22 @@ def occluding(object: Object, cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", "point") target_point = (world_to_cam * cam_to_point).to_pose() - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_bullet_world)[2] + seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] # All indices where the object that could be occluded is in the image # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension pix = np.dstack((seg_mask == object.id).nonzero())[0] - p.restoreState(state, physicsClientId=BulletWorld.current_bullet_world.client_id) + p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) occluding = [] - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_bullet_world)[2] + seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] for c in pix: if not seg_mask[c[0]][c[1]] == object.id: occluding.append(seg_mask[c[0]][c[1]]) - occ_objects = list(set(map(BulletWorld.current_bullet_world.get_object_by_id, occluding))) - occ_objects = list(map(world.get_bullet_object_for_shadow, occ_objects)) + occ_objects = list(set(map(BulletWorld.current_world.get_object_by_id, occluding))) + occ_objects = list(map(world.get_object_from_prospection, occ_objects)) return occ_objects @@ -267,8 +267,8 @@ def reachable(pose: Union[Object, Pose], if type(pose) == Object: pose = pose.get_pose() - shadow_robot = BulletWorld.current_bullet_world.get_shadow_object(robot) - with Use_shadow_world(): + shadow_robot = BulletWorld.current_world.get_prospection_object(robot) + with UseProspectionWorld(BulletWorld): arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" joints = robot_description.chains[arm].joints try: @@ -305,8 +305,8 @@ def blocking(pose_or_object: Union[Object, Pose], else: input_pose = pose_or_object - shadow_robot = BulletWorld.current_bullet_world.get_shadow_object(robot) - with Use_shadow_world(): + shadow_robot = BulletWorld.current_world.get_prospection_object(robot) + with UseProspectionWorld(BulletWorld): arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" joints = robot_description.chains[arm].joints local_transformer = LocalTransformer() @@ -327,9 +327,9 @@ def blocking(pose_or_object: Union[Object, Pose], _apply_ik(shadow_robot, inv, joints) block = [] - for obj in BulletWorld.current_bullet_world.objects: + for obj in BulletWorld.current_world.objects: if contact(shadow_robot, obj): - block.append(BulletWorld.current_bullet_world.get_bullet_object_for_shadow(obj)) + block.append(BulletWorld.current_world.get_object_from_prospection(obj)) return block @@ -357,8 +357,8 @@ def link_pose_for_joint_config(object: Object, joint_config: Dict[str, float], l :param link_name: Name of the link for which the pose should be returned :return: The pose of the link after applying the joint configuration """ - shadow_object = BulletWorld.current_bullet_world.get_shadow_object(object) - with Use_shadow_world(): + shadow_object = BulletWorld.current_world.get_prospection_object(object) + with UseProspectionWorld(BulletWorld): for joint, pose in joint_config.items(): shadow_object.set_joint_position(joint, pose) return shadow_object.get_link_pose(link_name) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index fbe4d3605..704711756 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -25,7 +25,7 @@ def setUpClass(cls): ProcessModule.execution_delay = False def setUp(self): - self.world.reset_bullet_world() + self.world.reset_world() def test_object_movement(self): self.milk.set_position(Pose([0, 1, 1])) @@ -39,7 +39,7 @@ def test_robot_orientation(self): self.assertEqual(self.robot.get_link_position('head_pan_link').z, head_position) def tearDown(self): - self.world.reset_bullet_world() + self.world.reset_world() @classmethod def tearDownClass(cls): diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 124057c4d..993dea9bc 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -79,7 +79,7 @@ def test_costmap_with_obstacles(self): raise pycram.plan_failures.PlanFailure() def tearDown(self) -> None: - self.world.reset_bullet_world() + self.world.reset_world() @classmethod def tearDownClass(cls) -> None: diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index 5a9101ebe..9da659e5b 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -89,7 +89,7 @@ def test_costmap_with_obstacles(self): raise pycram.plan_failures.PlanFailure() def tearDown(self) -> None: - self.world.reset_bullet_world() + self.world.reset_world() @classmethod def tearDownClass(cls) -> None: diff --git a/test/test_task_tree.py b/test/test_task_tree.py index abe3f3ab8..5648ddf00 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -65,7 +65,7 @@ def failing_plan(): def test_execution(self): self.plan() - self.world.reset_bullet_world() + self.world.reset_world() tt = pycram.task.task_tree # self.setUpBulletWorld(False) with simulated_robot: From 2d834a42352f618313f3113397e493072d8b3812 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Dec 2023 16:51:33 +0100 Subject: [PATCH 13/69] [AbstractWorld] Completed world.py refactoring, tests are passing except test_attachment_exclusion in test_costmap.py. --- src/pycram/costmaps.py | 6 +- src/pycram/designator.py | 16 +- src/pycram/designators/action_designator.py | 8 +- src/pycram/designators/location_designator.py | 4 +- src/pycram/designators/motion_designator.py | 6 +- src/pycram/designators/object_designator.py | 9 +- src/pycram/enums.py | 3 + src/pycram/external_interfaces/robokudo.py | 4 +- src/pycram/orm/object_designator.py | 2 +- .../process_modules/boxy_process_modules.py | 2 +- .../process_modules/donbot_process_modules.py | 2 +- .../process_modules/hsr_process_modules.py | 2 +- .../process_modules/pr2_process_modules.py | 2 +- .../resolver/location/database_location.py | 2 +- .../resolver/location/giskard_location.py | 2 +- src/pycram/resolver/location/jpt_location.py | 2 +- src/pycram/task.py | 4 +- src/pycram/world.py | 228 ++++++++++-------- src/pycram/world_reasoning.py | 14 +- test/local_transformer_tests.py | 8 +- test/test_action_designator.py | 3 +- test/test_object_designator.py | 2 +- 22 files changed, 175 insertions(+), 156 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 86017cdbd..080d31c71 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -385,7 +385,7 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: i = 0 j = 0 for n in self._chunks(np.array(rays), 16380): - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): r_t = p.rayTestBatch(n[:, 0], n[:, 1], numThreads=0, physicsClientId=BulletWorld.current_world.client_id) while r_t is None: @@ -494,7 +494,7 @@ def _create_images(self) -> List[np.ndarray]: images = [] camera_pose = self.origin - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): origin_copy = self.origin.copy() origin_copy.position.y += 1 images.append( @@ -726,7 +726,7 @@ def get_aabb_for_link(self) -> Tuple[List[float], List[float]]: :return: Two points in world coordinate space, which span a rectangle """ shadow_obj = BulletWorld.current_world.get_prospection_object(self.object) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): shadow_obj.set_orientation(Pose(orientation=[0, 0, 0, 1])) link_orientation = shadow_obj.get_link_pose(self.link) link_orientation_trans = link_orientation.to_transform(self.object.get_link_tf_frame(self.link)) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 3329fba27..d7c7a670d 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -487,7 +487,7 @@ class Action: def __post_init__(self): self.robot_position = BulletWorld.robot.get_pose() self.robot_torso_height = BulletWorld.robot.get_joint_position(robot_description.torso_joint) - self.robot_type = BulletWorld.robot.type + self.robot_type = BulletWorld.robot.obj_type @with_tree def perform(self) -> Any: @@ -596,7 +596,7 @@ class Object: Name of the object """ - type: str + obj_type: str """ Type of the object """ @@ -622,7 +622,7 @@ def to_sql(self) -> ORMObjectDesignator: :return: The created ORM object. """ - return ORMObjectDesignator(self.type, self.name) + return ORMObjectDesignator(self.obj_type, self.name) def insert(self, session: Session) -> ORMObjectDesignator: """ @@ -651,7 +651,7 @@ def data_copy(self) -> 'ObjectDesignatorDescription.Object': object is not copied. The _pose gets set to a method that statically returns the pose of the object when this method was called. """ - result = ObjectDesignatorDescription.Object(self.name, self.type, None) + result = ObjectDesignatorDescription.Object(self.name, self.obj_type, None) # get current object pose and set resulting pose to always be that pose = self.pose result.pose = lambda: pose @@ -693,8 +693,8 @@ def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: pose_in_object = lt.transform_pose_to_target_frame(pose, self.bullet_world_object.tf_frame) special_knowledge = [] # Initialize as an empty list - if self.type in SPECIAL_KNOWLEDGE: - special_knowledge = SPECIAL_KNOWLEDGE[self.type] + if self.obj_type in SPECIAL_KNOWLEDGE: + special_knowledge = SPECIAL_KNOWLEDGE[self.obj_type] for key, value in special_knowledge: if key == grasp: @@ -760,7 +760,7 @@ def __iter__(self) -> Iterable[Object]: continue # skip if type does not match specification - if self.types and obj.type not in self.types: + if self.types and obj.obj_type not in self.types: continue - yield self.Object(obj.name, obj.type, obj) \ No newline at end of file + yield self.Object(obj.name, obj.obj_type, obj) \ No newline at end of file diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index b92363bf1..c44f496ab 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -401,8 +401,10 @@ def ground(self) -> Action: :return: A performable designator """ - obj_desig = self.object_designator_description if isinstance(self.object_designator_description, - ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() + if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object): + obj_desig = self.object_designator_description + else: + obj_desig = self.object_designator_description.resolve() return self.Action(obj_desig, self.arms[0], self.grasps[0]) @@ -702,7 +704,7 @@ class Action(ActionDesignatorDescription.Action): @with_tree def perform(self) -> Any: - return DetectingMotion(object_type=self.object_designator.type).resolve().perform() + return DetectingMotion(object_type=self.object_designator.obj_type).resolve().perform() def to_sql(self) -> ORMDetectAction: return ORMDetectAction() diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 99c2a4b0e..aae10e4b7 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -182,7 +182,7 @@ def __iter__(self): robot_object = self.visible_for.bullet_world_object if self.visible_for else self.reachable_for.bullet_world_object test_robot = BulletWorld.current_world.get_prospection_object(robot_object) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): for maybe_pose in pose_generator(final_map, number_of_samples=600): res = True @@ -275,7 +275,7 @@ def __iter__(self) -> Location: container_joint: self.handle.bullet_world_object.get_joint_limits(container_joint)[1] / 1.5}, self.handle.name) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): for maybe_pose in pose_generator(final_map, number_of_samples=600, orientation_generator=lambda p, o: generate_orientation(p, half_pose)): diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 8d618ed20..f148aee18 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -385,10 +385,10 @@ def perform(self): raise PerceptionObjectNotFound( f"Could not find an object with the type {self.object_type} in the FOV of the robot") if ProcessModuleManager.execution_type == "real": - return RealObject.Object(bullet_world_object.name, bullet_world_object.type, - bullet_world_object, bullet_world_object.get_pose()) + return RealObject.Object(bullet_world_object.name, bullet_world_object.obj_type, + bullet_world_object, bullet_world_object.get_pose()) - return ObjectDesignatorDescription.Object(bullet_world_object.name, bullet_world_object.type, + return ObjectDesignatorDescription.Object(bullet_world_object.name, bullet_world_object.obj_type, bullet_world_object) def to_sql(self) -> ORMDetectingMotion: diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 0b063ac9f..8d87c4936 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -21,7 +21,7 @@ class Object(ObjectDesignatorDescription.Object): """ def to_sql(self) -> ORMBelieveObject: - return ORMBelieveObject(self.type, self.name) + return ORMBelieveObject(self.obj_type, self.name) def insert(self, session: sqlalchemy.orm.session.Session) -> ORMBelieveObject: self_ = self.to_sql() @@ -44,7 +44,7 @@ class Object(ObjectDesignatorDescription.Object): part_pose: Pose def to_sql(self) -> ORMObjectPart: - return ORMObjectPart(self.type, self.name) + return ORMObjectPart(self.obj_type, self.name) def insert(self, session: sqlalchemy.orm.session.Session) -> ORMObjectPart: obj = self.to_sql() @@ -169,9 +169,6 @@ def __iter__(self): """ object_candidates = query(self) for obj_desig in object_candidates: - for bullet_obj in BulletWorld.get_objects_by_type(obj_desig.type): + for bullet_obj in BulletWorld.get_objects_by_type(obj_desig.obj_type): obj_desig.bullet_world_object = bullet_obj yield obj_desig - # if bullet_obj.get_pose().dist(obj_deisg.pose) < 0.05: - # obj_deisg.bullet_world_object = bullet_obj - # yield obj_deisg diff --git a/src/pycram/enums.py b/src/pycram/enums.py index dc0dc6ed5..40257a3c2 100644 --- a/src/pycram/enums.py +++ b/src/pycram/enums.py @@ -30,6 +30,9 @@ class JointType(Enum): PLANAR = 3 FIXED = 4 + def as_int(self): + return self.value + class Grasp(Enum): """ diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index c6e897af8..9e0164978 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -85,7 +85,7 @@ def msg_from_obj_desig(obj_desc: ObjectDesignatorDescription) -> 'robokudo_Objet """ obj_msg = robokudo_ObjetDesignator() obj_msg.uid = str(id(obj_desc)) - obj_msg.type = obj_desc.types[0] # For testing purposes + obj_msg.obj_type = obj_desc.types[0] # For testing purposes return obj_msg @@ -99,7 +99,7 @@ def make_query_goal_msg(obj_desc: ObjectDesignatorDescription) -> 'QueryGoal': """ goal_msg = QueryGoal() goal_msg.obj.uid = str(id(obj_desc)) - goal_msg.obj.type = str(obj_desc.types[0].name) # For testing purposes + goal_msg.obj.obj_type = str(obj_desc.types[0].name) # For testing purposes if ObjectType.JEROEN_CUP == obj_desc.types[0]: goal_msg.obj.color.append("blue") elif ObjectType.BOWL == obj_desc.types[0]: diff --git a/src/pycram/orm/object_designator.py b/src/pycram/orm/object_designator.py index 78a584282..fad1d3c6e 100644 --- a/src/pycram/orm/object_designator.py +++ b/src/pycram/orm/object_designator.py @@ -26,7 +26,7 @@ class Object(PoseMixin, Base): """ORM class of pycram.designators.object_designator.ObjectDesignator""" dtype: Mapped[str] = mapped_column(init=False) - type: Mapped[ObjectType] + obj_type: Mapped[ObjectType] name: Mapped[str] __mapper_args__ = { diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index e1314e41c..41c93f0fe 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -283,7 +283,7 @@ def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "world-state-detecting": obj_type = solution['object_type'] - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_world.objects))[0] + return list(filter(lambda obj: obj.obj_type == obj_type, BulletWorld.current_world.objects))[0] class BoxyManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index d893d8048..1d1320cfd 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -240,7 +240,7 @@ def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "world-state-detecting": obj_type = solution['object_type'] - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_world.objects))[0] + return list(filter(lambda obj: obj.obj_type == obj_type, BulletWorld.current_world.objects))[0] class DonbotManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 09b715e1c..62120af7c 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -213,7 +213,7 @@ def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "world-state-detecting": obj_type = solution['object_type'] - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_world.objects))[0] + return list(filter(lambda obj: obj.obj_type == obj_type, BulletWorld.current_world.objects))[0] class HSRManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 3432533d0..bcadb46ec 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -205,7 +205,7 @@ class Pr2WorldStateDetecting(ProcessModule): def _execute(self, desig: WorldStateDetectingMotion.Motion): obj_type = desig.object_type - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_world.objects))[0] + return list(filter(lambda obj: obj.obj_type == obj_type, BulletWorld.current_world.objects))[0] class Pr2Open(ProcessModule): diff --git a/src/pycram/resolver/location/database_location.py b/src/pycram/resolver/location/database_location.py index b4e92c075..392fab35c 100644 --- a/src/pycram/resolver/location/database_location.py +++ b/src/pycram/resolver/location/database_location.py @@ -54,7 +54,7 @@ def create_query_from_occupancy_costmap(self) -> sqlalchemy.orm.Query: join(filtered_tasks, filtered_tasks.c.code == filtered_code.c.id).subquery() # filter all objects that have the same type as the target - filtered_objects = self.session.query(Object).filter(Object.type == self.target.type).\ + filtered_objects = self.session.query(Object).filter(Object.obj_type == self.target.obj_type).\ subquery() query = self.session.query(PickUpAction.arm, PickUpAction.grasp, diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index 2a2840f36..5c891b4e0 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -29,7 +29,7 @@ def __iter__(self) -> CostmapLocation.Location: robot_description.get_tool_frame("left")) test_robot = BulletWorld.current_world.get_prospection_object(BulletWorld.robot) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): valid, arms = reachability_validator(pose_right, test_robot, self.target, {}) if valid: yield CostmapLocation.Location(pose_right, arms) diff --git a/src/pycram/resolver/location/jpt_location.py b/src/pycram/resolver/location/jpt_location.py index 5c7bf1bae..748f559bb 100644 --- a/src/pycram/resolver/location/jpt_location.py +++ b/src/pycram/resolver/location/jpt_location.py @@ -116,7 +116,7 @@ def create_evidence(self, use_success=True) -> jpt.variables.LabelAssignment: """ evidence = dict() - evidence["type"] = {self.target.type} + evidence["type"] = {self.target.obj_type} if use_success: evidence["status"] = {"SUCCEEDED"} diff --git a/src/pycram/task.py b/src/pycram/task.py index 7ec966918..aa21f41a0 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -254,7 +254,7 @@ def __enter__(self): def simulation(): return None self.suspended_tree = task_tree - self.world_state, self.objects2attached = BulletWorld.current_world.save_state() + self.world_state = BulletWorld.current_world.save_state() self.simulated_root = TaskTreeNode(code=Code(simulation)) task_tree = self.simulated_root pybullet.addUserDebugText("Simulating...", [0, 0, 1.75], textColorRGB=[0, 0, 0], @@ -267,7 +267,7 @@ def __exit__(self, exc_type, exc_val, exc_tb): """ global task_tree task_tree = self.suspended_tree - BulletWorld.current_world.restore_state(self.world_state, self.objects2attached) + BulletWorld.current_world.restore_state(self.world_state) pybullet.removeAllUserDebugItems() diff --git a/src/pycram/world.py b/src/pycram/world.py index c78b1eeb5..413a44492 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -42,6 +42,7 @@ def __init__(self, parent_link_id: Optional[int] = -1, # -1 means base link child_link_id: Optional[int] = -1, bidirectional: Optional[bool] = False, + child_to_parent_transform: Optional[Transform] = None, constraint_id: Optional[int] = None): """ Creates an attachment between the parent object and the child object. @@ -51,28 +52,41 @@ def __init__(self, self.parent_link_id = parent_link_id self.child_link_id = child_link_id self.bidirectional = bidirectional - self.child_to_parent_transform = self.calculate_transforms() self._loose = False and not self.bidirectional - self.constraint_id = self.update_constraint() if constraint_id is None else constraint_id + + self.child_to_parent_transform = child_to_parent_transform + if self.child_to_parent_transform is None: + self.update_transform() + + self.constraint_id = constraint_id + if self.constraint_id is None: + self.add_constraint_and_update_objects_constraints_collection() def update_attachment(self): - self.update_transforms() + self.update_transform() self.update_constraint() - self.update_objects_attachments_collection() - def update_transforms(self): - self.child_to_parent_transform = self.calculate_transforms() + def update_transform(self): + self.child_to_parent_transform = self.calculate_transform() def update_constraint(self): + self.remove_constraint_if_exists() + self.add_constraint_and_update_objects_constraints_collection() + + def calculate_transform(self): + relative_pose = self.parent_object.get_other_object_link_pose_relative_to_my_link(self.parent_link_id, + self.child_link_id, + self.child_object) + return relative_pose.to_transform(self.child_object.get_link_tf_frame_by_id(self.child_link_id)) + + def remove_constraint_if_exists(self): if self.constraint_id is not None: self.parent_object.world.remove_constraint(self.constraint_id) + + def add_constraint_and_update_objects_constraints_collection(self): self.constraint_id = self.add_fixed_constraint() self.update_objects_constraints_collection() - def update_objects_attachments_collection(self): - self.parent_object.attachments[self.child_object] = self - self.child_object.attachments[self.parent_object] = self.get_inverse() - def add_fixed_constraint(self): constraint_id = self.parent_object.world.add_fixed_constraint(self.parent_object.id, self.child_object.id, @@ -87,13 +101,11 @@ def update_objects_constraints_collection(self): def get_inverse(self): attachment = Attachment(self.child_object, self.parent_object, self.child_link_id, - self.parent_link_id, self.bidirectional, self.constraint_id) + self.parent_link_id, self.bidirectional, self.child_to_parent_transform.invert(), + self.constraint_id) attachment.loose = False if self.loose else True return attachment - def calculate_transforms(self): - return self.parent_object.get_transform_between_two_links(self.parent_link_id, self.child_link_id) - @property def loose(self) -> bool: """ @@ -153,7 +165,7 @@ class World(ABC): URDF with the name of the URDF on the parameter server. """ - data_directory: List[str] = {os.path.dirname(__file__) + "/../../resources"} + data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] """ Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. """ @@ -169,16 +181,19 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. """ + + self._init_current_world_and_simulation_time_step(simulation_time_step) + # init the current world and simulation time step which are class variables. + self.objects: List[Object] = [] self.client_id: int = -1 self.mode: str = mode + self._init_events() - World.current_world = self if World.current_world is None else World.current_world self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} self.local_transformer = LocalTransformer() self.is_prospection_world: bool = is_prospection_world - if is_prospection_world: # then no need to add another prospection world self.prospection_world = None self.world_sync = None @@ -186,9 +201,12 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: self._init_and_sync_prospection_world() self._update_local_transformer_worlds() + self._saved_states: Dict[int, WorldState] = {} + # Different states of the world indexed by int state id. + + def _init_current_world_and_simulation_time_step(self, simulation_time_step): + World.current_world = self if World.current_world is None else World.current_world World.simulation_time_step = simulation_time_step - self.simulation_frequency = int(1/self.simulation_time_step) - self._saved_states: Dict[int, WorldState] = {} # Different states of the world indexed by int state id. def _init_events(self): self.detachment_event: Event = Event() @@ -203,13 +221,18 @@ def _update_local_transformer_worlds(self): self.local_transformer.world = self self.local_transformer.prospection_world = self.prospection_world - def _init_prospection_world(self): - self.prospection_world: World = World("DIRECT", True) + @classmethod + def _init_prospection_world(cls): + cls.prospection_world: World = cls("DIRECT", True) def _sync_prospection_world(self): self.world_sync: WorldSync = WorldSync(self, self.prospection_world) self.world_sync.start() + @property + def simulation_frequency(self): + return int(1/World.simulation_time_step) + @abstractmethod def load_urdf_at_pose_and_get_object_id(self, path: str, pose: Pose) -> int: pass @@ -230,7 +253,7 @@ def get_objects_by_type(self, obj_type: str) -> List[Object]: :param obj_type: The type of the returned Objects. :return: A list of all Objects that have the type 'obj_type'. """ - return list(filter(lambda obj: obj.type == obj_type, self.objects)) + return list(filter(lambda obj: obj.obj_type == obj_type, self.objects)) def get_object_by_id(self, id: int) -> Object: """ @@ -266,6 +289,7 @@ def _set_attached_objects(self, parent, prev_object: List[Object]) -> None: continue if not parent.attachments[child].bidirectional: parent.attachments[child].update_attachment() + child.attachments[parent].update_attachment() else: link_to_object = parent.attachments[child].child_to_parent_transform @@ -290,9 +314,17 @@ def attach_objects(self, """ # Add the attachment to the attachment dictionary of both objects - Attachment(parent_object, child_object, parent_link_id, child_link_id, bidirectional) + attachment = Attachment(parent_object, child_object, parent_link_id, child_link_id, bidirectional) + self.update_objects_attachments_collection(parent_object, child_object, attachment) self.attachment_event(parent_object, [parent_object, child_object]) + def update_objects_attachments_collection(self, + parent_object: Object, + child_object: Object, + attachment: Attachment) -> None: + parent_object.attachments[child_object] = attachment + child_object.attachments[parent_object] = attachment.get_inverse() + def add_fixed_constraint(self, parent_object_id: int, child_object_id: int, @@ -629,7 +661,7 @@ def get_object_AABB(self, obj: Object) -> Tuple[List[float], List[float]]: pass @abstractmethod - def get_object_link_AABB(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: + def get_object_link_aabb(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. @@ -827,7 +859,7 @@ def _copy(self) -> World: """ world = BulletWorld("DIRECT") for obj in self.objects: - o = Object(obj.name, obj.type, obj.path, Pose(obj.get_position(), obj.get_orientation()), + o = Object(obj.name, obj.obj_type, obj.path, Pose(obj.get_position(), obj.get_orientation()), world, obj.color) for joint in obj.joints: o.set_joint_position(joint, obj.get_joint_position(joint)) @@ -915,7 +947,7 @@ def update_transforms_for_objects_in_current_world(self) -> None: """ curr_time = rospy.Time.now() for obj in list(self.current_world.objects): - obj._update_link_transforms(curr_time) + obj.update_link_transforms(curr_time) class BulletWorld(World): @@ -925,25 +957,6 @@ class is the main interface to the Bullet Physics Engine and should be used to s manipulate the Bullet World. """ - current_world: BulletWorld = None - """ - Global reference to the currently used World, usually this is the - graphical one. However, if you are inside a Use_shadow_world() environment the current_world points to the - shadow world. In this way you can comfortably use the current_world, which should point towards the World - used at the moment. - """ - - robot: Object = None - """ - Global reference to the spawned Object that represents the robot. The robot is identified by checking the name - in the URDF with the name of the URDF on the parameter server. - """ - - data_directory: List[str] = {os.path.dirname(__file__) + "/../../resources"} - """ - Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. - """ - # Check is for sphinx autoAPI to be able to work in a CI workflow if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): rospy.init_node('pycram') @@ -998,7 +1011,7 @@ def add_constraint(self, constraint: Constraint) -> int: constraint.parent_link_id, constraint.child_obj_id, constraint.child_link_id, - constraint.joint_type, + constraint.joint_type.as_int(), constraint.joint_axis_in_child_link_frame, constraint.joint_frame_position_wrt_parent_origin, constraint.joint_frame_position_wrt_child_origin, @@ -1071,7 +1084,7 @@ def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List] return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] def get_object_contact_points(self, obj: Object) -> List: - """ + """l.update_transforms_for_object(self.milk) Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned list please look at `PyBullet Doc `_ @@ -1187,7 +1200,7 @@ def get_object_AABB(self, obj: Object) -> Tuple[List[float], List[float]]: """ return p.getAABB(obj.id, physicsClientId=self.client_id) - def get_object_link_AABB(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: + def get_object_link_aabb(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. @@ -1259,7 +1272,7 @@ def restore_physics_simulator_state(self, state_id): Restores the objects and environment state in the physics simulator according to the given state using the unique state id. """ - p.restoreState(state_id, self.client_id) + p.restoreState(state_id, physicsClientId=self.client_id) def add_vis_axis(self, pose: Pose, length: Optional[float] = 0.2) -> None: @@ -1312,27 +1325,23 @@ class UseProspectionWorld: NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() """ - def __init__(self, world_impl: Type[World]): - """ - :param world_impl: A class that implements a World (for example BulletWorld). - """ - self.world_impl = world_impl - self.prev_world: type(world_impl) = None + def __init__(self): + self.prev_world: World = None def __enter__(self): - if not self.world_impl.current_world.is_prospection_world: - time.sleep(20 * self.world_impl.simulation_time_step) + if not World.current_world.is_prospection_world: + time.sleep(20 * World.simulation_time_step) # blocks until the adding queue is ready - self.world_impl.current_world.world_sync.add_obj_queue.join() + World.current_world.world_sync.add_obj_queue.join() - self.prev_world = self.world_impl.current_world - self.world_impl.current_world.world_sync.pause_sync = True - self.world_impl.current_world = self.world_impl.current_world.prospection_world + self.prev_world = World.current_world + World.current_world.world_sync.pause_sync = True + World.current_world = World.current_world.prospection_world def __exit__(self, *args): if self.prev_world is not None: - self.world_impl.current_world = self.prev_world - self.world_impl.current_world.world_sync.pause_sync = False + World.current_world = self.prev_world + World.current_world.world_sync.pause_sync = False class WorldSync(threading.Thread): @@ -1643,34 +1652,35 @@ class Object: Represents a spawned Object in the World. """ - def __init__(self, name: str, type: Union[str, ObjectType], path: str, + # TODO: make a color dataclass or change initialization of color to be non mutable + def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, pose: Pose = None, world: World = None, color: Optional[List[float]] = [1, 1, 1, 1], - ignoreCachedFiles: Optional[bool] = False): + ignore_cached_files: Optional[bool] = False): """ The constructor loads the urdf file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. The color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. :param name: The name of the object - :param type: The type of the object + :param obj_type: The type of the object :param path: The path to the source file, if only a filename is provided then the resourcer directories will be searched :param pose: The pose at which the Object should be spawned :param world: The World in which the object should be spawned, if no world is specified the :py:attr:`~World.current_world` will be used :param color: The color with which the object should be spawned. - :param ignoreCachedFiles: If true the file will be spawned while ignoring cached files. + :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ if not pose: pose = Pose() self.world: World = world if world is not None else World.current_world self.local_transformer = LocalTransformer() self.name: str = name - self.type: str = type + self.obj_type: Union[str, ObjectType] = obj_type self.color: List[float] = color pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") position, orientation = pose_in_map.to_list() - self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignoreCachedFiles) + self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignore_cached_files) self.joints: Dict[str, int] = self._joint_or_link_name_to_id("joint") self.links: Dict[str, int] = self._joint_or_link_name_to_id("link") self.attachments: Dict[Object, Attachment] = {} @@ -1682,7 +1692,7 @@ def __init__(self, name: str, type: Union[str, ObjectType], path: str, # This means "world" is not the prospection world since it has a reference to a prospection world if self.world.prospection_world is not None: self.world.world_sync.add_obj_queue.put( - [name, type, path, position, orientation, self.world.prospection_world, color, self]) + [name, obj_type, path, position, orientation, self.world.prospection_world, color, self]) with open(self.path) as f: self.urdf_object = URDF.from_xml_string(f.read()) @@ -1696,14 +1706,21 @@ def __init__(self, name: str, type: Union[str, ObjectType], path: str, self._current_link_transforms: Dict[str, Transform] = {} self._current_joints_positions = {} self._init_current_positions_of_joint() - self._update_link_poses() + self._update_current_link_poses_and_transforms() self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) - self._update_link_transforms() - self.link_to_geometry = self._get_geometry_for_link() + self.update_link_transforms() + self.link_to_geometry = self.get_geometry_for_link() self.world.objects.append(self) + def _init_current_positions_of_joint(self) -> None: + """ + Initialize the cached joint position for each joint. + """ + for joint_name in self.joints.keys(): + self._current_joints_positions[joint_name] = self.world.get_object_joint_position(self, joint_name) + def __repr__(self): skip_attr = ["links", "joints", "urdf_object", "attachments", "cids", "_current_link_poses", "_current_link_transforms", "link_to_geometry"] @@ -1811,7 +1828,7 @@ def set_pose(self, pose: Pose, base: bool = False) -> None: position = np.array(position) + self.base_origin_shift self.world.reset_object_base_pose(self, position, orientation) self._current_pose = pose_in_map - self._update_link_poses() + self._update_current_link_poses_and_transforms() self.world._set_attached_objects(self, [self]) @property @@ -1957,21 +1974,28 @@ def get_joint_by_id(self, joint_id: int) -> str: """ return dict(zip(self.joints.values(), self.joints.keys()))[joint_id] - def get_link_pose_relative_to_other_link(self, child_link_name: str, parent_link_name: str) -> Pose: + def get_other_object_link_pose_relative_to_my_link(self, + my_link_id: int, + other_link_id: int, + other_object: Object) -> Pose: """ Calculates the pose of a link (child_link) in the coordinate frame of another link (parent_link). :return: The pose of the source frame in the target frame """ - child_link_pose = self.get_link_pose(child_link_name) - parent_link_frame = self.get_link_tf_frame(parent_link_name) + + child_link_pose = other_object.get_link_pose(other_object.get_link_by_id(other_link_id)) + parent_link_frame = self.get_link_tf_frame(self.get_link_by_id(my_link_id)) return self.local_transformer.transform_pose_to_target_frame(child_link_pose, parent_link_frame) - def get_transform_between_two_links(self, source_link_id: int, target_link_id: int) -> Transform: + def get_transform_of_other_object_link_relative_to_my_link(self, + my_link_id: int, + other_object: Object, + other_link_id: int) -> Transform: """ Calculates the transform from source link frame to target link frame. """ - source_tf_frame = self.get_link_tf_frame_by_id(source_link_id) - target_tf_frame = self.get_link_tf_frame_by_id(target_link_id) + source_tf_frame = self.get_link_tf_frame_by_id(my_link_id) + target_tf_frame = other_object.get_link_tf_frame_by_id(other_link_id) return self.local_transformer.lookup_transform_from_source_to_target_frame(source_tf_frame, target_tf_frame) def get_link_position(self, name: str) -> Pose.position: @@ -2026,8 +2050,7 @@ def set_joint_position(self, joint_name: str, joint_pose: float) -> None: # return self.world.reset_joint_position(self, joint_name, joint_pose) self._current_joints_positions[joint_name] = joint_pose - # self.local_transformer.update_transforms_for_object(self) - self._update_link_poses() + self._update_current_link_poses_and_transforms() self.world._set_attached_objects(self, [self]) def set_positions_of_all_joints(self, joint_poses: dict) -> None: @@ -2041,7 +2064,7 @@ def set_positions_of_all_joints(self, joint_poses: dict) -> None: for joint_name, joint_pose in joint_poses.items(): self.world.reset_joint_position(self, joint_name, joint_pose) self._current_joints_positions[joint_name] = joint_pose - self._update_link_poses() + self._update_current_link_poses_and_transforms() self.world._set_attached_objects(self, [self]) def get_joint_position(self, joint_name: str) -> float: @@ -2155,16 +2178,15 @@ def get_link_aabb(self, link_name: str) -> Tuple[List[float], List[float]]: :param link_name: The name of a link of this object. :return: Two lists of x,y,z which define the bounding box. """ - return self.world.get_object_link_AABB(self, link_name) + return self.world.get_object_link_aabb(self, link_name) - def get_base_origin(self, link_name: Optional[str] = None) -> Pose: + def get_base_origin(self) -> Pose: """ - Returns the origin of the base/bottom of an object/link + Returns the origin of the base/bottom of an object - :param link_name: The link name for which the bottom position should be returned - :return: The position of the bottom of this Object or link + :return: The position of the bottom of this Object """ - aabb = self.get_link_aabb(link_name=link_name) + aabb = self.get_link_aabb(link_name=self.urdf_object.get_root()) base_width = np.absolute(aabb[0][0] - aabb[1][0]) base_length = np.absolute(aabb[0][1] - aabb[1][1]) return Pose([aabb[0][0] + base_width / 2, aabb[0][1] + base_length / 2, aabb[0][2]], @@ -2243,7 +2265,7 @@ def get_link_tf_frame(self, link_name: str) -> str: def get_link_tf_frame_by_id(self, link_id: int) -> str: return self.get_link_tf_frame(self.get_link_by_id(link_id)) - def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.GeometricType]: + def get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.GeometricType]: """ Extracts the geometry information for each collision of each link and links them to the respective link. @@ -2258,28 +2280,28 @@ def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.GeometricType] link_to_geometry[link] = link_obj.collision.geometry return link_to_geometry - def _update_link_transforms(self, transform_time: Optional[rospy.Time] = None): + def update_link_transforms(self, transform_time: Optional[rospy.Time] = None): self.local_transformer.update_transforms(self._current_link_transforms.values(), transform_time) - def _update_link_poses(self) -> None: + def _update_current_link_poses_and_transforms(self) -> None: """ Updates the cached poses and transforms for each link of this Object """ for link_name in self.links.keys(): if link_name == self.urdf_object.get_root(): - self._current_link_poses[link_name] = self._current_pose - self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame) + self._update_root_link_pose_and_transform() else: - self._current_link_poses[link_name] = Pose(*self.world.get_object_link_pose(self, link_name)) - self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( - self.get_link_tf_frame(link_name)) + self._update_link_pose_and_transform(link_name) - def _init_current_positions_of_joint(self) -> None: - """ - Initialize the cached joint position for each joint. - """ - for joint_name in self.joints.keys(): - self._current_joints_positions[joint_name] = self.world.get_object_joint_position(self, joint_name) + def _update_root_link_pose_and_transform(self) -> None: + link_name = self.urdf_object.get_root() + self._current_link_poses[link_name] = self._current_pose + self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame) + + def _update_link_pose_and_transform(self, link_name: str) -> None: + self._current_link_poses[link_name] = Pose(*self.world.get_object_link_pose(self, link_name)) + self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( + self.get_link_tf_frame(link_name)) def filter_contact_points(contact_points, exclude_ids) -> List: diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index a849f8a10..369760991 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -98,7 +98,7 @@ def stable(object: Object, """ world, world_id = _world_and_id(world) shadow_obj = BulletWorld.current_world.get_prospection_object(object) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): coords_prev = shadow_obj.pose.position_as_list() state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) p.setGravity(0, 0, -9.8, BulletWorld.current_world.client_id) @@ -128,7 +128,7 @@ def contact(object1: Object, :return: True if the two objects are in contact False else. If links should be returned a list of links in contact """ - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): shadow_obj1 = BulletWorld.current_world.get_prospection_object(object1) shadow_obj2 = BulletWorld.current_world.get_prospection_object(object2) p.performCollisionDetection(BulletWorld.current_world.client_id) @@ -163,7 +163,7 @@ def visible(object: Object, :return: True if the object is visible from the camera_position False if not """ front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): shadow_obj = BulletWorld.current_world.get_prospection_object(object) if BulletWorld.robot: shadow_robot = BulletWorld.current_world.get_prospection_object(BulletWorld.robot) @@ -215,7 +215,7 @@ def occluding(object: Object, world, world_id = _world_and_id(world) # occ_world = world.copy() # state = p.saveState(physicsClientId=occ_world.client_id) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) for obj in BulletWorld.current_world.objects: if obj.name == BulletWorld.robot.name: @@ -268,7 +268,7 @@ def reachable(pose: Union[Object, Pose], pose = pose.get_pose() shadow_robot = BulletWorld.current_world.get_prospection_object(robot) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" joints = robot_description.chains[arm].joints try: @@ -306,7 +306,7 @@ def blocking(pose_or_object: Union[Object, Pose], input_pose = pose_or_object shadow_robot = BulletWorld.current_world.get_prospection_object(robot) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" joints = robot_description.chains[arm].joints local_transformer = LocalTransformer() @@ -358,7 +358,7 @@ def link_pose_for_joint_config(object: Object, joint_config: Dict[str, float], l :return: The pose of the link after applying the joint configuration """ shadow_object = BulletWorld.current_world.get_prospection_object(object) - with UseProspectionWorld(BulletWorld): + with UseProspectionWorld(): for joint, pose in joint_config.items(): shadow_object.set_joint_position(joint, pose) return shadow_object.get_link_pose(link_name) diff --git a/test/local_transformer_tests.py b/test/local_transformer_tests.py index b3f3b8bd1..d39296395 100644 --- a/test/local_transformer_tests.py +++ b/test/local_transformer_tests.py @@ -1,11 +1,7 @@ -import unittest import rospy -from time import sleep -from src.pycram.world import BulletWorld, Object -from src.pycram.local_transformer import LocalTransformer -from pycram.robot_descriptions import robot_description +from pycram.local_transformer import LocalTransformer from pycram.pose import Pose, Transform import test_bullet_world @@ -44,7 +40,7 @@ def test_transform_pose_position(self): def test_update_for_object(self): l = LocalTransformer() self.milk.set_pose(Pose([1, 2, 1])) - l.update_transforms_for_object(self.milk) + self.milk.update_link_transforms() self.assertTrue(l.canTransform("map", self.milk.tf_frame, rospy.Time(0))) diff --git a/test/test_action_designator.py b/test/test_action_designator.py index e196bb51e..e5bf75c46 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -87,14 +87,13 @@ def test_look_at(self): def test_detect(self): self.kitchen.set_pose(Pose([10, 10, 0])) self.milk.set_pose(Pose([1.5, 0, 1.2])) - # time.sleep(1) object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.DetectAction(object_description) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: detected_object = description.resolve().perform() self.assertEqual(detected_object.name, "milk") - self.assertEqual(detected_object.mode, ObjectType.MILK) + self.assertEqual(detected_object.obj_type, ObjectType.MILK) self.assertEqual(detected_object.bullet_world_object, self.milk) # Skipped since open and close work only in the apartment at the moment diff --git a/test/test_object_designator.py b/test/test_object_designator.py index fa2079e5b..ae7ff0c52 100644 --- a/test/test_object_designator.py +++ b/test/test_object_designator.py @@ -11,7 +11,7 @@ def test_object_grounding(self): obj = description.ground() self.assertEqual(obj.name, "milk") - self.assertEqual(obj.type, ObjectType.MILK) + self.assertEqual(obj.obj_type, ObjectType.MILK) def test_data_copy(self): description = ObjectDesignatorDescription(["milk"], [ObjectType.MILK]) From 534390d3d1f6d2d373729b42372b57796c290c47 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Dec 2023 17:04:47 +0100 Subject: [PATCH 14/69] [AbstractWorld] some cleaning up. --- src/pycram/local_transformer.py | 2 ++ src/pycram/world.py | 19 +++++++------------ 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 05d149277..7ed2c3fd7 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -78,6 +78,8 @@ def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union return Pose(*copy_pose.to_list(), frame=new_pose.header.frame_id) + # TODO: This is not working correctly, the lookup fails to find the frames, + # this is probably because the frames are not published. def lookup_transform_from_source_to_target_frame(self, source_frame: str, target_frame: str, time: Optional[rospy.rostime.Time] = None) -> Transform: """ diff --git a/src/pycram/world.py b/src/pycram/world.py index 413a44492..0b234aae1 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -74,10 +74,9 @@ def update_constraint(self): self.add_constraint_and_update_objects_constraints_collection() def calculate_transform(self): - relative_pose = self.parent_object.get_other_object_link_pose_relative_to_my_link(self.parent_link_id, - self.child_link_id, - self.child_object) - return relative_pose.to_transform(self.child_object.get_link_tf_frame_by_id(self.child_link_id)) + return self.parent_object.get_transform_of_other_object_link_relative_to_my_link(self.parent_link_id, + self.child_link_id, + self.child_object) def remove_constraint_if_exists(self): if self.constraint_id is not None: @@ -1989,14 +1988,10 @@ def get_other_object_link_pose_relative_to_my_link(self, def get_transform_of_other_object_link_relative_to_my_link(self, my_link_id: int, - other_object: Object, - other_link_id: int) -> Transform: - """ - Calculates the transform from source link frame to target link frame. - """ - source_tf_frame = self.get_link_tf_frame_by_id(my_link_id) - target_tf_frame = other_object.get_link_tf_frame_by_id(other_link_id) - return self.local_transformer.lookup_transform_from_source_to_target_frame(source_tf_frame, target_tf_frame) + other_link_id: int, + other_object: Object) -> Transform: + pose = self.get_other_object_link_pose_relative_to_my_link(my_link_id, other_link_id, other_object) + return pose.to_transform(other_object.get_link_tf_frame_by_id(other_link_id)) def get_link_position(self, name: str) -> Pose.position: """ From e2e2f12045bd5576765397ff8f12346909bd89f5 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Dec 2023 17:58:48 +0100 Subject: [PATCH 15/69] [AbstractWorld] Moved BulletWorld and GUI classes to bullet_world.py, removed pybullet dependency from world.py, tests are running except test_attachment_exclusion in test_costmaps.py --- demos/pycram_bullet_world_demo/demo.py | 2 +- demos/pycram_ur5_demo/demo.py | 3 +- doc/source/notebooks/bullet_world.ipynb | 2 +- src/pycram/bullet_world.py | 593 +++++++++++++ src/pycram/costmaps.py | 3 +- src/pycram/designator.py | 3 +- src/pycram/designators/action_designator.py | 2 +- src/pycram/designators/location_designator.py | 3 +- src/pycram/designators/motion_designator.py | 3 +- src/pycram/designators/object_designator.py | 3 +- src/pycram/external_interfaces/giskard.py | 3 +- src/pycram/external_interfaces/robokudo.py | 2 +- src/pycram/pose_generator_and_validator.py | 3 +- .../process_modules/boxy_process_modules.py | 2 +- .../process_modules/donbot_process_modules.py | 2 +- .../process_modules/hsr_process_modules.py | 2 +- .../process_modules/pr2_process_modules.py | 2 +- src/pycram/resolver/location/jpt_location.py | 3 +- src/pycram/ros/force_torque_sensor.py | 2 +- src/pycram/ros/joint_state_publisher.py | 2 +- src/pycram/ros/robot_state_updater.py | 2 +- src/pycram/ros/tf_broadcaster.py | 2 +- src/pycram/ros/viz_marker_publisher.py | 2 +- src/pycram/task.py | 2 +- src/pycram/world.py | 836 +++--------------- src/pycram/world_reasoning.py | 3 +- test/test_bullet_world.py | 3 +- test/test_database_resolver.py | 3 +- test/test_jpt_resolver.py | 3 +- 29 files changed, 761 insertions(+), 735 deletions(-) create mode 100755 src/pycram/bullet_world.py diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 0216c5474..c19ad349c 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -2,7 +2,7 @@ from pycram.designators.location_designator import * from pycram.designators.object_designator import * from pycram.pose import Pose -from pycram.world import BulletWorld, Object +from pycram.bullet_world import BulletWorld, Object from pycram.process_module import simulated_robot, with_simulated_robot from pycram.enums import ObjectType diff --git a/demos/pycram_ur5_demo/demo.py b/demos/pycram_ur5_demo/demo.py index 2fc87dc42..f8fe23198 100644 --- a/demos/pycram_ur5_demo/demo.py +++ b/demos/pycram_ur5_demo/demo.py @@ -5,7 +5,8 @@ import pybullet as pb from pycram import robot_description -from pycram.world import BulletWorld, Object +from pycram.bullet_world import BulletWorld +from pycram.world import Object from pycram.pose import Pose from pycram.ros.force_torque_sensor import ForceTorqueSensor from pycram.ros.joint_state_publisher import JointStatePublisher diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb index cf736fd77..538c2a475 100644 --- a/doc/source/notebooks/bullet_world.ipynb +++ b/doc/source/notebooks/bullet_world.ipynb @@ -41,7 +41,7 @@ } ], "source": [ - "from pycram.world import BulletWorld\n", + "from pycram.bullet_world import BulletWorld\n", "from pycram.pose import Pose\n", "from pycram.enums import ObjectType\n", "\n", diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py new file mode 100755 index 000000000..a60bcedb3 --- /dev/null +++ b/src/pycram/bullet_world.py @@ -0,0 +1,593 @@ +# used for delayed evaluation of typing until python 3.11 becomes mainstream +from __future__ import annotations + +import threading +import time +from typing import List, Optional, Dict, Tuple + +import numpy as np +import pybullet as p +import rospy +import rosgraph + +from .enums import JointType, ObjectType +from .pose import Pose +from .world import World, Object, Constraint + + +class BulletWorld(World): + """ + This class represents a BulletWorld, which is a simulation environment that uses the Bullet Physics Engine. This + class is the main interface to the Bullet Physics Engine and should be used to spawn Objects, simulate Physic and + manipulate the Bullet World. + """ + + # Check is for sphinx autoAPI to be able to work in a CI workflow + if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): + rospy.init_node('pycram') + + def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): + """ + Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the + background. There can only be one rendered simulation. + The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. + + :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" + :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. + """ + super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_time_step=0.004167) # 240 Hz + + self._gui_thread: Gui = Gui(self, mode) + self._gui_thread.start() + + # This disables file caching from PyBullet, since this would also cache + # files that can not be loaded + p.setPhysicsEngineParameter(enableFileCaching=0) + + # Needed to let the other thread start the simulation, before Objects are spawned. + time.sleep(0.1) + self.vis_axis: List[Object] = [] + + # Some default settings + self.set_gravity([0, 0, -9.8]) + + if not is_prospection_world: + plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) + + def load_urdf_at_pose_and_get_object_id(self, path: str, pose: Pose) -> int: + return p.loadURDF(path, + basePosition=pose.position_as_list(), + baseOrientation=pose.orientation_as_list(), physicsClientId=self.client_id) + + def remove_object(self, obj_id: int) -> None: + """ + Remove an object by its ID. + + :param obj_id: The unique id of the object to be removed. + """ + + p.removeBody(obj_id, self.client_id) + + def add_constraint(self, constraint: Constraint) -> int: + """ + Add a constraint between two objects so that attachment they become attached + """ + constraint_id = p.createConstraint(constraint.parent_obj_id, + constraint.parent_link_id, + constraint.child_obj_id, + constraint.child_link_id, + constraint.joint_type.as_int(), + constraint.joint_axis_in_child_link_frame, + constraint.joint_frame_position_wrt_parent_origin, + constraint.joint_frame_position_wrt_child_origin, + constraint.joint_frame_orientation_wrt_parent_origin, + constraint.joint_frame_orientation_wrt_child_origin, + physicsClientId=self.client_id) + return constraint_id + + def remove_constraint(self, constraint_id): + p.removeConstraint(constraint_id, physicsClientId=self.client_id) + + def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: + """ + Get the joint upper limit of an articulated object + + :param obj: The object. + :param joint_name: The name of the joint. + :return: The joint upper limit as a float. + """ + return p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8] + + def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: + """ + Get the joint lower limit of an articulated object + + :param obj: The object. + :param joint_name: The name of the joint. + :return: The joint lower limit as a float. + """ + return p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[9] + + def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: + """ + Returns the axis along which a joint is moving. The given joint_name has to be part of this object. + + :param obj: The object + :param joint_name: Name of the joint for which the axis should be returned. + :return: The axis a vector of xyz + """ + return p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[13] + + def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: + """ + Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. + + :param obj: The object + :param joint_name: Joint name for which the type should be returned + :return: The type of the joint + """ + joint_type = p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[2] + return JointType(joint_type) + + def get_object_joint_position(self, obj: Object, joint_name: str) -> float: + """ + Get the state of a joint of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + """ + return p.getJointState(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[0] + + def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: + """ + Get the pose of a link of an articulated object with respect to the world frame. + The pose is given as a tuple of position and orientation. + + :param obj: The object + :param link_name: The name of the link + """ + return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] + + def get_object_contact_points(self, obj: Object) -> List: + """l.update_transforms_for_object(self.milk) + Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned + list please look at `PyBullet Doc `_ + + :param obj: The object. + :return: A list of all contact points with other objects + """ + return p.getContactPoints(obj.id) + + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + """ + Returns a list of contact points between obj1 and obj2. + + :param obj1: The first object. + :param obj2: The second object. + :return: A list of all contact points between the two objects. + """ + return p.getContactPoints(obj1.id, obj2.id) + + def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: + """ + Get the ID of a joint in an articulated object. + + :param obj: The object + :param joint_idx: The index of the joint (would indicate order). + """ + return p.getJointInfo(obj.id, joint_idx, self.client_id)[0] + + def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: + """ + Get the name of a joint in an articulated object. + + :param obj: The object + :param joint_idx: The index of the joint (would indicate order). + """ + return p.getJointInfo(obj.id, joint_idx, self.client_id)[1].decode('utf-8') + + def get_object_link_name(self, obj: Object, link_idx: int) -> str: + """ + Get the name of a link in an articulated object. + + :param obj: The object + :param link_idx: The index of the link (would indicate order). + """ + return p.getJointInfo(obj.id, link_idx, self.client_id)[12].decode('utf-8') + + def get_object_number_of_joints(self, obj: Object) -> int: + """ + Get the number of joints of an articulated object + + :param obj: The object + """ + return p.getNumJoints(obj.id, self.client_id) + + def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + """ + Reset the joint position instantly without physics simulation + + :param obj: The object + :param joint_name: The name of the joint + :param joint_pose: The new joint pose + """ + p.resetJointState(obj.id, obj.joints[joint_name], joint_pose, physicsClientId=self.client_id) + + def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): + """ + 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 position: The new position of the object as a vector of x,y,z + :param orientation: The new orientation of the object as a quaternion of x,y,z,w + """ + p.resetBasePositionAndOrientation(obj.id, position, orientation, self.client_id) + + def step(self): + """ + Step the world simulation using forward dynamics + """ + p.stepSimulation(self.client_id) + + def set_object_link_color(self, obj: Object, link_id: int, rgba_color: List[float]): + """ + Changes the color of a link of this object, the color has to be given as a 4 element list + of RGBA values. + + :param obj: The object which should be colored + :param link_id: The link id of the link which should be colored + :param rgba_color: The color as RGBA values between 0 and 1 + """ + p.changeVisualShape(obj.id, link_id, rgbaColor=rgba_color, physicsClientId=self.client_id) + + def get_object_colors(self, obj: Object) -> Dict[str, List[float]]: + """ + Get the RGBA colors of each link in the object as a dictionary from link name to color. + + :param obj: The object + :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. + """ + visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) + swap = {v: k for k, v in obj.links.items()} + links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) + colors = list(map(lambda x: x[7], visual_data)) + link_to_color = dict(zip(links, colors)) + return link_to_color + + def get_object_AABB(self, obj: Object) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of this object. The return of this method are two points in + world coordinate frame which define a bounding box. + + :param obj: The object for which the bounding box should be returned. + :return: Two lists of x,y,z which define the bounding box. + """ + return p.getAABB(obj.id, physicsClientId=self.client_id) + + def get_object_link_aabb(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: + """ + Returns the axis aligned bounding box of the link. The return of this method are two points in + world coordinate frame which define a bounding box. + + :param obj: The object for which the bounding box should be returned. + :param link_name: The name of a link of this object. + :return: Two lists of x,y,z which define the bounding box. + """ + return p.getAABB(obj.id, obj.links[link_name], self.client_id) + + def set_realtime(self, real_time: bool) -> None: + """ + Enables the real time simulation of Physic in the BulletWorld. By default, this is disabled and Physics is only + simulated to reason about it. + + :param real_time: Whether the World should simulate Physics in real time. + """ + p.setRealTimeSimulation(1 if real_time else 0, self.client_id) + + def set_gravity(self, gravity_vector: List[float]) -> None: + """ + Sets the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). + Gravity is given as a vector in x,y,z. Gravity is only applied while simulating Physic. + + :param gravity_vector: The gravity vector that should be used in the World. + """ + p.setGravity(gravity_vector[0], gravity_vector[1], gravity_vector[2], physicsClientId=self.client_id) + + @classmethod + def set_robot(cls, robot: Object) -> None: + """ + Sets the global variable for the robot Object. This should be set on spawning the robot. + + :param robot: The Object reference to the Object representing the robot. + """ + cls.robot = robot + + def exit(self, wait_time_before_exit_in_secs: Optional[float] = 0.1) -> None: + """ + Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the + preferred method to close the BulletWorld. + """ + super().exit(wait_time_before_exit_in_secs) + + def disconnect_from_physics_server(self): + """ + Disconnects the world from the physics server. + """ + p.disconnect(self.client_id) + + def join_threads(self): + """ + Join any running threads. Useful for example when exiting the world. + """ + self.join_gui_thread_if_exists() + + def join_gui_thread_if_exists(self): + if self._gui_thread: + self._gui_thread.join() + + def save_physics_simulator_state(self) -> int: + """ + Saves the state of the physics simulator and returns the unique id of the state. + """ + return p.saveState(self.client_id) + + def restore_physics_simulator_state(self, state_id): + """ + Restores the objects and environment state in the physics simulator according to + the given state using the unique state id. + """ + p.restoreState(state_id, physicsClientId=self.client_id) + + def add_vis_axis(self, pose: Pose, + length: Optional[float] = 0.2) -> None: + """ + Creates a Visual object which represents the coordinate frame at the given + position and orientation. There can be an unlimited amount of vis axis objects. + + :param pose: The pose at which the axis should be spawned + :param length: Optional parameter to configure the length of the axes + """ + + position, orientation = pose.to_list() + + vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], + rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) + vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], + rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) + vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], + rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) + + obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], + basePosition=position, baseOrientation=orientation, + linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], + linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], + linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], + linkParentIndices=[0, 0, 0], + linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], + linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], + linkCollisionShapeIndices=[-1, -1, -1]) + + self.vis_axis.append(obj) + + def remove_vis_axis(self) -> None: + """ + Removes all spawned vis axis objects that are currently in this BulletWorld. + """ + for id in self.vis_axis: + p.removeBody(id) + self.vis_axis = [] + + +class Gui(threading.Thread): + """ + For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~World.exit` + Also contains the code for controlling the camera. + """ + + def __init__(self, world: World, mode: str): + threading.Thread.__init__(self) + self.world = world + self.mode: str = mode + + def run(self): + """ + Initializes the new simulation and checks in an endless loop + if it is still active. If it is the thread will be suspended for 1/80 seconds, if it is not the method and + thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. + """ + if self.mode != "GUI": + self.world.client_id = p.connect(p.DIRECT) + else: + self.world.client_id = p.connect(p.GUI) + + # Disable the side windows of the GUI + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + # Change the init camera pose + p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, + cameraTargetPosition=[-2, 0, 1]) + + # Get the initial camera target location + cameraTargetPosition = p.getDebugVisualizerCamera()[11] + + sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1]) + + # Create a sphere with a radius of 0.05 and a mass of 0 + sphereUid = p.createMultiBody(baseMass=0.0, + baseInertialFramePosition=[0, 0, 0], + baseVisualShapeIndex=sphereVisualId, + basePosition=cameraTargetPosition) + + # Define the maxSpeed, used in calculations + maxSpeed = 16 + + # Set initial Camera Rotation + cameraYaw = 50 + cameraPitch = -35 + + # Keep track of the mouse state + mouseState = [0, 0, 0] + oldMouseX, oldMouseY = 0, 0 + + # Determines if the sphere at cameraTargetPosition is visible + visible = 1 + + # Loop to update the camera position based on keyboard events + while p.isConnected(self.world.client_id): + # Monitor user input + keys = p.getKeyboardEvents() + mouse = p.getMouseEvents() + + # Get infos about the camera + width, height, dist = p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], \ + p.getDebugVisualizerCamera()[10] + cameraTargetPosition = p.getDebugVisualizerCamera()[11] + + # Get vectors used for movement on x,y,z Vector + xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] + yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] + zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] + + # Check the mouse state + if mouse: + for m in mouse: + + mouseX = m[2] + mouseY = m[1] + + # update mouseState + # Left Mouse button + if m[0] == 2 and m[3] == 0: + mouseState[0] = m[4] + # Middle mouse butto (scroll wheel) + if m[0] == 2 and m[3] == 1: + mouseState[1] = m[4] + # right mouse button + if m[0] == 2 and m[3] == 2: + mouseState[2] = m[4] + + # change visibility by clicking the mousewheel + if m[4] == 6 and m[3] == 1 and visible == 1: + visible = 0 + elif m[4] == 6 and visible == 0: + visible = 1 + + # camera movement when the left mouse button is pressed + if mouseState[0] == 3: + speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < maxSpeed else maxSpeed + speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < maxSpeed else maxSpeed + + # max angle of 89.5 and -89.5 to make sure the camera does not flip (is annoying) + if mouseX < oldMouseX: + if (cameraPitch + speedX) < 89.5: + cameraPitch += (speedX / 4) + 1 + elif mouseX > oldMouseX: + if (cameraPitch - speedX) > -89.5: + cameraPitch -= (speedX / 4) + 1 + + if mouseY < oldMouseY: + cameraYaw += (speedY / 4) + 1 + elif mouseY > oldMouseY: + cameraYaw -= (speedY / 4) + 1 + + if mouseState[1] == 3: + speedX = abs(oldMouseX - mouseX) + factor = 0.05 + + if mouseX < oldMouseX: + dist = dist - speedX * factor + elif mouseX > oldMouseX: + dist = dist + speedX * factor + dist = max(dist, 0.1) + + # camera movement when the right mouse button is pressed + if mouseState[2] == 3: + speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < 5 else 5 + speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < 5 else 5 + factor = 0.05 + + if mouseX < oldMouseX: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(zVec, factor), speedX)) + elif mouseX > oldMouseX: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(zVec, factor), speedX)) + + if mouseY < oldMouseY: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(xVec, factor), speedY)) + elif mouseY > oldMouseY: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(xVec, factor), speedY)) + # update oldMouse values + oldMouseY, oldMouseX = mouseY, mouseX + + # check the keyboard state + if keys: + # if shift is pressed, double the speed + if p.B3G_SHIFT in keys: + speedMult = 5 + else: + speedMult = 2.5 + + # if control is pressed, the movements caused by the arrowkeys, the '+' as well as the '-' key + # change + if p.B3G_CONTROL in keys: + + # the up and down arrowkeys cause the targetPos to move along the z axis of the map + if p.B3G_DOWN_ARROW in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(zVec, 0.03), speedMult)) + elif p.B3G_UP_ARROW in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(zVec, 0.03), speedMult)) + + # left and right arrowkeys cause the targetPos to move horizontally relative to the camera + if p.B3G_LEFT_ARROW in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(xVec, 0.03), speedMult)) + elif p.B3G_RIGHT_ARROW in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(xVec, 0.03), speedMult)) + + # the '+' and '-' keys cause the targetpos to move forwards and backwards relative to the camera + # while the camera stays at a constant distance. SHIFT + '=' is for US layout + if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: + cameraTargetPosition = np.subtract(cameraTargetPosition, + np.multiply(np.multiply(yVec, 0.03), speedMult)) + elif ord("-") in keys: + cameraTargetPosition = np.add(cameraTargetPosition, + np.multiply(np.multiply(yVec, 0.03), speedMult)) + + # standard bindings for thearrowkeys, the '+' as well as the '-' key + else: + + # left and right arrowkeys cause the camera to rotate around the yaw axis + if p.B3G_RIGHT_ARROW in keys: + cameraYaw += (360 / width) * speedMult + elif p.B3G_LEFT_ARROW in keys: + cameraYaw -= (360 / width) * speedMult + + # the up and down arrowkeys cause the camera to rotate around the pitch axis + if p.B3G_DOWN_ARROW in keys: + if (cameraPitch + (360 / height) * speedMult) < 89.5: + cameraPitch += (360 / height) * speedMult + elif p.B3G_UP_ARROW in keys: + if (cameraPitch - (360 / height) * speedMult) > -89.5: + cameraPitch -= (360 / height) * speedMult + + # the '+' and '-' keys cause the camera to zoom towards and away from the targetPos without + # moving it. SHIFT + '=' is for US layout since the events can't handle shift plus something + if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: + if (dist - (dist * 0.02) * speedMult) > 0.1: + dist -= dist * 0.02 * speedMult + elif ord("-") in keys: + dist += dist * 0.02 * speedMult + + p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, + cameraTargetPosition=cameraTargetPosition) + if visible == 0: + cameraTargetPosition = (0.0, -50, 50) + p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) + time.sleep(1. / 80.) \ No newline at end of file diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 080d31c71..85579203c 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -8,7 +8,8 @@ from matplotlib import colors import psutil import time -from .world import BulletWorld, UseProspectionWorld, Object +from .bullet_world import BulletWorld +from pycram.world import UseProspectionWorld, Object from .world_reasoning import _get_images_for_target from nav_msgs.msg import OccupancyGrid, MapMetaData from typing import Tuple, List, Union, Optional diff --git a/src/pycram/designator.py b/src/pycram/designator.py index d7c7a670d..017008d05 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -9,7 +9,8 @@ from sqlalchemy.orm.session import Session import rospy -from .world import (Object as BulletWorldObject, BulletWorld) +from .world import Object as BulletWorldObject +from pycram.bullet_world import BulletWorld from .helper import GeneratorList, bcolors from threading import Lock from time import time diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index c44f496ab..deeb06d68 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -23,7 +23,7 @@ from ..task import with_tree from ..enums import Arms from ..designator import ActionDesignatorDescription -from ..world import BulletWorld +from ..bullet_world import BulletWorld from ..pose import Pose from ..helper import multiply_quaternions diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index aae10e4b7..1a95ec346 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -3,7 +3,8 @@ from typing import List, Tuple, Union, Iterable, Optional, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart -from ..world import Object, BulletWorld, UseProspectionWorld +from ..world import Object, UseProspectionWorld +from ..bullet_world import BulletWorld from ..world_reasoning import link_pose_for_joint_config from ..designator import Designator, DesignatorError, LocationDesignatorDescription from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index f148aee18..c602227c2 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -2,7 +2,8 @@ from sqlalchemy.orm import Session from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject -from ..world import Object, BulletWorld +from ..world import Object +from pycram.bullet_world import BulletWorld from ..designator import DesignatorError from ..plan_failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 8d87c4936..8f5f5200c 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,7 +1,8 @@ import dataclasses from typing import List, Union, Optional, Callable, Tuple, Iterable import sqlalchemy.orm -from ..world import BulletWorld, Object as BulletWorldObject +from ..bullet_world import BulletWorld +from pycram.world import Object as BulletWorldObject from ..designator import DesignatorDescription, ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 7c4c5f4c6..f556a7ab0 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -6,7 +6,8 @@ from ..pose import Pose from ..robot_descriptions import robot_description -from ..world import BulletWorld, Object +from ..bullet_world import BulletWorld +from pycram.world import Object from ..robot_description import ManipulatorDescription from typing import List, Tuple, Dict, Callable diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index 9e0164978..bbfcadd50 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -9,7 +9,7 @@ from ..designator import ObjectDesignatorDescription from ..pose import Pose from ..local_transformer import LocalTransformer -from ..world import BulletWorld +from ..bullet_world import BulletWorld from ..enums import ObjectType try: diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index ae3f9994d..5ae4b0b9d 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -3,7 +3,8 @@ import rospy import pybullet as p -from .world import Object, BulletWorld, UseProspectionWorld +from .world import Object +from .bullet_world import BulletWorld from .world_reasoning import contact from .costmaps import Costmap from .pose import Pose, Transform diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 41c93f0fe..2e8fc74f3 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -5,7 +5,7 @@ import pycram.world_reasoning as btr import pycram.helper as helper -from ..world import BulletWorld +from ..bullet_world import BulletWorld from ..external_interfaces.ik import request_ik from ..local_transformer import LocalTransformer as local_tf from ..process_module import ProcessModule, ProcessModuleManager diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 1d1320cfd..dfc5dc7ed 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -5,7 +5,7 @@ import pycram.world_reasoning as btr import pycram.helper as helper -from ..world import BulletWorld +from ..bullet_world import BulletWorld from ..local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 62120af7c..974ef8ac3 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -2,7 +2,7 @@ from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..world import BulletWorld +from ..bullet_world import BulletWorld from ..helper import _apply_ik import pycram.world_reasoning as btr import pybullet as p diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index bcadb46ec..253d37df1 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -13,7 +13,7 @@ from ..plan_failures import EnvironmentManipulationImpossible from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..world import BulletWorld, Object +from ..bullet_world import BulletWorld, Object from ..helper import transform from ..external_interfaces.ik import request_ik, IKError from ..helper import _transform_to_torso, _apply_ik, calculate_wrist_tool_offset, inverseTimes diff --git a/src/pycram/resolver/location/jpt_location.py b/src/pycram/resolver/location/jpt_location.py index 748f559bb..b9de0ccb2 100644 --- a/src/pycram/resolver/location/jpt_location.py +++ b/src/pycram/resolver/location/jpt_location.py @@ -12,7 +12,8 @@ from ...costmaps import OccupancyCostmap, plot_grid from ...plan_failures import PlanFailure from ...pose import Pose -from pycram.world import BulletWorld, Object +from pycram.bullet_world import BulletWorld +from pycram.world import Object class JPTCostmapLocation(pycram.designators.location_designator.CostmapLocation): diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros/force_torque_sensor.py index c67c2bca2..74b48024f 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros/force_torque_sensor.py @@ -7,7 +7,7 @@ from geometry_msgs.msg import WrenchStamped from std_msgs.msg import Header -from..world import BulletWorld +from..bullet_world import BulletWorld class ForceTorqueSensor: diff --git a/src/pycram/ros/joint_state_publisher.py b/src/pycram/ros/joint_state_publisher.py index 4f72e3a61..efc51334d 100644 --- a/src/pycram/ros/joint_state_publisher.py +++ b/src/pycram/ros/joint_state_publisher.py @@ -6,7 +6,7 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header -from ..world import BulletWorld +from ..bullet_world import BulletWorld class JointStatePublisher: diff --git a/src/pycram/ros/robot_state_updater.py b/src/pycram/ros/robot_state_updater.py index 845d20351..f8f8bab82 100644 --- a/src/pycram/ros/robot_state_updater.py +++ b/src/pycram/ros/robot_state_updater.py @@ -6,7 +6,7 @@ from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState -from pycram.world import BulletWorld +from pycram.bullet_world import BulletWorld from pycram.robot_descriptions import robot_description from pycram.pose import Transform, Pose diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index 907119275..6880949f6 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -4,7 +4,7 @@ import atexit from ..pose import Pose -from ..world import BulletWorld +from ..bullet_world import BulletWorld from tf2_msgs.msg import TFMessage diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index f4a815df3..83193c80e 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -5,7 +5,7 @@ from geometry_msgs.msg import Vector3 from std_msgs.msg import ColorRGBA -from pycram.world import BulletWorld, Object +from pycram.bullet_world import BulletWorld from visualization_msgs.msg import MarkerArray, Marker import rospy import urdf_parser_py diff --git a/src/pycram/task.py b/src/pycram/task.py index aa21f41a0..92b80628d 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -14,7 +14,7 @@ import sqlalchemy.orm.session import tqdm -from .world import BulletWorld +from .bullet_world import BulletWorld from .orm.task import (Code as ORMCode, TaskTreeNode as ORMTaskTreeNode) from .orm.base import ProcessMetaData from .plan_failures import PlanFailure diff --git a/src/pycram/world.py b/src/pycram/world.py index 0b234aae1..d87a82349 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -10,14 +10,12 @@ import xml.etree.ElementTree from queue import Queue import tf -from typing import List, Optional, Dict, Tuple, Callable, Set, Type, TypeVar +from typing import List, Optional, Dict, Tuple, Callable from typing import Union import numpy as np -import pybullet as p import rospkg import rospy -import rosgraph import urdf_parser_py.urdf from geometry_msgs.msg import Quaternion, Point @@ -31,120 +29,10 @@ from .pose import Pose, Transform -from abc import ABC, abstractproperty, abstractmethod +from abc import ABC, abstractmethod from dataclasses import dataclass -class Attachment: - def __init__(self, - parent_object: Object, - child_object: Object, - parent_link_id: Optional[int] = -1, # -1 means base link - child_link_id: Optional[int] = -1, - bidirectional: Optional[bool] = False, - child_to_parent_transform: Optional[Transform] = None, - constraint_id: Optional[int] = None): - """ - Creates an attachment between the parent object and the child object. - """ - self.parent_object = parent_object - self.child_object = child_object - self.parent_link_id = parent_link_id - self.child_link_id = child_link_id - self.bidirectional = bidirectional - self._loose = False and not self.bidirectional - - self.child_to_parent_transform = child_to_parent_transform - if self.child_to_parent_transform is None: - self.update_transform() - - self.constraint_id = constraint_id - if self.constraint_id is None: - self.add_constraint_and_update_objects_constraints_collection() - - def update_attachment(self): - self.update_transform() - self.update_constraint() - - def update_transform(self): - self.child_to_parent_transform = self.calculate_transform() - - def update_constraint(self): - self.remove_constraint_if_exists() - self.add_constraint_and_update_objects_constraints_collection() - - def calculate_transform(self): - return self.parent_object.get_transform_of_other_object_link_relative_to_my_link(self.parent_link_id, - self.child_link_id, - self.child_object) - - def remove_constraint_if_exists(self): - if self.constraint_id is not None: - self.parent_object.world.remove_constraint(self.constraint_id) - - def add_constraint_and_update_objects_constraints_collection(self): - self.constraint_id = self.add_fixed_constraint() - self.update_objects_constraints_collection() - - def add_fixed_constraint(self): - constraint_id = self.parent_object.world.add_fixed_constraint(self.parent_object.id, - self.child_object.id, - self.child_to_parent_transform, - self.parent_link_id, - self.child_link_id) - return constraint_id - - def update_objects_constraints_collection(self): - self.parent_object.cids[self.child_object] = self.constraint_id - self.child_object.cids[self.parent_object] = self.constraint_id - - def get_inverse(self): - attachment = Attachment(self.child_object, self.parent_object, self.child_link_id, - self.parent_link_id, self.bidirectional, self.child_to_parent_transform.invert(), - self.constraint_id) - attachment.loose = False if self.loose else True - return attachment - - @property - def loose(self) -> bool: - """ - If true, then the child object will not move when parent moves. - """ - return self._loose - - @loose.setter - def loose(self, loose: bool): - self._loose = loose and not self.bidirectional - - @property - def is_reversed(self) -> bool: - """ - If true means that when child moves, parent moves not the other way around. - """ - return self.loose - - -@dataclass -class Constraint: - parent_obj_id: int - parent_link_id: int - child_obj_id: int - child_link_id: int - joint_type: JointType - joint_axis_in_child_link_frame: List[int] - joint_frame_position_wrt_parent_origin: List[float] - joint_frame_position_wrt_child_origin: List[float] - joint_frame_orientation_wrt_parent_origin: Optional[List[float]] = None - joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None - - -@dataclass -class WorldState: - state_id: int - attachments: Dict[Object, Dict[Object, Attachment]] - constraint_ids: Dict[Object, Dict[Object, int]] - - class World(ABC): """ The World Class represents the physics Simulation and belief state. @@ -222,7 +110,7 @@ def _update_local_transformer_worlds(self): @classmethod def _init_prospection_world(cls): - cls.prospection_world: World = cls("DIRECT", True) + cls.prospection_world: World = cls("DIRECT", True, World.simulation_time_step) def _sync_prospection_world(self): self.world_sync: WorldSync = WorldSync(self, self.prospection_world) @@ -856,7 +744,7 @@ def _copy(self) -> World: :return: The reference to the new BulletWorld """ - world = BulletWorld("DIRECT") + world = World("DIRECT", False, World.simulation_time_step) for obj in self.objects: o = Object(obj.name, obj.obj_type, obj.path, Pose(obj.get_position(), obj.get_orientation()), world, obj.color) @@ -949,371 +837,6 @@ def update_transforms_for_objects_in_current_world(self) -> None: obj.update_link_transforms(curr_time) -class BulletWorld(World): - """ - This class represents a BulletWorld, which is a simulation environment that uses the Bullet Physics Engine. This - class is the main interface to the Bullet Physics Engine and should be used to spawn Objects, simulate Physic and - manipulate the Bullet World. - """ - - # Check is for sphinx autoAPI to be able to work in a CI workflow - if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): - rospy.init_node('pycram') - - def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): - """ - Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the - background. There can only be one rendered simulation. - The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. - - :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" - :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. - """ - super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_time_step=0.004167) # 240 Hz - - self._gui_thread: Gui = Gui(self, mode) - self._gui_thread.start() - - # This disables file caching from PyBullet, since this would also cache - # files that can not be loaded - p.setPhysicsEngineParameter(enableFileCaching=0) - - # Needed to let the other thread start the simulation, before Objects are spawned. - time.sleep(0.1) - self.vis_axis: List[Object] = [] - - # Some default settings - self.set_gravity([0, 0, -9.8]) - - if not is_prospection_world: - plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - - def load_urdf_at_pose_and_get_object_id(self, path: str, pose: Pose) -> int: - return p.loadURDF(path, - basePosition=pose.position_as_list(), - baseOrientation=pose.orientation_as_list(), physicsClientId=self.client_id) - - def remove_object(self, obj_id: int) -> None: - """ - Remove an object by its ID. - - :param obj_id: The unique id of the object to be removed. - """ - - p.removeBody(obj_id, self.client_id) - - def add_constraint(self, constraint: Constraint) -> int: - """ - Add a constraint between two objects so that attachment they become attached - """ - constraint_id = p.createConstraint(constraint.parent_obj_id, - constraint.parent_link_id, - constraint.child_obj_id, - constraint.child_link_id, - constraint.joint_type.as_int(), - constraint.joint_axis_in_child_link_frame, - constraint.joint_frame_position_wrt_parent_origin, - constraint.joint_frame_position_wrt_child_origin, - constraint.joint_frame_orientation_wrt_parent_origin, - constraint.joint_frame_orientation_wrt_child_origin, - physicsClientId=self.client_id) - return constraint_id - - def remove_constraint(self, constraint_id): - p.removeConstraint(constraint_id, physicsClientId=self.client_id) - - def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: - """ - Get the joint upper limit of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: The joint upper limit as a float. - """ - return p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8] - - def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: - """ - Get the joint lower limit of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: The joint lower limit as a float. - """ - return p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[9] - - def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: - """ - Returns the axis along which a joint is moving. The given joint_name has to be part of this object. - - :param obj: The object - :param joint_name: Name of the joint for which the axis should be returned. - :return: The axis a vector of xyz - """ - return p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[13] - - def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: - """ - Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - - :param obj: The object - :param joint_name: Joint name for which the type should be returned - :return: The type of the joint - """ - joint_type = p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[2] - return JointType(joint_type) - - def get_object_joint_position(self, obj: Object, joint_name: str) -> float: - """ - Get the state of a joint of an articulated object - - :param obj: The object - :param joint_name: The name of the joint - """ - return p.getJointState(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[0] - - def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: - """ - Get the pose of a link of an articulated object with respect to the world frame. - The pose is given as a tuple of position and orientation. - - :param obj: The object - :param link_name: The name of the link - """ - return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] - - def get_object_contact_points(self, obj: Object) -> List: - """l.update_transforms_for_object(self.milk) - Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ - - :param obj: The object. - :return: A list of all contact points with other objects - """ - return p.getContactPoints(obj.id) - - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: - """ - Returns a list of contact points between obj1 and obj2. - - :param obj1: The first object. - :param obj2: The second object. - :return: A list of all contact points between the two objects. - """ - return p.getContactPoints(obj1.id, obj2.id) - - def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: - """ - Get the ID of a joint in an articulated object. - - :param obj: The object - :param joint_idx: The index of the joint (would indicate order). - """ - return p.getJointInfo(obj.id, joint_idx, self.client_id)[0] - - def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: - """ - Get the name of a joint in an articulated object. - - :param obj: The object - :param joint_idx: The index of the joint (would indicate order). - """ - return p.getJointInfo(obj.id, joint_idx, self.client_id)[1].decode('utf-8') - - def get_object_link_name(self, obj: Object, link_idx: int) -> str: - """ - Get the name of a link in an articulated object. - - :param obj: The object - :param link_idx: The index of the link (would indicate order). - """ - return p.getJointInfo(obj.id, link_idx, self.client_id)[12].decode('utf-8') - - def get_object_number_of_joints(self, obj: Object) -> int: - """ - Get the number of joints of an articulated object - - :param obj: The object - """ - return p.getNumJoints(obj.id, self.client_id) - - def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: - """ - Reset the joint position instantly without physics simulation - - :param obj: The object - :param joint_name: The name of the joint - :param joint_pose: The new joint pose - """ - p.resetJointState(obj.id, obj.joints[joint_name], joint_pose, physicsClientId=self.client_id) - - def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): - """ - 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 position: The new position of the object as a vector of x,y,z - :param orientation: The new orientation of the object as a quaternion of x,y,z,w - """ - p.resetBasePositionAndOrientation(obj.id, position, orientation, self.client_id) - - def step(self): - """ - Step the world simulation using forward dynamics - """ - p.stepSimulation(self.client_id) - - def set_object_link_color(self, obj: Object, link_id: int, rgba_color: List[float]): - """ - Changes the color of a link of this object, the color has to be given as a 4 element list - of RGBA values. - - :param obj: The object which should be colored - :param link_id: The link id of the link which should be colored - :param rgba_color: The color as RGBA values between 0 and 1 - """ - p.changeVisualShape(obj.id, link_id, rgbaColor=rgba_color, physicsClientId=self.client_id) - - def get_object_colors(self, obj: Object) -> Dict[str, List[float]]: - """ - Get the RGBA colors of each link in the object as a dictionary from link name to color. - - :param obj: The object - :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. - """ - visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) - swap = {v: k for k, v in obj.links.items()} - links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) - colors = list(map(lambda x: x[7], visual_data)) - link_to_color = dict(zip(links, colors)) - return link_to_color - - def get_object_AABB(self, obj: Object) -> Tuple[List[float], List[float]]: - """ - Returns the axis aligned bounding box of this object. The return of this method are two points in - world coordinate frame which define a bounding box. - - :param obj: The object for which the bounding box should be returned. - :return: Two lists of x,y,z which define the bounding box. - """ - return p.getAABB(obj.id, physicsClientId=self.client_id) - - def get_object_link_aabb(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: - """ - Returns the axis aligned bounding box of the link. The return of this method are two points in - world coordinate frame which define a bounding box. - - :param obj: The object for which the bounding box should be returned. - :param link_name: The name of a link of this object. - :return: Two lists of x,y,z which define the bounding box. - """ - return p.getAABB(obj.id, obj.links[link_name], self.client_id) - - def set_realtime(self, real_time: bool) -> None: - """ - Enables the real time simulation of Physic in the BulletWorld. By default, this is disabled and Physics is only - simulated to reason about it. - - :param real_time: Whether the World should simulate Physics in real time. - """ - p.setRealTimeSimulation(1 if real_time else 0, self.client_id) - - def set_gravity(self, gravity_vector: List[float]) -> None: - """ - Sets the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). - Gravity is given as a vector in x,y,z. Gravity is only applied while simulating Physic. - - :param gravity_vector: The gravity vector that should be used in the World. - """ - p.setGravity(gravity_vector[0], gravity_vector[1], gravity_vector[2], physicsClientId=self.client_id) - - @classmethod - def set_robot(cls, robot: Object) -> None: - """ - Sets the global variable for the robot Object. This should be set on spawning the robot. - - :param robot: The Object reference to the Object representing the robot. - """ - cls.robot = robot - - def exit(self, wait_time_before_exit_in_secs: Optional[float] = 0.1) -> None: - """ - Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the - preferred method to close the BulletWorld. - """ - super().exit(wait_time_before_exit_in_secs) - - def disconnect_from_physics_server(self): - """ - Disconnects the world from the physics server. - """ - p.disconnect(self.client_id) - - def join_threads(self): - """ - Join any running threads. Useful for example when exiting the world. - """ - self.join_gui_thread_if_exists() - - def join_gui_thread_if_exists(self): - if self._gui_thread: - self._gui_thread.join() - - def save_physics_simulator_state(self) -> int: - """ - Saves the state of the physics simulator and returns the unique id of the state. - """ - return p.saveState(self.client_id) - - def restore_physics_simulator_state(self, state_id): - """ - Restores the objects and environment state in the physics simulator according to - the given state using the unique state id. - """ - p.restoreState(state_id, physicsClientId=self.client_id) - - def add_vis_axis(self, pose: Pose, - length: Optional[float] = 0.2) -> None: - """ - Creates a Visual object which represents the coordinate frame at the given - position and orientation. There can be an unlimited amount of vis axis objects. - - :param pose: The pose at which the axis should be spawned - :param length: Optional parameter to configure the length of the axes - """ - - position, orientation = pose.to_list() - - vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], - rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) - vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], - rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) - vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], - rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) - - obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], - basePosition=position, baseOrientation=orientation, - linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkParentIndices=[0, 0, 0], - linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], - linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], - linkCollisionShapeIndices=[-1, -1, -1]) - - self.vis_axis.append(obj) - - def remove_vis_axis(self) -> None: - """ - Removes all spawned vis axis objects that are currently in this BulletWorld. - """ - for id in self.vis_axis: - p.removeBody(id) - self.vis_axis = [] - - class UseProspectionWorld: """ An environment for using the prospection world, while in this environment the :py:attr:`~World.current_world` @@ -1395,17 +918,17 @@ def run(self): del self.object_mapping[obj] self.remove_obj_queue.task_done() - for bulletworld_obj, prospection_obj in self.object_mapping.items(): - b_pose = bulletworld_obj.get_pose() + for world_obj, prospection_obj in self.object_mapping.items(): + b_pose = world_obj.get_pose() s_pose = prospection_obj.get_pose() if b_pose.dist(s_pose) != 0.0: - prospection_obj.set_pose(bulletworld_obj.get_pose()) + prospection_obj.set_pose(world_obj.get_pose()) # Manage joint positions - if len(bulletworld_obj.joints) > 2: - for joint_name in bulletworld_obj.joints.keys(): - if prospection_obj.get_joint_position(joint_name) != bulletworld_obj.get_joint_position(joint_name): - prospection_obj.set_positions_of_all_joints(bulletworld_obj.get_positions_of_all_joints()) + if len(world_obj.joints) > 2: + for joint_name in world_obj.joints.keys(): + if prospection_obj.get_joint_position(joint_name) != world_obj.get_joint_position(joint_name): + prospection_obj.set_positions_of_all_joints(world_obj.get_positions_of_all_joints()) break self.check_for_pause() @@ -1433,217 +956,11 @@ def check_for_equal(self) -> None: self.equal_states = eql -class Gui(threading.Thread): - """ - For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~World.exit` - Also contains the code for controlling the camera. - """ - - def __init__(self, world: World, mode: str): - threading.Thread.__init__(self) - self.world = world - self.mode: str = mode - - def run(self): - """ - Initializes the new simulation and checks in an endless loop - if it is still active. If it is the thread will be suspended for 1/80 seconds, if it is not the method and - thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. - """ - if self.mode != "GUI": - self.world.client_id = p.connect(p.DIRECT) - else: - self.world.client_id = p.connect(p.GUI) - - # Disable the side windows of the GUI - p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) - # Change the init camera pose - p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, - cameraTargetPosition=[-2, 0, 1]) - - # Get the initial camera target location - cameraTargetPosition = p.getDebugVisualizerCamera()[11] - - sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1]) - - # Create a sphere with a radius of 0.05 and a mass of 0 - sphereUid = p.createMultiBody(baseMass=0.0, - baseInertialFramePosition=[0, 0, 0], - baseVisualShapeIndex=sphereVisualId, - basePosition=cameraTargetPosition) - - # Define the maxSpeed, used in calculations - maxSpeed = 16 - - # Set initial Camera Rotation - cameraYaw = 50 - cameraPitch = -35 - - # Keep track of the mouse state - mouseState = [0, 0, 0] - oldMouseX, oldMouseY = 0, 0 - - # Determines if the sphere at cameraTargetPosition is visible - visible = 1 - - # Loop to update the camera position based on keyboard events - while p.isConnected(self.world.client_id): - # Monitor user input - keys = p.getKeyboardEvents() - mouse = p.getMouseEvents() - - # Get infos about the camera - width, height, dist = p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], \ - p.getDebugVisualizerCamera()[10] - cameraTargetPosition = p.getDebugVisualizerCamera()[11] - - # Get vectors used for movement on x,y,z Vector - xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] - yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] - zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] - - # Check the mouse state - if mouse: - for m in mouse: - - mouseX = m[2] - mouseY = m[1] - - # update mouseState - # Left Mouse button - if m[0] == 2 and m[3] == 0: - mouseState[0] = m[4] - # Middle mouse butto (scroll wheel) - if m[0] == 2 and m[3] == 1: - mouseState[1] = m[4] - # right mouse button - if m[0] == 2 and m[3] == 2: - mouseState[2] = m[4] - - # change visibility by clicking the mousewheel - if m[4] == 6 and m[3] == 1 and visible == 1: - visible = 0 - elif m[4] == 6 and visible == 0: - visible = 1 - - # camera movement when the left mouse button is pressed - if mouseState[0] == 3: - speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < maxSpeed else maxSpeed - speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < maxSpeed else maxSpeed - - # max angle of 89.5 and -89.5 to make sure the camera does not flip (is annoying) - if mouseX < oldMouseX: - if (cameraPitch + speedX) < 89.5: - cameraPitch += (speedX / 4) + 1 - elif mouseX > oldMouseX: - if (cameraPitch - speedX) > -89.5: - cameraPitch -= (speedX / 4) + 1 - - if mouseY < oldMouseY: - cameraYaw += (speedY / 4) + 1 - elif mouseY > oldMouseY: - cameraYaw -= (speedY / 4) + 1 - - if mouseState[1] == 3: - speedX = abs(oldMouseX - mouseX) - factor = 0.05 - - if mouseX < oldMouseX: - dist = dist - speedX * factor - elif mouseX > oldMouseX: - dist = dist + speedX * factor - dist = max(dist, 0.1) - - # camera movement when the right mouse button is pressed - if mouseState[2] == 3: - speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < 5 else 5 - speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < 5 else 5 - factor = 0.05 - - if mouseX < oldMouseX: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(zVec, factor), speedX)) - elif mouseX > oldMouseX: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(zVec, factor), speedX)) - - if mouseY < oldMouseY: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(xVec, factor), speedY)) - elif mouseY > oldMouseY: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(xVec, factor), speedY)) - # update oldMouse values - oldMouseY, oldMouseX = mouseY, mouseX - - # check the keyboard state - if keys: - # if shift is pressed, double the speed - if p.B3G_SHIFT in keys: - speedMult = 5 - else: - speedMult = 2.5 - - # if control is pressed, the movements caused by the arrowkeys, the '+' as well as the '-' key - # change - if p.B3G_CONTROL in keys: - - # the up and down arrowkeys cause the targetPos to move along the z axis of the map - if p.B3G_DOWN_ARROW in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(zVec, 0.03), speedMult)) - elif p.B3G_UP_ARROW in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(zVec, 0.03), speedMult)) - - # left and right arrowkeys cause the targetPos to move horizontally relative to the camera - if p.B3G_LEFT_ARROW in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(xVec, 0.03), speedMult)) - elif p.B3G_RIGHT_ARROW in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(xVec, 0.03), speedMult)) - - # the '+' and '-' keys cause the targetpos to move forwards and backwards relative to the camera - # while the camera stays at a constant distance. SHIFT + '=' is for US layout - if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(yVec, 0.03), speedMult)) - elif ord("-") in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(yVec, 0.03), speedMult)) - - # standard bindings for thearrowkeys, the '+' as well as the '-' key - else: - - # left and right arrowkeys cause the camera to rotate around the yaw axis - if p.B3G_RIGHT_ARROW in keys: - cameraYaw += (360 / width) * speedMult - elif p.B3G_LEFT_ARROW in keys: - cameraYaw -= (360 / width) * speedMult - - # the up and down arrowkeys cause the camera to rotate around the pitch axis - if p.B3G_DOWN_ARROW in keys: - if (cameraPitch + (360 / height) * speedMult) < 89.5: - cameraPitch += (360 / height) * speedMult - elif p.B3G_UP_ARROW in keys: - if (cameraPitch - (360 / height) * speedMult) > -89.5: - cameraPitch -= (360 / height) * speedMult - - # the '+' and '-' keys cause the camera to zoom towards and away from the targetPos without - # moving it. SHIFT + '=' is for US layout since the events can't handle shift plus something - if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: - if (dist - (dist * 0.02) * speedMult) > 0.1: - dist -= dist * 0.02 * speedMult - elif ord("-") in keys: - dist += dist * 0.02 * speedMult - - p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, - cameraTargetPosition=cameraTargetPosition) - if visible == 0: - cameraTargetPosition = (0.0, -50, 50) - p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) - time.sleep(1. / 80.) +@dataclass +class WorldState: + state_id: int + attachments: Dict[Object, Dict[Object, Attachment]] + constraint_ids: Dict[Object, Dict[Object, int]] class Object: @@ -2088,10 +1405,10 @@ def contact_points_simulated(self) -> List: :return: A list of contact points between this Object and other Objects """ - s = self.world.save_state() + state_id = self.world.save_state() self.world.step() contact_points = self.contact_points() - self.world.restore_state(*s) + self.world.restore_state(state_id) return contact_points def update_joints_from_topic(self, topic_name: str) -> None: @@ -2340,6 +1657,109 @@ def _get_robot_name_from_urdf(urdf_string: str) -> str: raise Exception("Robot Name not Found") +class Attachment: + def __init__(self, + parent_object: Object, + child_object: Object, + parent_link_id: Optional[int] = -1, # -1 means base link + child_link_id: Optional[int] = -1, + bidirectional: Optional[bool] = False, + child_to_parent_transform: Optional[Transform] = None, + constraint_id: Optional[int] = None): + """ + Creates an attachment between the parent object and the child object. + """ + self.parent_object = parent_object + self.child_object = child_object + self.parent_link_id = parent_link_id + self.child_link_id = child_link_id + self.bidirectional = bidirectional + self._loose = False and not self.bidirectional + + self.child_to_parent_transform = child_to_parent_transform + if self.child_to_parent_transform is None: + self.update_transform() + + self.constraint_id = constraint_id + if self.constraint_id is None: + self.add_constraint_and_update_objects_constraints_collection() + + def update_attachment(self): + self.update_transform() + self.update_constraint() + + def update_transform(self): + self.child_to_parent_transform = self.calculate_transform() + + def update_constraint(self): + self.remove_constraint_if_exists() + self.add_constraint_and_update_objects_constraints_collection() + + def calculate_transform(self): + return self.parent_object.get_transform_of_other_object_link_relative_to_my_link(self.parent_link_id, + self.child_link_id, + self.child_object) + + def remove_constraint_if_exists(self): + if self.constraint_id is not None: + self.parent_object.world.remove_constraint(self.constraint_id) + + def add_constraint_and_update_objects_constraints_collection(self): + self.constraint_id = self.add_fixed_constraint() + self.update_objects_constraints_collection() + + def add_fixed_constraint(self): + constraint_id = self.parent_object.world.add_fixed_constraint(self.parent_object.id, + self.child_object.id, + self.child_to_parent_transform, + self.parent_link_id, + self.child_link_id) + return constraint_id + + def update_objects_constraints_collection(self): + self.parent_object.cids[self.child_object] = self.constraint_id + self.child_object.cids[self.parent_object] = self.constraint_id + + def get_inverse(self): + attachment = Attachment(self.child_object, self.parent_object, self.child_link_id, + self.parent_link_id, self.bidirectional, self.child_to_parent_transform.invert(), + self.constraint_id) + attachment.loose = False if self.loose else True + return attachment + + @property + def loose(self) -> bool: + """ + If true, then the child object will not move when parent moves. + """ + return self._loose + + @loose.setter + def loose(self, loose: bool): + self._loose = loose and not self.bidirectional + + @property + def is_reversed(self) -> bool: + """ + If true means that when child moves, parent moves not the other way around. + """ + return self.loose + + +@dataclass +class Constraint: + parent_obj_id: int + parent_link_id: int + child_obj_id: int + child_link_id: int + joint_type: JointType + joint_axis_in_child_link_frame: List[int] + joint_frame_position_wrt_parent_origin: List[float] + joint_frame_position_wrt_child_origin: List[float] + joint_frame_orientation_wrt_parent_origin: Optional[List[float]] = None + joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None + + def _load_object(name: str, path: str, position: List[float], @@ -2415,7 +1835,7 @@ def _load_object(name: str, try: obj = world.load_urdf_at_pose_and_get_object_id(path, Pose(position, orientation)) return obj, path - except p.error as e: + except Exception as e: logging.error( "The File could not be loaded. Plese note that the path has to be either a URDF, stl or obj file or the name of an URDF string on the parameter server.") os.remove(path) @@ -2435,13 +1855,13 @@ def _is_cached(path: str, name: str, cach_dir: str) -> bool: :return: True if there already exists a chached file, False in any other case. """ file_name = pathlib.Path(path).name - p = pathlib.Path(cach_dir + file_name) - if p.exists(): + full_path = pathlib.Path(cach_dir + file_name) + if full_path.exists(): return True # Returns filename without the filetype, e.g. returns "test" for "test.txt" file_stem = pathlib.Path(path).stem - p = pathlib.Path(cach_dir + file_stem + ".urdf") - if p.exists(): + full_path = pathlib.Path(cach_dir + file_stem + ".urdf") + if full_path.exists(): return True return False @@ -2579,5 +1999,5 @@ def _world_and_id(world: World) -> Tuple[World, int]: :return: The World object and the id of this World """ world = world if world is not None else World.current_world - id = world.client_id if world is not None else World.current_world.client_id - return world, id + client_id = world.client_id if world is not None else World.current_world.client_id + return world, client_id diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 369760991..7950741ee 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -3,7 +3,8 @@ import numpy as np import rospy -from .world import _world_and_id, Object, UseProspectionWorld, BulletWorld +from .world import _world_and_id, Object, UseProspectionWorld +from .bullet_world import BulletWorld from .external_interfaces.ik import request_ik from .local_transformer import LocalTransformer from .plan_failures import IKError diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 704711756..b32256c8c 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -1,7 +1,8 @@ import unittest import numpy as np -from pycram.world import BulletWorld, Object, fix_missing_inertial +from pycram.bullet_world import BulletWorld +from pycram.world import Object, fix_missing_inertial from pycram.pose import Pose from pycram.robot_descriptions import robot_description from pycram.process_module import ProcessModule diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 993dea9bc..5bdf72c3e 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -5,7 +5,8 @@ import sqlalchemy.orm import pycram.plan_failures -from pycram.world import BulletWorld, Object +from pycram.bullet_world import BulletWorld +from pycram.world import Object from pycram.designators import action_designator, object_designator from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index 9da659e5b..ff80a0714 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -5,7 +5,8 @@ import requests import pycram.plan_failures -from pycram.world import BulletWorld, Object +from pycram.bullet_world import BulletWorld +from pycram.world import Object from pycram.designators import action_designator, object_designator from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot From f05ef6459b048130434e64fe2ca31ff9939e758e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Dec 2023 14:55:14 +0100 Subject: [PATCH 16/69] [WorldAbstractClass] Tests are running after using object.tf_frame instead of root link tf frame. --- src/pycram/bullet_world.py | 4 ++-- src/pycram/local_transformer.py | 2 -- src/pycram/world.py | 38 +++++++++++++++++++-------------- 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index a60bcedb3..ba7e76aec 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -26,7 +26,7 @@ class is the main interface to the Bullet Physics Engine and should be used to s if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): rospy.init_node('pycram') - def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): + def __init__(self, mode: str = "GUI", is_prospection_world: bool = False, sim_time_step=0.004167): # 240 Hz """ Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -35,7 +35,7 @@ def __init__(self, mode: str = "GUI", is_prospection_world: bool = False): :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. """ - super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_time_step=0.004167) # 240 Hz + super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_time_step=sim_time_step) self._gui_thread: Gui = Gui(self, mode) self._gui_thread.start() diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 7ed2c3fd7..05d149277 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -78,8 +78,6 @@ def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union return Pose(*copy_pose.to_list(), frame=new_pose.header.frame_id) - # TODO: This is not working correctly, the lookup fails to find the frames, - # this is probably because the frames are not published. def lookup_transform_from_source_to_target_frame(self, source_frame: str, target_frame: str, time: Optional[rospy.rostime.Time] = None) -> Transform: """ diff --git a/src/pycram/world.py b/src/pycram/world.py index d87a82349..5d3b2683c 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -10,7 +10,7 @@ import xml.etree.ElementTree from queue import Queue import tf -from typing import List, Optional, Dict, Tuple, Callable +from typing import List, Optional, Dict, Tuple, Callable, Type from typing import Union import numpy as np @@ -33,6 +33,13 @@ from dataclasses import dataclass +@dataclass +class WorldState: + state_id: int + attachments: Dict[Object, Dict[Object, Attachment]] + constraint_ids: Dict[Object, Dict[Object, int]] + + class World(ABC): """ The World Class represents the physics Simulation and belief state. @@ -58,6 +65,10 @@ class World(ABC): """ simulation_time_step: float = None + """ + Global reference for the simulation time step, this is used to calculate the frequency of the simulation, + and also for calculating the equivalent real time for the simulation. + """ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: float): """ @@ -110,7 +121,7 @@ def _update_local_transformer_worlds(self): @classmethod def _init_prospection_world(cls): - cls.prospection_world: World = cls("DIRECT", True, World.simulation_time_step) + cls.prospection_world: World = cls("DIRECT", True, cls.simulation_time_step) def _sync_prospection_world(self): self.world_sync: WorldSync = WorldSync(self, self.prospection_world) @@ -956,13 +967,6 @@ def check_for_equal(self) -> None: self.equal_states = eql -@dataclass -class WorldState: - state_id: int - attachments: Dict[Object, Dict[Object, Attachment]] - constraint_ids: Dict[Object, Dict[Object, int]] - - class Object: """ Represents a spawned Object in the World. @@ -1303,10 +1307,10 @@ def get_other_object_link_pose_relative_to_my_link(self, parent_link_frame = self.get_link_tf_frame(self.get_link_by_id(my_link_id)) return self.local_transformer.transform_pose_to_target_frame(child_link_pose, parent_link_frame) - def get_transform_of_other_object_link_relative_to_my_link(self, - my_link_id: int, - other_link_id: int, - other_object: Object) -> Transform: + def get_transform_from_my_link_to_other_object_link(self, + my_link_id: int, + other_link_id: int, + other_object: Object) -> Transform: pose = self.get_other_object_link_pose_relative_to_my_link(my_link_id, other_link_id, other_object) return pose.to_transform(other_object.get_link_tf_frame_by_id(other_link_id)) @@ -1572,6 +1576,8 @@ def get_link_tf_frame(self, link_name: str) -> str: :param link_name: Name of a link for which the tf frame should be returned :return: A TF frame name for a specific link """ + if link_name == self.urdf_object.get_root(): + return self.tf_frame return self.tf_frame + "/" + link_name def get_link_tf_frame_by_id(self, link_id: int) -> str: @@ -1696,9 +1702,9 @@ def update_constraint(self): self.add_constraint_and_update_objects_constraints_collection() def calculate_transform(self): - return self.parent_object.get_transform_of_other_object_link_relative_to_my_link(self.parent_link_id, - self.child_link_id, - self.child_object) + return self.parent_object.get_transform_from_my_link_to_other_object_link(self.parent_link_id, + self.child_link_id, + self.child_object) def remove_constraint_if_exists(self): if self.constraint_id is not None: From a81c48900776172ee7a53e2dbd8256d2fcbfac81 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 12 Dec 2023 16:05:34 +0100 Subject: [PATCH 17/69] [WorldAbstractClass] Added Color and AxisAlignedBoundingBox datalcasses and used them instead of Lists (changed function signatures to use them). --- src/pycram/bullet_world.py | 19 +++--- src/pycram/world.py | 105 +++++++++----------------------- src/pycram/world_dataclasses.py | 95 +++++++++++++++++++++++++++++ 3 files changed, 135 insertions(+), 84 deletions(-) create mode 100644 src/pycram/world_dataclasses.py diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index ba7e76aec..159fba0df 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -12,7 +12,8 @@ from .enums import JointType, ObjectType from .pose import Pose -from .world import World, Object, Constraint +from .world import World, Object +from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox class BulletWorld(World): @@ -230,7 +231,7 @@ def step(self): """ p.stepSimulation(self.client_id) - def set_object_link_color(self, obj: Object, link_id: int, rgba_color: List[float]): + def set_object_link_color(self, obj: Object, link_id: int, rgba_color: Color): """ Changes the color of a link of this object, the color has to be given as a 4 element list of RGBA values. @@ -241,7 +242,7 @@ def set_object_link_color(self, obj: Object, link_id: int, rgba_color: List[floa """ p.changeVisualShape(obj.id, link_id, rgbaColor=rgba_color, physicsClientId=self.client_id) - def get_object_colors(self, obj: Object) -> Dict[str, List[float]]: + def get_object_colors(self, obj: Object) -> Dict[str, Color]: """ Get the RGBA colors of each link in the object as a dictionary from link name to color. @@ -251,21 +252,21 @@ def get_object_colors(self, obj: Object) -> Dict[str, List[float]]: visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) swap = {v: k for k, v in obj.links.items()} links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) - colors = list(map(lambda x: x[7], visual_data)) + colors = Color.from_rgba(list(map(lambda x: x[7], visual_data))) link_to_color = dict(zip(links, colors)) return link_to_color - def get_object_AABB(self, obj: Object) -> Tuple[List[float], List[float]]: + def get_object_aabb(self, obj: Object) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. :param obj: The object for which the bounding box should be returned. - :return: Two lists of x,y,z which define the bounding box. + :return: AxisAlignedBoundingBox object with min and max box points. """ - return p.getAABB(obj.id, physicsClientId=self.client_id) + return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj.id, physicsClientId=self.client_id)) - def get_object_link_aabb(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: + def get_object_link_aabb(self, obj: Object, link_name: str) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. @@ -274,7 +275,7 @@ def get_object_link_aabb(self, obj: Object, link_name: str) -> Tuple[List[float] :param link_name: The name of a link of this object. :return: Two lists of x,y,z which define the bounding box. """ - return p.getAABB(obj.id, obj.links[link_name], self.client_id) + return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj.id, obj.links[link_name], self.client_id)) def set_realtime(self, real_time: bool) -> None: """ diff --git a/src/pycram/world.py b/src/pycram/world.py index 5d3b2683c..3e77e4f93 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -10,7 +10,7 @@ import xml.etree.ElementTree from queue import Queue import tf -from typing import List, Optional, Dict, Tuple, Callable, Type +from typing import List, Optional, Dict, Tuple, Callable from typing import Union import numpy as np @@ -31,6 +31,7 @@ from abc import ABC, abstractmethod from dataclasses import dataclass +from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox @dataclass @@ -467,14 +468,14 @@ def step(self): """ pass - def set_object_color(self, obj: Object, color: List[float], link: Optional[str] = ""): + def set_object_color(self, obj: Object, color: Color, link: Optional[str] = ""): """ Changes the color of this object, the color has to be given as a list of RGBA values. Optionally a link name can can be provided, if no link name is provided all links of this object will be colored. :param obj: The object which should be colored - :param color: The color as RGBA values between 0 and 1 + :param color: The color as Color object with RGBA values between 0 and 1 :param link: The link name of the link which should be colored """ if link == "": @@ -489,27 +490,26 @@ def set_object_color(self, obj: Object, color: List[float], link: Optional[str] self.set_object_link_color(obj, obj.links[link], color) @abstractmethod - def set_object_link_color(self, obj: Object, link_id: int, rgba_color: List[float]): + def set_object_link_color(self, obj: Object, link_id: int, rgba_color: Color): """ - Changes the color of a link of this object, the color has to be given as a 4 element list - of RGBA values. + Changes the color of a link of this object, the color has to be given as Color object. :param obj: The object which should be colored :param link_id: The link id of the link which should be colored - :param rgba_color: The color as RGBA values between 0 and 1 + :param rgba_color: The color as Color object with RGBA values between 0 and 1 """ pass def get_object_color(self, obj: Object, - link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: + link: Optional[str] = None) -> Union[Color, Dict[str, Color], None]: """ This method returns the color of this object or a link of this object. If no link is given then the return is either: - 1. A list with the color as RGBA values, this is the case if the object only has one link (this + 1. A Color object with RGBA values, this is the case if the object only has one link (this happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the color as value. The color is given as RGBA values. + 2. A dict with the link name as key and the color as value. The color is given as a Color Object. Please keep in mind that not every link may have a color. This is dependent on the URDF from which the object is spawned. @@ -538,35 +538,35 @@ def get_object_color(self, return link_to_color_dict @abstractmethod - def get_object_colors(self, obj: Object) -> Dict[str, List[float]]: + def get_object_colors(self, obj: Object) -> Dict[str, Color]: """ Get the RGBA colors of each link in the object as a dictionary from link name to color. :param obj: The object - :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. + :return: A dictionary with link names as keys and a Color object for each link as value. """ pass @abstractmethod - def get_object_AABB(self, obj: Object) -> Tuple[List[float], List[float]]: + def get_object_aabb(self, obj: Object) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. :param obj: The object for which the bounding box should be returned. - :return: Two lists of x,y,z which define the bounding box. + :return: AxisAlignedBoundingBox object containing the min and max points of the bounding box. """ pass @abstractmethod - def get_object_link_aabb(self, obj: Object, link_name: str) -> Tuple[List[float], List[float]]: + def get_object_link_aabb(self, obj: Object, link_name: str) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. :param obj: The object for which the bounding box should be returned. :param link_name: The name of a link of this object. - :return: Two lists of x,y,z which define the bounding box. + :return: AxisAlignedBoundingBox object containing the min and max points of the bounding box. """ pass @@ -916,7 +916,7 @@ def run(self): # self.equal_states = False for i in range(self.add_obj_queue.qsize()): obj = self.add_obj_queue.get() - # [name, type, path, position, orientation, self.world.prospection_world, color, bulletworld object] + # [name, type, path, position, orientation, self.world.prospection_world, color, world object] o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) # Maps the World object to the prospection world object self.object_mapping[obj[7]] = o @@ -972,11 +972,10 @@ class Object: Represents a spawned Object in the World. """ - # TODO: make a color dataclass or change initialization of color to be non mutable def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, pose: Pose = None, world: World = None, - color: Optional[List[float]] = [1, 1, 1, 1], + color: Optional[Color] = Color(), ignore_cached_files: Optional[bool] = False): """ The constructor loads the urdf file into the given World, if no World is specified the @@ -997,7 +996,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.local_transformer = LocalTransformer() self.name: str = name self.obj_type: Union[str, ObjectType] = obj_type - self.color: List[float] = color + self.color: Color = color pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") position, orientation = pose_in_map.to_list() self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignore_cached_files) @@ -1447,7 +1446,7 @@ def update_pose_from_tf(self, frame: str) -> None: position[0][2]] self.set_position(Pose(position, orientation)) - def set_color(self, color: List[float], link: Optional[str] = "") -> None: + def set_color(self, color: Color, link: Optional[str] = "") -> None: """ Changes the color of this object, the color has to be given as a list of RGBA values. Optionally a link name can can be provided, if no link @@ -1458,42 +1457,13 @@ def set_color(self, color: List[float], link: Optional[str] = "") -> None: """ self.world.set_object_color(self, color, link) - def get_color(self, link: Optional[str] = None) -> Union[List[float], Dict[str, List[float]], None]: - """ - This method returns the color of this object or a link of this object. If no link is given then the - return is either: - - 1. A list with the color as RGBA values, this is the case if the object only has one link (this - happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the color as value. The color is given as RGBA values. - Please keep in mind that not every link may have a color. This is dependent on the URDF from which the - object is spawned. - - If a link is specified then the return is a list with RGBA values representing the color of this link. - It may be that this link has no color, in this case the return is None as well as an error message. - - :param link: the link name for which the color should be returned. - :return: The color of the object or link, or a dictionary containing every colored link with its color - """ + def get_color(self, link: Optional[str] = None) -> Union[Color, Dict[str, Color], None]: return self.world.get_object_color(self, link) - def get_aabb(self) -> Tuple[List[float], List[float]]: - """ - Returns the axis aligned bounding box of this object. The return of this method are two points in - world coordinate frame which define a bounding box. - - :return: Two lists of x,y,z which define the bounding box. - """ - return self.world.get_object_AABB(self) - - def get_link_aabb(self, link_name: str) -> Tuple[List[float], List[float]]: - """ - Returns the axis aligned bounding box of the given link name. The return of this method are two points in - world coordinate frame which define a bounding box. + def get_aabb(self) -> AxisAlignedBoundingBox: + return self.world.get_object_aabb(self) - :param link_name: The name of a link of this object. - :return: Two lists of x,y,z which define the bounding box. - """ + def get_link_aabb(self, link_name: str) -> AxisAlignedBoundingBox: return self.world.get_object_link_aabb(self, link_name) def get_base_origin(self) -> Pose: @@ -1503,9 +1473,9 @@ def get_base_origin(self) -> Pose: :return: The position of the bottom of this Object """ aabb = self.get_link_aabb(link_name=self.urdf_object.get_root()) - base_width = np.absolute(aabb[0][0] - aabb[1][0]) - base_length = np.absolute(aabb[0][1] - aabb[1][1]) - return Pose([aabb[0][0] + base_width / 2, aabb[0][1] + base_length / 2, aabb[0][2]], + base_width = np.absolute(aabb.min_x - aabb.max_x) + base_length = np.absolute(aabb.min_y - aabb.max_y) + return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], self.get_pose().orientation_as_list()) def get_joint_limits(self, joint: str) -> Tuple[float, float]: @@ -1752,26 +1722,12 @@ def is_reversed(self) -> bool: return self.loose -@dataclass -class Constraint: - parent_obj_id: int - parent_link_id: int - child_obj_id: int - child_link_id: int - joint_type: JointType - joint_axis_in_child_link_frame: List[int] - joint_frame_position_wrt_parent_origin: List[float] - joint_frame_position_wrt_child_origin: List[float] - joint_frame_orientation_wrt_parent_origin: Optional[List[float]] = None - joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None - - def _load_object(name: str, path: str, position: List[float], orientation: List[float], world: World, - color: List[float], + color: Color, ignore_cached_files: bool) -> Tuple[int, str]: """ Loads an object to the given World with the given position and orientation. The color will only be @@ -1846,7 +1802,6 @@ def _load_object(name: str, "The File could not be loaded. Plese note that the path has to be either a URDF, stl or obj file or the name of an URDF string on the parameter server.") os.remove(path) raise (e) - # print(f"{bcolors.BOLD}{bcolors.WARNING}The path has to be either a path to a URDf file, stl file, obj file or the name of a URDF on the parameter server.{bcolors.ENDC}") def _is_cached(path: str, name: str, cach_dir: str) -> bool: @@ -1957,7 +1912,7 @@ def fix_link_attributes(urdf_string: str) -> str: return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') -def _generate_urdf_file(name: str, path: str, color: List[float], cach_dir: str) -> str: +def _generate_urdf_file(name: str, path: str, color: Color, cach_dir: str) -> str: """ Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given color will be used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with the name @@ -1988,7 +1943,7 @@ def _generate_urdf_file(name: str, path: str, color: List[float], cach_dir: str) \n \ ' urdf_template = fix_missing_inertial(urdf_template) - rgb = " ".join(list(map(str, color))) + rgb = " ".join(list(map(str, color.get_rgba()))) pathlib_obj = pathlib.Path(path) path = str(pathlib_obj.resolve()) content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py new file mode 100644 index 000000000..d91d396bb --- /dev/null +++ b/src/pycram/world_dataclasses.py @@ -0,0 +1,95 @@ +from dataclasses import dataclass +from typing import List, Optional, Tuple +from .enums import JointType + + +@dataclass +class Color: + """ + Dataclass for storing color as an RGBA value. + """ + R: float = 1 + G: float = 1 + B: float = 1 + A: float = 1 + + @classmethod + def from_rgba(cls, rgba: List[float]): + """ + Sets the color from a list of RGBA values. + + :param rgba: The list of RGBA values + """ + return cls(rgba[0], rgba[1], rgba[2], rgba[3]) + + def get_rgba(self) -> List[float]: + """ + Returns the color as a list of RGBA values. + + :return: The color as a list of RGBA values + """ + return [self.R, self.G, self.B, self.A] + + +@dataclass +class Constraint: + """ + Dataclass for storing a constraint between two objects. + """ + parent_obj_id: int + parent_link_id: int + child_obj_id: int + child_link_id: int + joint_type: JointType + joint_axis_in_child_link_frame: List[int] + joint_frame_position_wrt_parent_origin: List[float] + joint_frame_position_wrt_child_origin: List[float] + joint_frame_orientation_wrt_parent_origin: Optional[List[float]] = None + joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None + + +@dataclass +class AxisAlignedBoundingBox: + """ + Dataclass for storing an axis-aligned bounding box. + """ + min_x: float + min_y: float + min_z: float + max_x: float + max_y: float + max_z: float + + @classmethod + def from_min_max(cls, min_point: List[float], max_point: List[float]): + """ + Sets the axis-aligned bounding box from a minimum and maximum point. + + :param min_point: The minimum point + :param max_point: The maximum point + """ + return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2]) + + def get_min_max(self) -> Tuple[List[float], List[float]]: + """ + Returns the axis-aligned bounding box as a tuple of minimum and maximum points. + + :return: The axis-aligned bounding box as a tuple of minimum and maximum points + """ + return self.get_min_point(), self.get_max_point() + + def get_min_point(self) -> List[float]: + """ + Returns the minimum point of the axis-aligned bounding box. + + :return: The minimum point of the axis-aligned bounding box + """ + return [self.min_x, self.min_y, self.min_z] + + def get_max_point(self) -> List[float]: + """ + Returns the maximum point of the axis-aligned bounding box. + + :return: The maximum point of the axis-aligned bounding box + """ + return [self.max_x, self.max_y, self.max_z] From 278a7fee49d05625907644fb147867665fb0876d Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 13 Dec 2023 20:13:02 +0100 Subject: [PATCH 18/69] [WorldAbstractClass] Some cleaning up and refactoring of method names. --- demos/pycram_bullet_world_demo/demo.py | 3 +- demos/pycram_ur5_demo/demo.py | 2 +- src/pycram/bullet_world.py | 11 +- src/pycram/costmaps.py | 6 +- src/pycram/designators/location_designator.py | 4 +- .../resolver/location/giskard_location.py | 2 +- src/pycram/world.py | 145 ++++++++++-------- src/pycram/world_reasoning.py | 20 +-- 8 files changed, 103 insertions(+), 90 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index c19ad349c..36f908fbb 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -11,7 +11,8 @@ apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]), color=[1, 0, 0, 1]) -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]), color=[0, 1, 0, 1]) +cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]), + color=[0, 1, 0, 1]) spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=[0, 0, 1, 1]) bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=[1, 1, 0, 1]) apartment.attach(spoon, 'cabinet10_drawer_top') diff --git a/demos/pycram_ur5_demo/demo.py b/demos/pycram_ur5_demo/demo.py index f8fe23198..b54b5b6d7 100644 --- a/demos/pycram_ur5_demo/demo.py +++ b/demos/pycram_ur5_demo/demo.py @@ -38,7 +38,7 @@ lab = Object("kitchen", "environment", kitchen_urdf_path) robot = Object("ur", "robot", robot_urdf_path, pose=SPAWNING_POSES["robot"]) cereal = Object("cereal", "object", os.path.join(RESOURCE_DIR, "breakfast_cereal.stl"), - pose=SPAWNING_POSES["cereal"]) + pose=SPAWNING_POSES["cereal"]) BulletWorld.robot = robot tf_broadcaster = TFBroadcaster("projection", "odom", 1.0) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 159fba0df..13cc510df 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -204,7 +204,7 @@ def get_object_number_of_joints(self, obj: Object) -> int: """ return p.getNumJoints(obj.id, self.client_id) - def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + def reset_object_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ Reset the joint position instantly without physics simulation @@ -295,15 +295,6 @@ def set_gravity(self, gravity_vector: List[float]) -> None: """ p.setGravity(gravity_vector[0], gravity_vector[1], gravity_vector[2], physicsClientId=self.client_id) - @classmethod - def set_robot(cls, robot: Object) -> None: - """ - Sets the global variable for the robot Object. This should be set on spawning the robot. - - :param robot: The Object reference to the Object representing the robot. - """ - cls.robot = robot - def exit(self, wait_time_before_exit_in_secs: Optional[float] = 0.1) -> None: """ Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 85579203c..ca7136a6f 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -394,9 +394,9 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: physicsClientId=BulletWorld.current_world.client_id) j += len(n) if BulletWorld.robot: - shadow_robot = BulletWorld.current_world.get_prospection_object(BulletWorld.robot) + shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(BulletWorld.robot) attached_objs = BulletWorld.robot.attachments.keys() - attached_objs_shadow_id = [BulletWorld.current_world.get_prospection_object(x).id for x in + attached_objs_shadow_id = [BulletWorld.current_world.get_prospection_object_from_object(x).id for x in attached_objs] res[i:j] = [ 1 if ray[0] == -1 or ray[0] == shadow_robot.id or ray[0] in attached_objs_shadow_id else 0 for @@ -726,7 +726,7 @@ def get_aabb_for_link(self) -> Tuple[List[float], List[float]]: :return: Two points in world coordinate space, which span a rectangle """ - shadow_obj = BulletWorld.current_world.get_prospection_object(self.object) + shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(self.object) with UseProspectionWorld(): shadow_obj.set_orientation(Pose(orientation=[0, 0, 0, 1])) link_orientation = shadow_obj.get_link_pose(self.link) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 1a95ec346..a8f2ab689 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -181,7 +181,7 @@ def __iter__(self): if self.visible_for or self.reachable_for: robot_object = self.visible_for.bullet_world_object if self.visible_for else self.reachable_for.bullet_world_object - test_robot = BulletWorld.current_world.get_prospection_object(robot_object) + test_robot = BulletWorld.current_world.get_prospection_object_from_object(robot_object) with UseProspectionWorld(): @@ -257,7 +257,7 @@ def __iter__(self) -> Location: final_map = occupancy + gaussian - test_robot = BulletWorld.current_world.get_prospection_object(self.robot) + test_robot = BulletWorld.current_world.get_prospection_object_from_object(self.robot) # Find a Joint of type prismatic which is above the handle in the URDF tree container_joint = self.handle.bullet_world_object.find_joint_above(self.handle.name, JointType.PRISMATIC) diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index 5c891b4e0..88479c50d 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -28,7 +28,7 @@ def __iter__(self) -> CostmapLocation.Location: pose_left, end_config_left = self._get_reachable_pose_for_arm(self.target, robot_description.get_tool_frame("left")) - test_robot = BulletWorld.current_world.get_prospection_object(BulletWorld.robot) + test_robot = BulletWorld.current_world.get_prospection_object_from_object(BulletWorld.robot) with UseProspectionWorld(): valid, arms = reachability_validator(pose_right, test_robot, self.target, {}) if valid: diff --git a/src/pycram/world.py b/src/pycram/world.py index 3e77e4f93..0203716e1 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -258,7 +258,8 @@ def add_constraint(self, constraint: Constraint) -> int: """ pass - def detach_objects(self, obj1: Object, obj2: Object) -> None: + @staticmethod + def detach_objects(obj1: Object, obj2: Object) -> None: """ Detaches obj2 from obj1. This is done by deleting the attachment from the attachments dictionary of both objects @@ -270,9 +271,6 @@ def detach_objects(self, obj1: Object, obj2: Object) -> None: """ del obj1.attachments[obj2] del obj2.attachments[obj1] - - self.remove_constraint(obj1.cids[obj2]) - del obj1.cids[obj2] del obj2.cids[obj1] @@ -439,7 +437,7 @@ def get_object_number_of_joints(self, obj: Object) -> int: pass @abstractmethod - def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + def reset_object_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ Reset the joint position instantly without physics simulation @@ -614,15 +612,32 @@ def set_gravity(self, gravity_vector: List[float]) -> None: """ pass - @classmethod - @abstractmethod - def set_robot(cls, robot: Object) -> None: + def set_robot_if_not_set(self, robot: Object) -> None: + """ + Sets the robot if it is not set yet. + + :param robot: The Object reference to the Object representing the robot. + """ + if not self.robot_is_set(): + self.set_robot(robot) + + @staticmethod + def set_robot(robot: Union[Object, None]) -> None: """ Sets the global variable for the robot Object. This should be set on spawning the robot. :param robot: The Object reference to the Object representing the robot. """ - pass + World.robot = robot + + @staticmethod + def robot_is_set() -> bool: + """ + Returns whether the robot has been set or not. + + :return: True if the robot has been set, False otherwise. + """ + return World.robot is not None def exit(self, wait_time_before_exit_in_secs: Optional[float] = None) -> None: """ @@ -649,8 +664,8 @@ def disconnect_from_physics_server(self): pass def reset_current_world(self): - if self.current_world == self: - self.current_world = None + if World.current_world == self: + World.current_world = None def reset_robot(self): self.set_robot(None) @@ -749,16 +764,16 @@ def restore_constraints_from_saved_world_state(self, state_id: int): def _copy(self) -> World: """ - Copies this Bullet World into another and returns it. The other BulletWorld - will be in Direct mode. The shadow world should always be preferred instead of creating a new BulletWorld. + Copies this World into another and returns it. The other World + will be in Direct mode. The prospection world should always be preferred instead of creating a new World. This method should only be used if necessary since there can be unforeseen problems. - :return: The reference to the new BulletWorld + :return: The reference to the new World """ world = World("DIRECT", False, World.simulation_time_step) for obj in self.objects: - o = Object(obj.name, obj.obj_type, obj.path, Pose(obj.get_position(), obj.get_orientation()), - world, obj.color) + o = Object(obj.name, obj.obj_type, obj.path, Pose(obj.get_position(), obj.get_orientation()), world, + obj.color) for joint in obj.joints: o.set_joint_position(joint, obj.get_joint_position(joint)) return world @@ -778,7 +793,7 @@ def register_two_objects_collision_callbacks(self, object_a: Object, object_b: O self.coll_callbacks[(object_a, object_b)] = (on_collision_callback, on_collision_removal_callback) @classmethod - def add_additional_resource_path(cls, path: str) -> None: + def add_resource_path(cls, path: str) -> None: """ Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an Object is spawned only with a filename. @@ -787,7 +802,7 @@ def add_additional_resource_path(cls, path: str) -> None: """ cls.data_directory.append(path) - def get_prospection_object(self, obj: Object) -> Object: + def get_prospection_object_from_object(self, obj: Object) -> Object: """ Returns the corresponding object from the prospection world for a given object in the main world. If the given Object is already in the prospection world, it is returned. @@ -807,7 +822,7 @@ def get_prospection_object(self, obj: Object) -> Object: f" the object isn't anymore in the main (graphical) World" f" or if the given object is already a prospection object. ") - def get_object_from_prospection(self, prospection_object: Object) -> Object: + def get_object_from_prospection_object(self, prospection_object: Object) -> Object: """ Returns the corresponding object from the main World for a given object in the shadow world. If the given object is not in the shadow @@ -824,19 +839,14 @@ def get_object_from_prospection(self, prospection_object: Object) -> Object: def reset_world(self) -> None: """ - Resets the BulletWorld to the state it was first spawned in. + Resets the World to the state it was first spawned in. All attached objects will be detached, all joints will be set to the default position of 0 and all objects will be set to the position and orientation in which they were spawned. """ for obj in self.objects: - if obj.attachments: - attached_objects = list(obj.attachments.keys()) - for att_obj in attached_objects: - obj.detach(att_obj) - joint_names = list(obj.joints.keys()) - joint_poses = [0 for j in joint_names] - obj.set_positions_of_all_joints(dict(zip(joint_names, joint_poses))) + obj.detach_all() + obj.reset_all_joints_positions() obj.set_pose(obj.original_pose) def update_transforms_for_objects_in_current_world(self) -> None: @@ -972,11 +982,8 @@ class Object: Represents a spawned Object in the World. """ - def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, - pose: Pose = None, - world: World = None, - color: Optional[Color] = Color(), - ignore_cached_files: Optional[bool] = False): + def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, pose: Pose = None, world: World = None, + color: Optional[Color] = Color(), ignore_cached_files: Optional[bool] = False): """ The constructor loads the urdf file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. @@ -990,7 +997,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, :param color: The color with which the object should be spawned. :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ - if not pose: + if pose is None: pose = Pose() self.world: World = world if world is not None else World.current_world self.local_transformer = LocalTransformer() @@ -1008,15 +1015,14 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) - # This means "world" is not the prospection world since it has a reference to a prospection world - if self.world.prospection_world is not None: + if not self.world.is_prospection_world: self.world.world_sync.add_obj_queue.put( [name, obj_type, path, position, orientation, self.world.prospection_world, color, self]) with open(self.path) as f: self.urdf_object = URDF.from_xml_string(f.read()) - if self.urdf_object.name == robot_description.name and not World.robot: - World.robot = self + if self.urdf_object.name == robot_description.name: + self.world.set_robot_if_not_set(self) self.links[self.urdf_object.get_root()] = -1 @@ -1050,18 +1056,21 @@ def remove(self) -> None: """ Removes this object from the World it currently resides in. For the object to be removed it has to be detached from all objects it - is currently attached to. After this is done a call to PyBullet is done - to remove this Object from the simulation. + is currently attached to. After this is done a call to world remove object is done + to remove this Object from the simulation/world. """ - for obj in self.attachments.keys(): - self.world.detach_objects(self, obj) + self.detach_all() + self.world.objects.remove(self) + # This means the current world of the object is not the prospection world, since it # has a reference to the prospection world if self.world.prospection_world is not None: self.world.world_sync.remove_obj_queue.put(self) self.world.world_sync.remove_obj_queue.join() + self.world.remove_object(self.id) + if World.robot == self: World.robot = None @@ -1345,40 +1354,49 @@ def get_link_pose(self, name: str) -> Pose: def get_link_pose_by_id(self, link_id: int) -> Pose: return self.get_link_pose(self.get_link_by_id(link_id)) - def set_joint_position(self, joint_name: str, joint_pose: float) -> None: + 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.joints.keys()) + joint_positions = [0] * len(joint_names) + self.set_positions_of_all_joints(dict(zip(joint_names, joint_positions))) + + def set_positions_of_all_joints(self, joint_poses: dict) -> 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: + :return: + """ + for joint_name, joint_position in joint_poses.items(): + self.world.reset_object_joint_position(self, joint_name, joint_position) + self._current_joints_positions[joint_name] = joint_position + self._update_current_link_poses_and_transforms() + self.world._set_attached_objects(self, [self]) + + def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, as stated in the URDF, an error will be printed. However, the joint will be set either way. :param joint_name: The name of the joint - :param joint_pose: The target pose for this joint + :param joint_position: The target pose for this joint """ # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly up_lim, low_lim = self.world.get_object_joint_limits(self, joint_name) if low_lim > up_lim: low_lim, up_lim = up_lim, low_lim - if not low_lim <= joint_pose <= up_lim: + if not low_lim <= joint_position <= up_lim: logging.error( - f"The joint position has to be within the limits of the joint. The joint limits for {joint_name} are {low_lim} and {up_lim}") - logging.error(f"The given joint position was: {joint_pose}") + f"The joint position has to be within the limits of the joint. The joint limits for {joint_name}" + f" are {low_lim} and {up_lim}") + logging.error(f"The given joint position was: {joint_position}") # Temporarily disabled because kdl outputs values exciting joint limits # return - self.world.reset_joint_position(self, joint_name, joint_pose) - self._current_joints_positions[joint_name] = joint_pose - self._update_current_link_poses_and_transforms() - self.world._set_attached_objects(self, [self]) - - def set_positions_of_all_joints(self, joint_poses: dict) -> 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: - :return: - """ - for joint_name, joint_pose in joint_poses.items(): - self.world.reset_joint_position(self, joint_name, joint_pose) - self._current_joints_positions[joint_name] = joint_pose + self.world.reset_object_joint_position(self, joint_name, joint_position) + self._current_joints_positions[joint_name] = joint_position self._update_current_link_poses_and_transforms() self.world._set_attached_objects(self, [self]) @@ -1721,6 +1739,9 @@ def is_reversed(self) -> bool: """ return self.loose + def __del__(self): + self.remove_constraint_if_exists() + def _load_object(name: str, path: str, diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 7950741ee..9847100ba 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -98,7 +98,7 @@ def stable(object: Object, :return: True if the given object is stable in the world False else """ world, world_id = _world_and_id(world) - shadow_obj = BulletWorld.current_world.get_prospection_object(object) + shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) with UseProspectionWorld(): coords_prev = shadow_obj.pose.position_as_list() state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) @@ -130,8 +130,8 @@ def contact(object1: Object, """ with UseProspectionWorld(): - shadow_obj1 = BulletWorld.current_world.get_prospection_object(object1) - shadow_obj2 = BulletWorld.current_world.get_prospection_object(object2) + shadow_obj1 = BulletWorld.current_world.get_prospection_object_from_object(object1) + shadow_obj2 = BulletWorld.current_world.get_prospection_object_from_object(object2) p.performCollisionDetection(BulletWorld.current_world.client_id) con_points = p.getContactPoints(shadow_obj1.id, shadow_obj2.id, physicsClientId=BulletWorld.current_world.client_id) @@ -165,9 +165,9 @@ def visible(object: Object, """ front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis with UseProspectionWorld(): - shadow_obj = BulletWorld.current_world.get_prospection_object(object) + shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) if BulletWorld.robot: - shadow_robot = BulletWorld.current_world.get_prospection_object(BulletWorld.robot) + shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(BulletWorld.robot) state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) for obj in BulletWorld.current_world.objects: if obj == shadow_obj or BulletWorld.robot and obj == shadow_robot: @@ -245,7 +245,7 @@ def occluding(object: Object, occluding.append(seg_mask[c[0]][c[1]]) occ_objects = list(set(map(BulletWorld.current_world.get_object_by_id, occluding))) - occ_objects = list(map(world.get_object_from_prospection, occ_objects)) + occ_objects = list(map(world.get_object_from_prospection_object, occ_objects)) return occ_objects @@ -268,7 +268,7 @@ def reachable(pose: Union[Object, Pose], if type(pose) == Object: pose = pose.get_pose() - shadow_robot = BulletWorld.current_world.get_prospection_object(robot) + shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(robot) with UseProspectionWorld(): arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" joints = robot_description.chains[arm].joints @@ -306,7 +306,7 @@ def blocking(pose_or_object: Union[Object, Pose], else: input_pose = pose_or_object - shadow_robot = BulletWorld.current_world.get_prospection_object(robot) + shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(robot) with UseProspectionWorld(): arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" joints = robot_description.chains[arm].joints @@ -330,7 +330,7 @@ def blocking(pose_or_object: Union[Object, Pose], block = [] for obj in BulletWorld.current_world.objects: if contact(shadow_robot, obj): - block.append(BulletWorld.current_world.get_object_from_prospection(obj)) + block.append(BulletWorld.current_world.get_object_from_prospection_object(obj)) return block @@ -358,7 +358,7 @@ def link_pose_for_joint_config(object: Object, joint_config: Dict[str, float], l :param link_name: Name of the link for which the pose should be returned :return: The pose of the link after applying the joint configuration """ - shadow_object = BulletWorld.current_world.get_prospection_object(object) + shadow_object = BulletWorld.current_world.get_prospection_object_from_object(object) with UseProspectionWorld(): for joint, pose in joint_config.items(): shadow_object.set_joint_position(joint, pose) From 205c9f2da2f51cb72a0dcddd36ee12c6cb8fb809 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 14 Dec 2023 20:12:01 +0100 Subject: [PATCH 19/69] [ClassForLinks] Created Link class. --- src/pycram/bullet_world.py | 70 ++++---- src/pycram/costmaps.py | 19 +- src/pycram/designators/object_designator.py | 2 +- src/pycram/external_interfaces/giskard.py | 4 +- src/pycram/pose.py | 3 +- src/pycram/ros/force_torque_sensor.py | 4 +- src/pycram/ros/joint_state_publisher.py | 2 +- src/pycram/ros/tf_broadcaster.py | 2 +- src/pycram/ros/viz_marker_publisher.py | 6 +- src/pycram/world.py | 190 +++++++++++++------- src/pycram/world_dataclasses.py | 45 ++++- 11 files changed, 222 insertions(+), 125 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 13cc510df..1828de269 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -97,7 +97,7 @@ def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: :param joint_name: The name of the joint. :return: The joint upper limit as a float. """ - return p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[8] + return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[8] def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: """ @@ -107,7 +107,7 @@ def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: :param joint_name: The name of the joint. :return: The joint lower limit as a float. """ - return p.getJointInfo(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[9] + return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[9] def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: """ @@ -117,7 +117,7 @@ def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: :param joint_name: Name of the joint for which the axis should be returned. :return: The axis a vector of xyz """ - return p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[13] + return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], self.client_id)[13] def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: """ @@ -127,7 +127,7 @@ def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: :param joint_name: Joint name for which the type should be returned :return: The type of the joint """ - joint_type = p.getJointInfo(obj.id, obj.joints[joint_name], self.client_id)[2] + joint_type = p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], self.client_id)[2] return JointType(joint_type) def get_object_joint_position(self, obj: Object, joint_name: str) -> float: @@ -137,17 +137,17 @@ def get_object_joint_position(self, obj: Object, joint_name: str) -> float: :param obj: The object :param joint_name: The name of the joint """ - return p.getJointState(obj.id, obj.joints[joint_name], physicsClientId=self.client_id)[0] + return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[0] - def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: + def get_object_link_pose(self, obj_id: int, link_id: int) -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. The pose is given as a tuple of position and orientation. - :param obj: The object - :param link_name: The name of the link + :param obj_id: The object + :param link_id: The name of the link """ - return p.getLinkState(obj.id, obj.links[link_name], physicsClientId=self.client_id)[4:6] + return Pose(*p.getLinkState(obj_id, link_id, physicsClientId=self.client_id)[4:6]) def get_object_contact_points(self, obj: Object) -> List: """l.update_transforms_for_object(self.milk) @@ -169,40 +169,44 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ return p.getContactPoints(obj1.id, obj2.id) - def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: + def get_object_joint_names(self, obj_id: int) -> List[str]: + """ + Get the names of all joints of an articulated object. """ - Get the ID of a joint in an articulated object. + num_joints = self.get_object_number_of_joints(obj_id) + return [p.getJointInfo(obj_id, i, self.client_id)[1].decode('utf-8') for i in range(num_joints)] - :param obj: The object - :param joint_idx: The index of the joint (would indicate order). + def get_object_link_names(self, obj_id: int) -> List[str]: + """ + Get the names of all joints of an articulated object. """ - return p.getJointInfo(obj.id, joint_idx, self.client_id)[0] + num_links = self.get_object_number_of_links(obj_id) + return [p.getJointInfo(obj_id, i, self.client_id)[12].decode('utf-8') for i in range(num_links)] - def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: + def get_object_number_of_links(self, obj_id: int) -> int: """ - Get the name of a joint in an articulated object. + Get the number of links of an articulated object - :param obj: The object - :param joint_idx: The index of the joint (would indicate order). + :param obj_id: The object """ - return p.getJointInfo(obj.id, joint_idx, self.client_id)[1].decode('utf-8') + return self.get_object_number_of_joints(obj_id) - def get_object_link_name(self, obj: Object, link_idx: int) -> str: + def get_object_number_of_joints(self, obj_id: int) -> int: """ - Get the name of a link in an articulated object. + Get the number of joints of an articulated object - :param obj: The object - :param link_idx: The index of the link (would indicate order). + :param obj_id: The object """ - return p.getJointInfo(obj.id, link_idx, self.client_id)[12].decode('utf-8') + return p.getNumJoints(obj_id, self.client_id) - def get_object_number_of_joints(self, obj: Object) -> int: + def get_object_link_id(self, obj_id: int, link_name: str) -> int: """ - Get the number of joints of an articulated object + Get the ID of a link in an articulated object. :param obj: The object + :param link_name: The name of the link """ - return p.getNumJoints(obj.id, self.client_id) + return self.get_object_by_id(obj_id).link_name_to_id[link_name] def reset_object_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ @@ -212,7 +216,7 @@ def reset_object_joint_position(self, obj: Object, joint_name: str, joint_pose: :param joint_name: The name of the joint :param joint_pose: The new joint pose """ - p.resetJointState(obj.id, obj.joints[joint_name], joint_pose, physicsClientId=self.client_id) + p.resetJointState(obj.id, obj.joint_name_to_id[joint_name], joint_pose, physicsClientId=self.client_id) def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): """ @@ -250,7 +254,7 @@ def get_object_colors(self, obj: Object) -> Dict[str, Color]: :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. """ visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) - swap = {v: k for k, v in obj.links.items()} + swap = {v: k for k, v in obj.link_name_to_id.items()} links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) colors = Color.from_rgba(list(map(lambda x: x[7], visual_data))) link_to_color = dict(zip(links, colors)) @@ -266,16 +270,12 @@ def get_object_aabb(self, obj: Object) -> AxisAlignedBoundingBox: """ return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj.id, physicsClientId=self.client_id)) - def get_object_link_aabb(self, obj: Object, link_name: str) -> AxisAlignedBoundingBox: + def get_object_link_aabb(self, obj_id: int, link_id: int) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. - - :param obj: The object for which the bounding box should be returned. - :param link_name: The name of a link of this object. - :return: Two lists of x,y,z which define the bounding box. """ - return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj.id, obj.links[link_name], self.client_id)) + return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj_id, link_id, self.client_id)) def set_realtime(self, real_time: bool) -> None: """ diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index ca7136a6f..bc21b122d 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -11,6 +11,7 @@ from .bullet_world import BulletWorld from pycram.world import UseProspectionWorld, Object from .world_reasoning import _get_images_for_target +from .world_dataclasses import AxisAlignedBoundingBox from nav_msgs.msg import OccupancyGrid, MapMetaData from typing import Tuple, List, Union, Optional @@ -713,12 +714,12 @@ def generate_map(self) -> None: Generates the semantic costmap according to the provided parameters. To do this the axis aligned bounding box (AABB) for the link name will be used. Height and width of the final Costmap will be the x and y sizes of the AABB. """ - min, max = self.get_aabb_for_link() - self.height = int((max[0] - min[0]) // self.resolution) - self.width = int((max[1] - min[1]) // self.resolution) + min_p, max_p = self.get_aabb_for_link().get_min_max_points() + self.height = int((max_p.x - min_p.x) // self.resolution) + self.width = int((max_p.y - min_p.y) // self.resolution) self.map = np.ones((self.height, self.width)) - def get_aabb_for_link(self) -> Tuple[List[float], List[float]]: + def get_aabb_for_link(self) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box (AABB) of the link provided when creating this costmap. To try and let the AABB as close to the actual object as possible, the Object will be rotated such that the link will be in the @@ -726,14 +727,14 @@ def get_aabb_for_link(self) -> Tuple[List[float], List[float]]: :return: Two points in world coordinate space, which span a rectangle """ - shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(self.object) + prospection_object = BulletWorld.current_world.get_prospection_object_from_object(self.object) with UseProspectionWorld(): - shadow_obj.set_orientation(Pose(orientation=[0, 0, 0, 1])) - link_orientation = shadow_obj.get_link_pose(self.link) + prospection_object.set_orientation(Pose(orientation=[0, 0, 0, 1])) + link_orientation = prospection_object.get_link_pose(self.link) link_orientation_trans = link_orientation.to_transform(self.object.get_link_tf_frame(self.link)) inverse_orientation = link_orientation_trans.invert() - shadow_obj.set_orientation(inverse_orientation.to_pose()) - return shadow_obj.get_link_aabb(self.link) + prospection_object.set_orientation(inverse_orientation.to_pose()) + return prospection_object.get_link_aabb(self.object.get_link_id(self.link)) cmap = colors.ListedColormap(['white', 'black', 'green', 'red', 'blue']) diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 8f5f5200c..28f0e8cc9 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -95,7 +95,7 @@ def __iter__(self): :yield: A resolved Object designator """ for name in self.names: - if name in self.part_of.bullet_world_object.links.keys(): + if name in self.part_of.bullet_world_object.link_name_to_id.keys(): yield self.Object(name, self.type, self.part_of.bullet_world_object, self.part_of.bullet_world_object.get_link_pose(name)) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index f556a7ab0..691a62617 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -101,7 +101,7 @@ def sync_worlds() -> None: add_gripper_groups() bullet_object_names = set() for obj in BulletWorld.current_world.objects: - if obj.name != robot_description.name and len(obj.links) != 1: + if obj.name != robot_description.name and len(obj.link_name_to_id) != 1: bullet_object_names.add(obj.name + "_" + str(obj.id)) giskard_object_names = set(giskard_wrapper.get_group_names()) @@ -130,7 +130,7 @@ def spawn_object(object: Object) -> None: :param object: BulletWorld object that should be spawned """ - if len(object.links) == 1: + if len(object.link_name_to_id) == 1: geometry = object.urdf_object.link_map[object.urdf_object.get_root()].collision.geometry if isinstance(geometry, urdf_parser_py.urdf.Mesh): filename = geometry.filename diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 0b9feeaca..2ddbbfe7c 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -87,7 +87,7 @@ def frame(self, value: str) -> None: self.header.frame_id = value @property - def position(self) -> GeoPose: + def position(self) -> Union[Point,GeoPose]: """ Property that points to the position of this pose """ @@ -109,6 +109,7 @@ def position(self, value) -> None: self.pose.position.y = value[1] self.pose.position.z = value[2] else: + # TODO: Check if this is correct or if it should be handled as an error self.pose.position = value @property diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros/force_torque_sensor.py index 74b48024f..3b9b21af4 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros/force_torque_sensor.py @@ -28,8 +28,8 @@ def __init__(self, joint_name, fts_topic="/pycram/fts", interval=0.1): self.world = BulletWorld.current_world self.fts_joint_idx = None self.joint_name = joint_name - if joint_name in self.world.robot.joints.keys(): - self.fts_joint_idx = self.world.robot.joints[joint_name] + if joint_name in self.world.robot.joint_name_to_id.keys(): + self.fts_joint_idx = self.world.robot.joint_name_to_id[joint_name] else: raise RuntimeError(f"Could not register ForceTorqueSensor: Joint {joint_name} does not exist in robot object") pb.enableJointForceTorqueSensor(self.world.robot.id, self.fts_joint_idx, enableSensor=1) diff --git a/src/pycram/ros/joint_state_publisher.py b/src/pycram/ros/joint_state_publisher.py index efc51334d..fb7682e19 100644 --- a/src/pycram/ros/joint_state_publisher.py +++ b/src/pycram/ros/joint_state_publisher.py @@ -37,7 +37,7 @@ def _publish(self) -> None: The joint states are published as long as the kill_event is not set by :py:meth:`~JointStatePublisher._stop_publishing` """ robot = BulletWorld.robot - joint_names = [joint_name for joint_name in robot.joints.keys()] + joint_names = [joint_name for joint_name in robot.joint_name_to_id.keys()] seq = 0 while not self.kill_event.is_set(): diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index 6880949f6..1eb0e5943 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -53,7 +53,7 @@ def _update_objects(self) -> None: for obj in self.world.objects: pose = obj.get_pose() self._publish_pose(obj.tf_frame, pose) - for link in obj.links.keys(): + for link in obj.link_name_to_id.keys(): link_pose = obj.get_link_pose(link) self._publish_pose(obj.get_link_tf_frame(link), link_pose) diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 83193c80e..b91ed879a 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -59,14 +59,14 @@ def _make_marker_array(self) -> MarkerArray: for obj in BulletWorld.current_world.objects: if obj.name == "floor": continue - for link in obj.links.keys(): + for link in obj.link_name_to_id.keys(): geom = obj.link_to_geometry[link] if not geom: continue msg = Marker() msg.header.frame_id = "map" msg.ns = obj.name - msg.id = obj.links[link] + msg.id = obj.link_name_to_id[link] msg.type = Marker.MESH_RESOURCE msg.action = Marker.ADD link_pose = obj.get_link_pose(link).to_transform(link) @@ -78,7 +78,7 @@ def _make_marker_array(self) -> MarkerArray: link_pose_with_origin = link_pose * link_origin msg.pose = link_pose_with_origin.to_pose().pose - color = [1, 1, 1, 1] if obj.links[link] == -1 else obj.get_color(link) + color = [1, 1, 1, 1] if obj.link_name_to_id[link] == -1 else obj.get_color(link) msg.color = ColorRGBA(*color) msg.lifetime = rospy.Duration(1) diff --git a/src/pycram/world.py b/src/pycram/world.py index 0203716e1..a8dd25aaa 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -19,7 +19,7 @@ import urdf_parser_py.urdf from geometry_msgs.msg import Quaternion, Point -from urdf_parser_py.urdf import URDF +from urdf_parser_py.urdf import URDF, Collision, GeometricType from .event import Event from .robot_descriptions import robot_description @@ -345,13 +345,10 @@ def get_object_joint_position(self, obj: Object, joint_name: str) -> float: pass @abstractmethod - def get_object_link_pose(self, obj: Object, link_name: str) -> Tuple[List, List]: + def get_object_link_pose(self, obj_id: int, link_id: int) -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. The pose is given as a tuple of position and orientation. - - :param obj: The object - :param link_name: The name of the link """ pass @@ -398,27 +395,20 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> pass @abstractmethod - def get_object_joint_id(self, obj: Object, joint_idx: int) -> int: + def get_object_joint_names(self, obj_id: int) -> List[str]: """ - Get the ID of a joint in an articulated object. - - :param obj: The object - :param joint_idx: The index of the joint (would indicate order). + Get the names of all joints of an articulated object. """ pass @abstractmethod - def get_object_joint_name(self, obj: Object, joint_idx: int) -> str: + def get_object_link_names(self, obj_id: int) -> List[str]: """ - Get the name of a joint in an articulated object. - - :param obj: The object - :param joint_idx: The index of the joint (would indicate order). + Get the names of all links of an articulated object. """ pass - @abstractmethod - def get_object_link_name(self, obj: Object, link_idx: int) -> str: + def get_object_link_id(self, obj: Object, link_idx: int) -> str: """ Get the name of a link in an articulated object. @@ -428,11 +418,15 @@ def get_object_link_name(self, obj: Object, link_idx: int) -> str: pass @abstractmethod - def get_object_number_of_joints(self, obj: Object) -> int: + def get_object_number_of_joints(self, obj_id: int) -> int: """ Get the number of joints of an articulated object + """ + pass - :param obj: The object + def get_object_number_of_links(self, obj_id: int) -> int: + """ + Get the number of links of an articulated object """ pass @@ -479,13 +473,13 @@ def set_object_color(self, obj: Object, color: Color, link: Optional[str] = ""): if link == "": # Check if there is only one link, this is the case for primitive # forms or if loaded from an .stl or .obj file - if obj.links != {}: - for link_id in obj.links.values(): + if obj.link_name_to_id != {}: + for link_id in obj.link_name_to_id.values(): self.set_object_link_color(obj, link_id, color) else: self.set_object_link_color(obj, -1, color) else: - self.set_object_link_color(obj, obj.links[link], color) + self.set_object_link_color(obj, obj.link_name_to_id[link], color) @abstractmethod def set_object_link_color(self, obj: Object, link_id: int, rgba_color: Color): @@ -523,7 +517,7 @@ def get_object_color(self, if link: if link in link_to_color_dict.keys(): return link_to_color_dict[link] - elif link not in obj.links.keys(): + elif link not in obj.link_name_to_id.keys(): rospy.logerr(f"The link '{link}' is not part of this obejct") return None else: @@ -557,14 +551,10 @@ def get_object_aabb(self, obj: Object) -> AxisAlignedBoundingBox: pass @abstractmethod - def get_object_link_aabb(self, obj: Object, link_name: str) -> AxisAlignedBoundingBox: + def get_object_link_aabb(self, obj_id: int, link_id: int) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. - - :param obj: The object for which the bounding box should be returned. - :param link_name: The name of a link of this object. - :return: AxisAlignedBoundingBox object containing the min and max points of the bounding box. """ pass @@ -774,7 +764,7 @@ def _copy(self) -> World: for obj in self.objects: o = Object(obj.name, obj.obj_type, obj.path, Pose(obj.get_position(), obj.get_orientation()), world, obj.color) - for joint in obj.joints: + for joint in obj.joint_name_to_id: o.set_joint_position(joint, obj.get_joint_position(joint)) return world @@ -946,8 +936,8 @@ def run(self): prospection_obj.set_pose(world_obj.get_pose()) # Manage joint positions - if len(world_obj.joints) > 2: - for joint_name in world_obj.joints.keys(): + if len(world_obj.joint_name_to_id) > 2: + for joint_name in world_obj.joint_name_to_id.keys(): if prospection_obj.get_joint_position(joint_name) != world_obj.get_joint_position(joint_name): prospection_obj.set_positions_of_all_joints(world_obj.get_positions_of_all_joints()) break @@ -977,13 +967,81 @@ def check_for_equal(self) -> None: self.equal_states = eql +class Link: + + def __init__(self, + _id: int, + obj_id: int, + obj_tf_frame: str, + urdf_link: urdf_parser_py.urdf.Link, + world: World, + is_root: Optional[bool] = False): + self.id = _id + self.obj_id = obj_id + self.tf_frame = f"{obj_tf_frame}/{self.name}" if not is_root else obj_tf_frame + self.urdf_link = urdf_link + self.world = world + self.object = world.get_object_by_id(obj_id) + self.local_transformer = self.object.local_transformer + + @property + def transform(self) -> Transform: + """ + The transformation from the world frame to this link frame. + """ + return self.pose.to_transform(self.tf_frame) + + def get_transform_to_other_link(self, other_link: Link): + new_pose = self.local_transformer.transform_pose_to_target_frame(other_link.pose, self.tf_frame) + return new_pose.to_transform(other_link.tf_frame) + + def get_pose_wrt_other_link(self, other_link: Link): + return self.local_transformer.transform_pose_to_target_frame(self.pose, other_link.tf_frame) + + def get_aabb(self) -> AxisAlignedBoundingBox: + return self.world.get_object_link_aabb(self.obj_id, self.id) + + @property + def position(self) -> Point: + return self.pose.position + + @property + def orientation(self): + return self.pose.orientation + + @property + def pose(self): + """ + The pose of the link relative to the world frame. + """ + return self.world.get_object_link_pose(self.obj_id, self.id) + + @property + def name(self): + return self.urdf_link.name + + def get_geometry(self) -> GeometricType: + return None if not self.collision else self.collision.geometry + + @property + def collision(self): + return self.urdf_link.collision + + + + + + class Object: """ Represents a spawned Object in the World. """ - def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, pose: Pose = None, world: World = None, - color: Optional[Color] = Color(), ignore_cached_files: Optional[bool] = False): + def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, + pose: Pose = None, + world: World = None, + color: Optional[Color] = Color(), + ignore_cached_files: Optional[bool] = False): """ The constructor loads the urdf file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. @@ -1007,8 +1065,8 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, pose: pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") position, orientation = pose_in_map.to_list() self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignore_cached_files) - self.joints: Dict[str, int] = self._joint_or_link_name_to_id("joint") - self.links: Dict[str, int] = self._joint_or_link_name_to_id("link") + self.joint_name_to_id: Dict[str, int] = self._get_joint_name_to_id_map() + self.link_name_to_id: Dict[str, int] = self._get_link_name_to_id_map() self.attachments: Dict[Object, Attachment] = {} self.cids: Dict[Object, int] = {} self.original_pose = pose_in_map @@ -1024,7 +1082,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, pose: if self.urdf_object.name == robot_description.name: self.world.set_robot_if_not_set(self) - self.links[self.urdf_object.get_root()] = -1 + self.link_name_to_id[self.urdf_object.get_root()] = -1 self._current_pose = pose_in_map self._current_link_poses: Dict[str, Pose] = {} @@ -1043,7 +1101,7 @@ def _init_current_positions_of_joint(self) -> None: """ Initialize the cached joint position for each joint. """ - for joint_name in self.joints.keys(): + for joint_name in self.joint_name_to_id.keys(): self._current_joints_positions[joint_name] = self.world.get_object_joint_position(self, joint_name) def __repr__(self): @@ -1246,23 +1304,21 @@ def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: pose.pose.orientation = target_orientation self.set_pose(pose) - def _joint_or_link_name_to_id(self, name_type: str) -> Dict[str, int]: + def _get_joint_name_to_id_map(self) -> Dict[str, int]: + """ + Creates a dictionary which maps the joint names to their unique ids. """ - Creates a dictionary which maps the link or joint name to the unique ids used by pybullet. + joint_names = self.world.get_object_joint_names(self.id) + n_joints = len(joint_names) + return dict(zip(joint_names, range(n_joints))) - :param name_type: Determines if the dictionary should be for joints or links - :return: A dictionary that maps joint or link names to unique ids + def _get_link_name_to_id_map(self) -> Dict[str, int]: """ - n_joints = self.world.get_object_number_of_joints(self) - joint_name_to_id = {} - for i in range(0, n_joints): - _id = self.world.get_object_joint_id(self, i) - if name_type == "joint": - _name = self.world.get_object_joint_name(self, i) - else: - _name = self.world.get_object_link_name(self, i) - joint_name_to_id[_name] = _id - return joint_name_to_id + Creates a dictionary which maps the link names to their unique ids. + """ + link_names = self.world.get_object_link_names(self.id) + n_links = len(link_names) + return dict(zip(link_names, range(n_links))) def get_joint_id(self, name: str) -> int: """ @@ -1271,7 +1327,7 @@ def get_joint_id(self, name: str) -> int: :param name: The joint name :return: The unique id """ - return self.joints[name] + return self.joint_name_to_id[name] def get_link_id(self, name: str) -> int: """ @@ -1282,7 +1338,7 @@ def get_link_id(self, name: str) -> int: """ if name is None: return -1 - return self.links[name] + return self.link_name_to_id[name] def get_link_by_id(self, id: int) -> str: """ @@ -1291,7 +1347,7 @@ def get_link_by_id(self, id: int) -> str: :param id: id for link :return: The link name """ - return dict(zip(self.links.values(), self.links.keys()))[id] + return dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys()))[id] def get_joint_by_id(self, joint_id: int) -> str: """ @@ -1300,7 +1356,7 @@ def get_joint_by_id(self, joint_id: int) -> str: :param joint_id: The Pybullet id of for joint :return: The joint name """ - return dict(zip(self.joints.values(), self.joints.keys()))[joint_id] + return dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys()))[joint_id] def get_other_object_link_pose_relative_to_my_link(self, my_link_id: int, @@ -1347,7 +1403,7 @@ def get_link_pose(self, name: str) -> Pose: :param name: Link name for which a Pose should be returned :return: The pose of the link """ - if name in self.links.keys() and self.links[name] == -1: + if name in self.link_name_to_id.keys() and self.link_name_to_id[name] == -1: return self.get_pose() return self._current_link_poses[name] @@ -1358,7 +1414,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 = list(self.joints.keys()) + joint_names = list(self.joint_name_to_id.keys()) joint_positions = [0] * len(joint_names) self.set_positions_of_all_joints(dict(zip(joint_names, joint_positions))) @@ -1442,11 +1498,11 @@ def update_joints_from_topic(self, topic_name: str) -> None: msg = rospy.wait_for_message(topic_name, JointState) joint_names = msg.name joint_positions = msg.position - if set(joint_names).issubset(self.joints.keys()): + if set(joint_names).issubset(self.joint_name_to_id.keys()): for i in range(len(joint_names)): self.set_joint_position(joint_names[i], joint_positions[i]) else: - add_joints = set(joint_names) - set(self.joints.keys()) + add_joints = set(joint_names) - set(self.joint_name_to_id.keys()) rospy.logerr(f"There are joints in the published joint state which are not in this model: /n \ The following joint{'s' if len(add_joints) != 1 else ''}: {add_joints}") @@ -1481,8 +1537,8 @@ def get_color(self, link: Optional[str] = None) -> Union[Color, Dict[str, Color] def get_aabb(self) -> AxisAlignedBoundingBox: return self.world.get_object_aabb(self) - def get_link_aabb(self, link_name: str) -> AxisAlignedBoundingBox: - return self.world.get_object_link_aabb(self, link_name) + def get_link_aabb(self, link_id: int) -> AxisAlignedBoundingBox: + return self.world.get_object_link_aabb(self.id, link_id) def get_base_origin(self) -> Pose: """ @@ -1490,7 +1546,7 @@ def get_base_origin(self) -> Pose: :return: The position of the bottom of this Object """ - aabb = self.get_link_aabb(link_name=self.urdf_object.get_root()) + aabb = self.get_link_aabb(-1) base_width = np.absolute(aabb.min_x - aabb.max_x) base_length = np.absolute(aabb.min_y - aabb.max_y) return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], @@ -1504,7 +1560,7 @@ def get_joint_limits(self, joint: str) -> Tuple[float, float]: :param joint: The name of the joint for which the limits should be found. :return: The lower and upper limit of the joint. """ - if joint not in self.joints.keys(): + if joint not in self.joint_name_to_id.keys(): raise KeyError(f"The given Joint: {joint} is not part of this object") lower, upper = self.world.get_object_joint_limits(self, joint) if lower > upper: @@ -1541,7 +1597,7 @@ def find_joint_above(self, link_name: str, joint_type: JointType) -> str: reversed_chain = reversed(chain) container_joint = None for element in reversed_chain: - if element in self.joints and self.get_joint_type(element) == joint_type: + if element in self.joint_name_to_id and self.get_joint_type(element) == joint_type: container_joint = element break if not container_joint: @@ -1578,7 +1634,7 @@ def get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.GeometricType]: :return: A dictionary with link name as key and geometry information as value """ link_to_geometry = {} - for link in self.links.keys(): + for link in self.link_name_to_id.keys(): link_obj = self.urdf_object.link_map[link] if not link_obj.collision: link_to_geometry[link] = None @@ -1593,7 +1649,7 @@ def _update_current_link_poses_and_transforms(self) -> None: """ Updates the cached poses and transforms for each link of this Object """ - for link_name in self.links.keys(): + for link_name in self.link_name_to_id.keys(): if link_name == self.urdf_object.get_root(): self._update_root_link_pose_and_transform() else: @@ -1605,7 +1661,7 @@ def _update_root_link_pose_and_transform(self) -> None: self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame) def _update_link_pose_and_transform(self, link_name: str) -> None: - self._current_link_poses[link_name] = Pose(*self.world.get_object_link_pose(self, link_name)) + self._current_link_poses[link_name] = self.world.get_object_link_pose(self.id, self.get_link_id(link_name)) self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( self.get_link_tf_frame(link_name)) diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index d91d396bb..6598c511a 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -48,6 +48,21 @@ class Constraint: joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None +@dataclass +class Point: + x: float + y: float + z: float + + @classmethod + def from_list(cls, point: List[float]): + """ + Sets the point from a list of x, y, z values. + + :param point: The list of x, y, z values + """ + return cls(point[0], point[1], point[2]) + @dataclass class AxisAlignedBoundingBox: """ @@ -70,7 +85,7 @@ def from_min_max(cls, min_point: List[float], max_point: List[float]): """ return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2]) - def get_min_max(self) -> Tuple[List[float], List[float]]: + def get_min_max_points(self) -> Tuple[Point, Point]: """ Returns the axis-aligned bounding box as a tuple of minimum and maximum points. @@ -78,7 +93,31 @@ def get_min_max(self) -> Tuple[List[float], List[float]]: """ return self.get_min_point(), self.get_max_point() - def get_min_point(self) -> List[float]: + def get_min_point(self) -> Point: + """ + Returns the axis-aligned bounding box as a minimum point. + + :return: The axis-aligned bounding box as a minimum point + """ + return Point(self.min_x, self.min_y, self.min_z) + + def get_max_point(self) -> Point: + """ + Returns the axis-aligned bounding box as a maximum point. + + :return: The axis-aligned bounding box as a maximum point + """ + return Point(self.max_x, self.max_y, self.max_z) + + def get_min_max(self) -> Tuple[List[float], List[float]]: + """ + Returns the axis-aligned bounding box as a tuple of minimum and maximum points. + + :return: The axis-aligned bounding box as a tuple of minimum and maximum points + """ + return self.get_min(), self.get_max() + + def get_min(self) -> List[float]: """ Returns the minimum point of the axis-aligned bounding box. @@ -86,7 +125,7 @@ def get_min_point(self) -> List[float]: """ return [self.min_x, self.min_y, self.min_z] - def get_max_point(self) -> List[float]: + def get_max(self) -> List[float]: """ Returns the maximum point of the axis-aligned bounding box. From 17ed78f9c4a35f26195c6f1d9ba52f311c0574a1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 15 Dec 2023 12:01:37 +0100 Subject: [PATCH 20/69] [WorldAbstractClass] Created links dictionary inside Object class to point to link objects. Removed _current_link_poses and _current_link_transforms --- src/pycram/bullet_world.py | 4 -- src/pycram/world.py | 104 +++++++++++++++++-------------------- 2 files changed, 47 insertions(+), 61 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 1828de269..af139e7ed 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -142,10 +142,6 @@ def get_object_joint_position(self, obj: Object, joint_name: str) -> float: def get_object_link_pose(self, obj_id: int, link_id: int) -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. - The pose is given as a tuple of position and orientation. - - :param obj_id: The object - :param link_id: The name of the link """ return Pose(*p.getLinkState(obj_id, link_id, physicsClientId=self.client_id)[4:6]) diff --git a/src/pycram/world.py b/src/pycram/world.py index a8dd25aaa..7e4afdd55 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -348,7 +348,6 @@ def get_object_joint_position(self, obj: Object, joint_name: str) -> float: def get_object_link_pose(self, obj_id: int, link_id: int) -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. - The pose is given as a tuple of position and orientation. """ pass @@ -971,18 +970,21 @@ class Link: def __init__(self, _id: int, - obj_id: int, - obj_tf_frame: str, urdf_link: urdf_parser_py.urdf.Link, - world: World, - is_root: Optional[bool] = False): + obj: Object): self.id = _id - self.obj_id = obj_id - self.tf_frame = f"{obj_tf_frame}/{self.name}" if not is_root else obj_tf_frame self.urdf_link = urdf_link - self.world = world - self.object = world.get_object_by_id(obj_id) - self.local_transformer = self.object.local_transformer + self.object = obj + self.world = obj.world + self.tf_frame = f"{obj.tf_frame}/{urdf_link.name}" if not self.is_root else obj.tf_frame + self.local_transformer = LocalTransformer() + + @property + def is_root(self) -> bool: + return self.object.urdf_object.get_root() == self.urdf_link.name + + def update_transform(self, transform_time: Optional[rospy.Time] = None): + self.local_transformer.update_transforms([self.transform], transform_time) @property def transform(self) -> Transform: @@ -991,47 +993,47 @@ def transform(self) -> Transform: """ return self.pose.to_transform(self.tf_frame) - def get_transform_to_other_link(self, other_link: Link): - new_pose = self.local_transformer.transform_pose_to_target_frame(other_link.pose, self.tf_frame) - return new_pose.to_transform(other_link.tf_frame) + def calc_transform_to_link(self, link: Link): + return link.calc_transform_from_link(self) - def get_pose_wrt_other_link(self, other_link: Link): - return self.local_transformer.transform_pose_to_target_frame(self.pose, other_link.tf_frame) + def calc_transform_from_link(self, link: Link) -> Transform: + return self.calc_pose_wrt_link(link).to_transform(self.tf_frame) - def get_aabb(self) -> AxisAlignedBoundingBox: - return self.world.get_object_link_aabb(self.obj_id, self.id) + def calc_pose_wrt_link(self, link: Link) -> Pose: + return self.local_transformer.transform_pose_to_target_frame(self.pose, link.tf_frame) + + def calc_aabb(self) -> AxisAlignedBoundingBox: + return self.world.get_object_link_aabb(self.object.id, self.id) @property def position(self) -> Point: return self.pose.position @property - def orientation(self): + def orientation(self) -> Quaternion: return self.pose.orientation @property - def pose(self): + def pose(self) -> Pose: """ The pose of the link relative to the world frame. """ - return self.world.get_object_link_pose(self.obj_id, self.id) + if self.is_root: + return self.object.get_pose() + return self.world.get_object_link_pose(self.object.id, self.id) @property - def name(self): + def name(self) -> str: return self.urdf_link.name def get_geometry(self) -> GeometricType: return None if not self.collision else self.collision.geometry @property - def collision(self): + def collision(self) -> Collision: return self.urdf_link.collision - - - - class Object: """ Represents a spawned Object in the World. @@ -1045,13 +1047,16 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, """ The constructor loads the urdf file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. - The color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. + The color parameter is only used when loading .stl or .obj files, + for URDFs :func:`~Object.set_color` can be used. :param name: The name of the object :param obj_type: The type of the object - :param path: The path to the source file, if only a filename is provided then the resourcer directories will be searched + :param path: The path to the source file, if only a filename is provided then the resourcer directories will be + searched :param pose: The pose at which the Object should be spawned - :param world: The World in which the object should be spawned, if no world is specified the :py:attr:`~World.current_world` will be used + :param world: The World in which the object should be spawned, + if no world is specified the :py:attr:`~World.current_world` will be used. :param color: The color with which the object should be spawned. :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ @@ -1083,13 +1088,13 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.world.set_robot_if_not_set(self) self.link_name_to_id[self.urdf_object.get_root()] = -1 + self.links = self._init_links() self._current_pose = pose_in_map self._current_link_poses: Dict[str, Pose] = {} self._current_link_transforms: Dict[str, Transform] = {} self._current_joints_positions = {} self._init_current_positions_of_joint() - self._update_current_link_poses_and_transforms() self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) self.update_link_transforms() @@ -1097,6 +1102,14 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.world.objects.append(self) + def _init_links(self): + links = {} + for urdf_link in self.urdf_object.links: + link_name = urdf_link.name + link_id = self.link_name_to_id[link_name] + links[link_name] = Link(link_id, urdf_link, self) + return links + def _init_current_positions_of_joint(self) -> None: """ Initialize the cached joint position for each joint. @@ -1214,7 +1227,6 @@ def set_pose(self, pose: Pose, base: bool = False) -> None: position = np.array(position) + self.base_origin_shift self.world.reset_object_base_pose(self, position, orientation) self._current_pose = pose_in_map - self._update_current_link_poses_and_transforms() self.world._set_attached_objects(self, [self]) @property @@ -1403,9 +1415,8 @@ def get_link_pose(self, name: str) -> Pose: :param name: Link name for which a Pose should be returned :return: The pose of the link """ - if name in self.link_name_to_id.keys() and self.link_name_to_id[name] == -1: - return self.get_pose() - return self._current_link_poses[name] + link = self.links[name] + return link.pose if not link.is_root else self.get_pose() def get_link_pose_by_id(self, link_id: int) -> Pose: return self.get_link_pose(self.get_link_by_id(link_id)) @@ -1429,7 +1440,6 @@ def set_positions_of_all_joints(self, joint_poses: dict) -> None: for joint_name, joint_position in joint_poses.items(): self.world.reset_object_joint_position(self, joint_name, joint_position) self._current_joints_positions[joint_name] = joint_position - self._update_current_link_poses_and_transforms() self.world._set_attached_objects(self, [self]) def set_joint_position(self, joint_name: str, joint_position: float) -> None: @@ -1453,7 +1463,6 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: # return self.world.reset_object_joint_position(self, joint_name, joint_position) self._current_joints_positions[joint_name] = joint_position - self._update_current_link_poses_and_transforms() self.world._set_attached_objects(self, [self]) def get_joint_position(self, joint_name: str) -> float: @@ -1643,27 +1652,8 @@ def get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.GeometricType]: return link_to_geometry def update_link_transforms(self, transform_time: Optional[rospy.Time] = None): - self.local_transformer.update_transforms(self._current_link_transforms.values(), transform_time) - - def _update_current_link_poses_and_transforms(self) -> None: - """ - Updates the cached poses and transforms for each link of this Object - """ - for link_name in self.link_name_to_id.keys(): - if link_name == self.urdf_object.get_root(): - self._update_root_link_pose_and_transform() - else: - self._update_link_pose_and_transform(link_name) - - def _update_root_link_pose_and_transform(self) -> None: - link_name = self.urdf_object.get_root() - self._current_link_poses[link_name] = self._current_pose - self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame) - - def _update_link_pose_and_transform(self, link_name: str) -> None: - self._current_link_poses[link_name] = self.world.get_object_link_pose(self.id, self.get_link_id(link_name)) - self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform( - self.get_link_tf_frame(link_name)) + for link in self.links.values(): + link.update_transform(transform_time) def filter_contact_points(contact_points, exclude_ids) -> List: From 0e70fe2793cbe6136be8cf701bebab3d7ebb8fc4 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 15 Dec 2023 20:04:49 +0100 Subject: [PATCH 21/69] [WorldAbstractClass] removed all link related functionality from Object class to Link class. --- demos/pycram_bullet_world_demo/demo.py | 2 +- doc/source/notebooks/bullet_world.ipynb | 6 +- doc/source/notebooks/intro.ipynb | 1072 ++++++++++++++++- examples/bullet_world.ipynb | 6 +- examples/cram_plan_tutorial.ipynb | 14 +- examples/intro.ipynb | 2 +- examples/local_transformer.ipynb | 4 +- src/pycram/costmaps.py | 15 +- src/pycram/designators/action_designator.py | 8 +- src/pycram/designators/location_designator.py | 4 +- src/pycram/designators/object_designator.py | 2 +- src/pycram/external_interfaces/ik.py | 4 +- src/pycram/external_interfaces/robokudo.py | 2 +- src/pycram/helper.py | 9 +- src/pycram/pose.py | 2 +- src/pycram/pose_generator_and_validator.py | 12 +- .../process_modules/boxy_process_modules.py | 10 +- .../process_modules/donbot_process_modules.py | 6 +- .../process_modules/hsr_process_modules.py | 6 +- .../process_modules/pr2_process_modules.py | 15 +- src/pycram/ros/tf_broadcaster.py | 4 +- src/pycram/ros/viz_marker_publisher.py | 9 +- src/pycram/world.py | 190 +-- src/pycram/world_reasoning.py | 4 +- test/test_action_designator.py | 2 +- test/test_bullet_world.py | 4 +- test/test_bullet_world_reasoning.py | 4 +- 27 files changed, 1199 insertions(+), 219 deletions(-) mode change 120000 => 100644 doc/source/notebooks/intro.ipynb diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 36f908fbb..837c3a359 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -61,7 +61,7 @@ def move_and_detect(obj_type): spoon.detach(apartment) # Detect and pickup the spoon - LookAtAction([apartment.get_link_pose("handle_cab10_t")]).resolve().perform() + LookAtAction([apartment.links["handle_cab10_t"].pose]).resolve().perform() spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform() diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb index 538c2a475..39a9e1d8f 100644 --- a/doc/source/notebooks/bullet_world.ipynb +++ b/doc/source/notebooks/bullet_world.ipynb @@ -409,11 +409,11 @@ } ], "source": [ - "print(f\"Position: \\n{pr2.get_link_position('torso_lift_link')}\")\n", + "print(f\"Position: \\n{pr2.links['torso_lift_link'].position}\")\n", "\n", - "print(f\"Orientation: \\n{pr2.get_link_orientation('torso_lift_link')}\")\n", + "print(f\"Orientation: \\n{pr2.links['torso_lift_link'].orientation}\")\n", "\n", - "print(f\"Pose: \\n{pr2.get_link_pose('torso_lift_link')}\")" + "print(f\"Pose: \\n{pr2.links['torso_lift_link'].pose}\")" ] }, { diff --git a/doc/source/notebooks/intro.ipynb b/doc/source/notebooks/intro.ipynb deleted file mode 120000 index 31a4f6830..000000000 --- a/doc/source/notebooks/intro.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/intro.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/intro.ipynb b/doc/source/notebooks/intro.ipynb new file mode 100644 index 000000000..f5875cb3a --- /dev/null +++ b/doc/source/notebooks/intro.ipynb @@ -0,0 +1,1071 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# PyCRAM Presentation" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "ExecuteTime": { + "end_time": "2023-04-05T16:37:51.973404Z", + "start_time": "2023-04-05T16:37:51.483749Z" + } + }, + "outputs": [], + "source": [ + "import pycram" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Bullet World\n", + "\n", + "The BulletWorld is the internal simulation of PyCRAM. You can simulate different actions and reason about the outcome of different actions. \n", + "\n", + "It is possible to spawn objects and robots into the BulletWorld, these objects can come from URDF, OBJ or STL files. \n", + "\n", + "A BulletWorld can be created by simply creating an object of the BulletWorld class. " + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": { + "ExecuteTime": { + "end_time": "2023-04-05T16:37:59.788099Z", + "start_time": "2023-04-05T16:37:57.586879Z" + } + }, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", + "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", + "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", + "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" + ] + } + ], + "source": [ + "from pycram.bullet_world import BulletWorld, Object\n", + "from pycram.enums import ObjectType\n", + "from pycram.pose import Pose\n", + "\n", + "world = BulletWorld()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The world can also be closed with the 'exit' method" + ] + }, + { + "cell_type": "code", + "execution_count": 37, + "metadata": { + "ExecuteTime": { + "end_time": "2023-04-05T16:38:04.800655Z", + "start_time": "2023-04-05T16:38:03.943911Z" + } + }, + "outputs": [], + "source": [ + "world.exit()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The BulletWorld allows to render images from arbitrary positoins. In the following example we render images with the camera at the position [0.3, 0, 1] and pointing towards [1, 0, 1], so we are looking upwards along the x-axis. \n", + "\n", + "The renderer returns 3 different kinds of images which are also shown at the left side of the BulletWorld window. These images are:\n", + "* A RGB image which shows everything like it is rendered in the BulletWorld window, just from another perspective. \n", + "* A depth image which consists of distance values from the camera towards the objects in the field of view. \n", + "* A segmentation mask image which segments the image into the different objects displayed. The segmentation is done by assigning every pixel the unique id of the object that is displayed there. " + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[array([[[255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " ...,\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255]],\n", + " \n", + " [[255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " ...,\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255]],\n", + " \n", + " [[255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " ...,\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255],\n", + " [255, 255, 255, 255]],\n", + " \n", + " ...,\n", + " \n", + " [[239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " ...,\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255]],\n", + " \n", + " [[239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " ...,\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255]],\n", + " \n", + " [[239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " ...,\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255],\n", + " [239, 239, 239, 255]]], dtype=uint8),\n", + " array([[0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", + " 0.99999994],\n", + " [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", + " 0.99999994],\n", + " [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", + " 0.99999994],\n", + " ...,\n", + " [0.80473447, 0.80473447, 0.80473447, ..., 0.80473447, 0.80473447,\n", + " 0.80473447],\n", + " [0.8031688 , 0.8031688 , 0.8031688 , ..., 0.8031688 , 0.8031688 ,\n", + " 0.8031688 ],\n", + " [0.80160314, 0.80160314, 0.80160314, ..., 0.80160314, 0.80160314,\n", + " 0.80160314]], dtype=float32),\n", + " array([[-1, -1, -1, ..., -1, -1, -1],\n", + " [-1, -1, -1, ..., -1, -1, -1],\n", + " [-1, -1, -1, ..., -1, -1, -1],\n", + " ...,\n", + " [ 1, 1, 1, ..., 1, 1, 1],\n", + " [ 1, 1, 1, ..., 1, 1, 1],\n", + " [ 1, 1, 1, ..., 1, 1, 1]], dtype=int32)]" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "from pycram.bullet_world_reasoning import _get_images_for_target\n", + "\n", + "_get_images_for_target(Pose([1, 0, 1], [0, 0, 0, 1]), Pose([0.3, 0, 1], [0, 0, 0, 1]))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Objects\n", + "Everything that is located inside the BulletWorld is an Object. \n", + "Objects can be created from URDF, OBJ or STL files. Since everything is of type Object a robot might share the same methods as a milk (with some limitations).\n", + "\n", + "Signature:\n", + "Object:\n", + "* Name \n", + "* Type\n", + "* Filename or Filepath\n", + "\n", + " Optional:\n", + " * Position\n", + " * Orientation\n", + " * World \n", + " * Color \n", + " * Ignore Cached Files\n", + "\n", + "If there is only a filename and no path PyCRAM will check in the resource directory if there is a matching file. \n" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [], + "source": [ + "milk = Object(\"Milk\", ObjectType.MILK, \"milk.stl\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Objects provide methods to change the position and rotation, change the color, attach other objects, set the state of joints if the objects has any or get the position and orientation of a link. \n", + "\n", + "These methods are the same for every Object, however since some Objects may not have joints or more than one link methods related to these will not work. " + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [], + "source": [ + "milk.set_position(Pose([1, 0, 0]))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "To remove an Object from the BulletWorld just call the 'remove' method on the Object." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [], + "source": [ + "milk.remove()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Since everything inside the BulletWorld is an Object, even a complex environment Object like the kitchen can be spawned in the same way as the milk." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Scalar element defined multiple times: limit\n", + "Scalar element defined multiple times: limit\n" + ] + } + ], + "source": [ + "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Costmaps\n", + "\n", + "Costmaps are a way to get positions with respect to certain criterias. \n", + "The currently available costmaps are:\n", + "* Occupancy Costmap\n", + "* Visibility Costmap\n", + "* Semantic Costmap \n", + "* Gaussian Costmap\n", + "\n", + "It is also possible to merge multiple costmaps to combine different criteria." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Visibility Costmaps\n", + "Visibility costmaps determine every position, around a target position, from which the target is visible. Visibility Costmaps are able to work with cameras that are moveable in height, for example if the robot has a movable torso. " + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "import pycram.costmaps as cm\n", + "v = cm.VisibilityCostmap(1.27, 1.60, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1]))" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [], + "source": [ + "v.visualize()" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [], + "source": [ + "v.close_visualization()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Occupancy Costmap\n", + "Is valid for every position where the robot can be placed without colliding with an object." + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [], + "source": [ + "o = cm.OccupancyCostmap(0.2, from_ros=False, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1]))" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": {}, + "outputs": [], + "source": [ + "s = cm.SemanticCostmap(kitchen, \"kitchen_island_surface\", size=100, resolution=0.02)\n", + "\n", + "g = cm.GaussianCostmap(200, 15, resolution=0.02)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "You can visualize the costmap in the BulletWorld to get an impression what information is actually contained in the costmap. With this you could also check if the costmap was created correctly. \n", + "Visualization can be done via the 'visualize' method of each costmap." + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "metadata": {}, + "outputs": [], + "source": [ + "o.visualize()" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [], + "source": [ + "o.close_visualization()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "It is also possible to combine two costmap, this will result in a new costmap with the same size which contains the information of both previous costmaps. Combination is done by checking for each position in the two costmaps if they are zero, in this case to same position in the new costmap will also be zero in any other case the new position will be the normalized product of the two combined costmaps." + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [], + "source": [ + "ov = o + v" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "metadata": {}, + "outputs": [], + "source": [ + "ov.visualize()" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": {}, + "outputs": [], + "source": [ + "ov.close_visualization()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Bullet World Reasoning \n", + "Allows for geometric reasoning in the BulletWorld. At the moment the following types of reasoning are supported:\n", + "* Stable\n", + "* Contact\n", + "* Visible \n", + "* Occluding \n", + "* Reachable \n", + "* Blocking\n", + "* Supporting\n", + "\n", + "To show the geometric reasoning we first a robot as well as the milk Object again." + ] + }, + { + "cell_type": "code", + "execution_count": 19, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" + ] + } + ], + "source": [ + "import pycram.bullet_world_reasoning as btr\n", + "milk = Object(\"Milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1, 0, 1]))\n", + "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We start with testing for visibility " + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": { + "scrolled": true + }, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Milk visible: True\n" + ] + } + ], + "source": [ + "milk.set_position(Pose([1,0,1]))\n", + "visible = btr.visible(milk, pr2.links[\"wide_stereo_optical_frame\"].pose)\n", + "print(f\"Milk visible: {visible}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Milk is in contact with the floor: True\n" + ] + } + ], + "source": [ + "milk.set_position(Pose([1, 0, 0.05]))\n", + "\n", + "plane = BulletWorld.current_bullet_world.objects[0]\n", + "contact = btr.contact(milk, plane)\n", + "print(f\"Milk is in contact with the floor: {contact}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Milk is reachable for the PR2: True\n" + ] + } + ], + "source": [ + "milk.set_position(Pose([0.6, -0.5, 0.7]))\n", + "\n", + "reachable = btr.reachable(milk, pr2, \"r_gripper_tool_frame\")\n", + "print(f\"Milk is reachable for the PR2: {reachable}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Designator\n", + "Designator are symbolic descriptions of Actions, Motions, Objects or Locations. In PyCRAM the different types of designators are representet by a class which takes a description, the description then tells the designator what to do. \n", + "\n", + "For example, let's look at a Motion Designator to move the robot to a specific location. \n", + "\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Motion Designator\n", + "\n", + "When using a Motion Designator you need to specify which Process Module needs to be used, either the Process Module for the real or the simulated robot. This can be done either with a decorator which can be added to a function and then executes every designator in this function on the specified robot. The other possibility is a \"with\" scope which wraps a code piece. \n", + "\n", + "These two ways can also be combined, you could write a function which should be executed on the real robot and in the function is a \"with\" scope which executes something on the simulated robot for reasoning purposes. " + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "metadata": {}, + "outputs": [], + "source": [ + "from pycram.designators.motion_designator import *\n", + "from pycram.process_module import simulated_robot, with_simulated_robot\n", + "\n", + "description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1]))\n", + "\n", + "with simulated_robot:\n", + " description.resolve().perform()\n", + " \n" + ] + }, + { + "cell_type": "code", + "execution_count": 24, + "metadata": { + "scrolled": true + }, + "outputs": [], + "source": [ + "from pycram.process_module import with_simulated_robot\n", + "\n", + "@with_simulated_robot\n", + "def move():\n", + " MoveMotion(target=Pose([0, 0, 0], [0, 0, 0, 1])).resolve().perform()\n", + "\n", + "move()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Other implemented Motion Designator descriptions are:\n", + "* Pick up\n", + "* Place\n", + "* Accessing\n", + "* Move TCP\n", + "* Looking\n", + "* Move Gripper\n", + "* Detecting\n", + "* Move Arm Joint \n", + "* World State Detecting " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Object Designator\n", + "\n", + "Object Designator represent objects, these objects could either be from the BulletWorld or the real world. Object Designator are used, for example, by the PickUpAction to know which object should be picked up." + ] + }, + { + "cell_type": "code", + "execution_count": 25, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \n", + "local_transformer=, \n", + "name=Milk, \n", + "type=ObjectType.MILK, \n", + "color=[1, 1, 1, 1], \n", + "id=4, \n", + "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", + "joints: ..., \n", + "links: ..., \n", + "attachments: ..., \n", + "cids: ..., \n", + "original_pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1699448180\n", + " nsecs: 830921888\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 1.0\n", + " y: 0.0\n", + " z: 1.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0, \n", + "tf_frame=Milk_4, \n", + "urdf_object: ..., \n", + "_current_pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1699448184\n", + " nsecs: 735546827\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: -0.5\n", + " z: 0.7\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0, \n", + "_current_link_poses={'milk_main': header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1699448184\n", + " nsecs: 735546827\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: -0.5\n", + " z: 0.7\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0}, \n", + "_current_link_transforms={'milk_main': header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1699448185\n", + " nsecs: 354960680\n", + " frame_id: \"map\"\n", + "child_frame_id: \"Milk_4\"\n", + "transform: \n", + " translation: \n", + " x: 0.6\n", + " y: -0.5\n", + " z: 0.7\n", + " rotation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0}, \n", + "_current_joint_states={}, \n", + "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", + "link_to_geometry={'milk_main': }), _pose=, \n", + "local_transformer=, \n", + "name=Milk, \n", + "type=ObjectType.MILK, \n", + "color=[1, 1, 1, 1], \n", + "id=4, \n", + "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", + "joints: ..., \n", + "links: ..., \n", + "attachments: ..., \n", + "cids: ..., \n", + "original_pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1699448180\n", + " nsecs: 830921888\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 1.0\n", + " y: 0.0\n", + " z: 1.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0, \n", + "tf_frame=Milk_4, \n", + "urdf_object: ..., \n", + "_current_pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1699448184\n", + " nsecs: 735546827\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: -0.5\n", + " z: 0.7\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0, \n", + "_current_link_poses={'milk_main': header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1699448184\n", + " nsecs: 735546827\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: -0.5\n", + " z: 0.7\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0}, \n", + "_current_link_transforms={'milk_main': header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1699448185\n", + " nsecs: 354960680\n", + " frame_id: \"map\"\n", + "child_frame_id: \"Milk_4\"\n", + "transform: \n", + " translation: \n", + " x: 0.6\n", + " y: -0.5\n", + " z: 0.7\n", + " rotation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0\n", + " w: 1.0}, \n", + "_current_joint_states={}, \n", + "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", + "link_to_geometry={'milk_main': })>)" + ] + }, + "execution_count": 25, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "from pycram.designators.object_designator import *\n", + "\n", + "milk_desig = BelieveObject(names=[\"Milk\"])\n", + "milk_desig.resolve()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Location Designator\n", + "Location Designator can create a position in cartisian space from a symbolic desctiption" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "scrolled": false + }, + "outputs": [], + "source": [ + "from pycram.designators.location_designator import *\n", + "from pycram.designators.object_designator import *\n", + "\n", + "robot_desig = BelieveObject(types=[ObjectType.ROBOT]).resolve()\n", + "milk_desig = BelieveObject(names=[\"Milk\"]).resolve()\n", + "location_desig = CostmapLocation(target=milk_desig, visible_for=robot_desig)\n", + "\n", + "print(f\"Resolved: {location_desig.resolve()}\")\n", + "print()\n", + "\n", + "for pose in location_desig:\n", + " print(pose)\n", + " " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Action Designator\n", + "Action Designator are used to describe high-level actions. Action Designator are usually composed of other Designators to describe the high-level action in detail. " + ] + }, + { + "cell_type": "code", + "execution_count": 29, + "metadata": {}, + "outputs": [], + "source": [ + "from pycram.designators.action_designator import *\n", + "from pycram.enums import Arms\n", + "\n", + "with simulated_robot:\n", + " ParkArmsAction([Arms.BOTH]).resolve().perform()\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Making a simple plan\n", + "To get familiar with the PyCRAM Framework we will write a simple pick and place plan. This plan will let the robot grab a cereal box from the kitchen counter and place it on the kitchen island. This is a simple pick and place plan." + ] + }, + { + "cell_type": "code", + "execution_count": 30, + "metadata": {}, + "outputs": [], + "source": [ + "from pycram.designators.object_designator import *\n", + "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1.4, 1, 0.95]))\n" + ] + }, + { + "cell_type": "code", + "execution_count": 31, + "metadata": {}, + "outputs": [], + "source": [ + "cereal_desig = ObjectDesignatorDescription(names=[\"cereal\"])\n", + "kitchen_desig = ObjectDesignatorDescription(names=[\"kitchen\"])\n", + "robot_desig = ObjectDesignatorDescription(names=[\"pr2\"]).resolve()\n", + "with simulated_robot:\n", + " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", + "\n", + " MoveTorsoAction([0.3]).resolve().perform()\n", + "\n", + " pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()\n", + " pickup_arm = pickup_pose.reachable_arms[0]\n", + "\n", + " NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()\n", + "\n", + " PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[\"front\"]).resolve().perform()\n", + "\n", + " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", + "\n", + " place_island = SemanticCostmapLocation(\"kitchen_island_surface\", kitchen_desig.resolve(), cereal_desig.resolve()).resolve()\n", + "\n", + " place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve()\n", + "\n", + " NavigateAction(target_locations=[place_stand.pose]).resolve().perform()\n", + "\n", + " PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", + "\n", + " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", + " \n", + " \n", + " " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Task Trees\n", + "Task trees are a hirachical representation of all Actions involved in a plan. The Task tree can later be used to inspect and restructre the execution order of Actions in the plan." + ] + }, + { + "cell_type": "code", + "execution_count": 32, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "no_operation()\n", + "├── perform(ParkArmsAction, )\n", + "├── perform(ParkArmsAction, )\n", + "├── perform(MoveTorsoAction, )\n", + "├── perform(NavigateAction, )\n", + "├── perform(PickUpAction, )\n", + "├── perform(ParkArmsAction, )\n", + "├── perform(NavigateAction, )\n", + "├── perform(PlaceAction, )\n", + "└── perform(ParkArmsAction, )\n" + ] + } + ], + "source": [ + "import pycram.task\n", + "import anytree\n", + "tt = pycram.task.task_tree\n", + "print(anytree.RenderTree(tt))" + ] + }, + { + "cell_type": "code", + "execution_count": 33, + "metadata": {}, + "outputs": [], + "source": [ + "from anytree.dotexport import RenderTreeGraph, DotExporter\n", + "RenderTreeGraph(tt).to_picture(\"tree.png\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# ORM\n" + ] + }, + { + "cell_type": "code", + "execution_count": 34, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Inserting TaskTree into database: 100%|██████████| 10/10 [00:00<00:00, 143.50it/s]\n" + ] + }, + { + "data": { + "text/plain": [ + "pycram.orm.task.TaskTreeNode(1, 2023-11-08 13:55:09.816577, None, TaskStatus.RUNNING, None, None, 1, 1)" + ] + }, + "execution_count": 34, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "import sqlalchemy\n", + "import sqlalchemy.orm\n", + "import pycram.orm.base\n", + "import pycram.orm.task\n", + "import pycram.orm.object_designator\n", + "import pycram.orm.motion_designator\n", + "import pycram.orm.action_designator\n", + "\n", + "# set description of what we are doing\n", + "pycram.orm.base.MetaData().description = \"Tutorial for getting familiar with the ORM.\"\n", + "\n", + "engine = sqlalchemy.create_engine(\"sqlite+pysqlite:///:memory:\", echo=False)\n", + "session = sqlalchemy.orm.Session(bind=engine)\n", + "pycram.orm.base.Base.metadata.create_all(engine)\n", + "session.commit()\n", + "\n", + "\n", + "tt.insert(session)" + ] + }, + { + "cell_type": "code", + "execution_count": 35, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "pycram.orm.action_designator.NavigateAction(Navigate, 4, 4, 1, 4, 4)\n", + "pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9)\n" + ] + } + ], + "source": [ + "navigations = session.query(pycram.orm.action_designator.NavigateAction).all()\n", + "print(*navigations, sep=\"\\n\")" + ] + }, + { + "cell_type": "code", + "execution_count": 36, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(pycram.orm.action_designator.NavigateAction(Navigate, 4, 4, 1, 4, 4), pycram.orm.base.Position(0.5799999999999998, 1.28, 0.0, 4, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.16378361324016077, -0.9864962889104031, 4, None))\n", + "(pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9), pycram.orm.base.Position(-1.9074999952316287, 0.779200015068054, 0.0, 9, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.16439898730535735, 0.9863939238321437, 9, None))\n" + ] + } + ], + "source": [ + "navigations = session.query(pycram.orm.action_designator.NavigateAction, \n", + " pycram.orm.base.Position, \n", + " pycram.orm.base.Quaternion).\\\n", + " join(pycram.orm.base.Position).\\\n", + " join(pycram.orm.base.Quaternion).all()\n", + "print(*navigations, sep=\"\\n\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/bullet_world.ipynb b/examples/bullet_world.ipynb index 67016f4d2..110886ce6 100644 --- a/examples/bullet_world.ipynb +++ b/examples/bullet_world.ipynb @@ -351,11 +351,11 @@ } ], "source": [ - "print(f\"Position: \\n{pr2.get_link_position('torso_lift_link')}\")\n", + "print(f\"Position: \\n{pr2.links['torso_lift_link'].position}\")\n", "\n", - "print(f\"Orientation: \\n{pr2.get_link_orientation('torso_lift_link')}\")\n", + "print(f\"Orientation: \\n{pr2.links['torso_lift_link'].orientation}\")\n", "\n", - "print(f\"Pose: \\n{pr2.get_link_pose('torso_lift_link')}\")" + "print(f\"Pose: \\n{pr2.links['torso_lift_link'].pose}\")" ] }, { diff --git a/examples/cram_plan_tutorial.ipynb b/examples/cram_plan_tutorial.ipynb index d322d7be3..1777b511e 100644 --- a/examples/cram_plan_tutorial.ipynb +++ b/examples/cram_plan_tutorial.ipynb @@ -81,7 +81,7 @@ "robot_desig = ObjectDesignatorDescription(names=['pr2']).resolve()\n", "apartment = Object(\"apartment\", \"environment\", \"/home/abassi/cram_ws/src/iai_maps/iai_apartment/urdf/apartment.urdf\", position=[-1.5, -2.5, 0])\n", "apartment_desig = ObjectDesignatorDescription(names=['apartment']).resolve()\n", - "table_top = apartment.get_link_position(\"cooktop\")\n", + "table_top = apartment.links[\"cooktop\"].position\n", "# milk = Object(\"milk\", \"milk\", \"milk.stl\", position=[table_top[0]-0.15, table_top[1], table_top[2]])\n", "# milk.set_position(position=milk.get_position(), base=True)\n", "# cereal = Object(\"cereal\", \"cereal\", \"breakfast_cereal.stl\", position=table_top)\n", @@ -213,8 +213,8 @@ "source": [ "import pybullet as p\n", "for link_name in apartment.links.keys():\n", - " world.add_vis_axis(apartment.get_link_position_and_orientation(link_name))\n", - " p.addUserDebugText(link_name, apartment.get_link_position(link_name))" + " world.add_vis_axis(apartment.links[link_name].pose)\n", + " p.addUserDebugText(link_name, apartment.links[link_name].position)" ] }, { @@ -720,10 +720,10 @@ "evalue": "name 'world' is not defined", "output_type": "error", "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mNameError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[1], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[43mworld\u001b[49m\u001b[38;5;241m.\u001b[39mexit()\n", - "\u001b[0;31mNameError\u001b[0m: name 'world' is not defined" + "\u001B[0;31m---------------------------------------------------------------------------\u001B[0m", + "\u001B[0;31mNameError\u001B[0m Traceback (most recent call last)", + "Cell \u001B[0;32mIn[1], line 1\u001B[0m\n\u001B[0;32m----> 1\u001B[0m \u001B[43mworld\u001B[49m\u001B[38;5;241m.\u001B[39mexit()\n", + "\u001B[0;31mNameError\u001B[0m: name 'world' is not defined" ] } ], diff --git a/examples/intro.ipynb b/examples/intro.ipynb index aea7c14fb..f5875cb3a 100644 --- a/examples/intro.ipynb +++ b/examples/intro.ipynb @@ -491,7 +491,7 @@ ], "source": [ "milk.set_position(Pose([1,0,1]))\n", - "visible = btr.visible(milk, pr2.get_link_pose(\"wide_stereo_optical_frame\"))\n", + "visible = btr.visible(milk, pr2.links[\"wide_stereo_optical_frame\"].pose)\n", "print(f\"Milk visible: {visible}\")" ] }, diff --git a/examples/local_transformer.ipynb b/examples/local_transformer.ipynb index 31017bebb..4ff40178e 100644 --- a/examples/local_transformer.ipynb +++ b/examples/local_transformer.ipynb @@ -298,7 +298,7 @@ "\n", "Furthermore, links of an Object are represented by the Object frame_id + '/' + link name. Since link names need to be unique for an URDF this is no problem.\n", "\n", - "These frames need to be used in whenever you are transforming something with the local transformer. To get the base frame of an Object, meaning the frame name without any link there is the attribute tf_frame and for the frame of a link there is the method get_link_tf_frame. " + "These frames need to be used in whenever you are transforming something with the local transformer. To get the base frame of an Object, meaning the frame name without any link there is the attribute tf_frame and for the frame of a link there is the collection links from which you can access all link objects by name, link objects also have the attribute tf_frame which gives the tf_frame of the link. " ] }, { @@ -319,7 +319,7 @@ "source": [ "print(milk.tf_frame)\n", "\n", - "print(kitchen.get_link_tf_frame(\"kitchen_island_surface\"))" + "print(kitchen.links[\"kitchen_island_surface\"].tf_frame)" ] } ], diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index bc21b122d..b0d2f5ccb 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -9,7 +9,7 @@ import psutil import time from .bullet_world import BulletWorld -from pycram.world import UseProspectionWorld, Object +from pycram.world import UseProspectionWorld, Object, Link from .world_reasoning import _get_images_for_target from .world_dataclasses import AxisAlignedBoundingBox from nav_msgs.msg import OccupancyGrid, MapMetaData @@ -699,9 +699,9 @@ def __init__(self, object, urdf_link_name, size=100, resolution=0.02, world=None """ self.world: BulletWorld = world if world else BulletWorld.current_world self.object: Object = object - self.link: str = urdf_link_name + self.link: Link = object.links[urdf_link_name] self.resolution: float = resolution - self.origin: Pose = object.get_link_pose(urdf_link_name) + self.origin: Pose = object.links[urdf_link_name].pose self.height: int = 0 self.width: int = 0 self.map: np.ndarray = [] @@ -730,11 +730,10 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox: prospection_object = BulletWorld.current_world.get_prospection_object_from_object(self.object) with UseProspectionWorld(): prospection_object.set_orientation(Pose(orientation=[0, 0, 0, 1])) - link_orientation = prospection_object.get_link_pose(self.link) - link_orientation_trans = link_orientation.to_transform(self.object.get_link_tf_frame(self.link)) - inverse_orientation = link_orientation_trans.invert() - prospection_object.set_orientation(inverse_orientation.to_pose()) - return prospection_object.get_link_aabb(self.object.get_link_id(self.link)) + link_pose_trans = self.link.transform + inverse_trans = link_pose_trans.invert() + prospection_object.set_orientation(inverse_trans.to_pose()) + return self.link.get_aabb() cmap = colors.ListedColormap(['white', 'black', 'green', 'red', 'blue']) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index deeb06d68..1bb027ca0 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -328,7 +328,7 @@ def perform(self) -> None: # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" - gripper_frame = robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm)) + gripper_frame = robot.links[robot_description.get_tool_frame(self.arm)].tf_frame # First rotate the gripper, so the further calculations makes sense tmp_for_rotate_pose = object.local_transformer.transform_pose_to_target_frame(adjusted_oTm, gripper_frame) tmp_for_rotate_pose.pose.position.x = 0 @@ -436,9 +436,9 @@ def perform(self) -> None: local_tf = LocalTransformer() # Transformations such that the target position is the position of the object and not the tcp + tool_name = robot_description.get_tool_frame(self.arm) tcp_to_object = local_tf.transform_pose_to_target_frame(object_pose, - BulletWorld.robot.get_link_tf_frame( - robot_description.get_tool_frame(self.arm))) + BulletWorld.robot.links[tool_name].tf_frame) target_diff = self.target_location.to_transform("target").inverse_times( tcp_to_object.to_transform("object")).to_pose() @@ -887,7 +887,7 @@ def perform(self) -> Any: gripper_name = robot_description.get_tool_frame(self.arm) object_pose_in_gripper = lt.transform_pose_to_target_frame(object_pose, - BulletWorld.robot.get_link_tf_frame(gripper_name)) + BulletWorld.robot.links[gripper_name].tf_frame) pre_grasp = object_pose_in_gripper.copy() pre_grasp.pose.position.x -= 0.1 diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index a8f2ab689..d726f8957 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -339,8 +339,8 @@ def __iter__(self): sem_costmap = SemanticCostmap(self.part_of.bullet_world_object, self.urdf_link_name) height_offset = 0 if self.for_object: - min, max = self.for_object.bullet_world_object.get_aabb() - height_offset = (max[2] - min[2]) / 2 + min_p, max_p = self.for_object.bullet_world_object.get_aabb().get_min_max_points() + height_offset = (max_p.z - min_p.z) / 2 for maybe_pose in pose_generator(sem_costmap): maybe_pose.position.z += height_offset yield self.Location(maybe_pose) diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 28f0e8cc9..7ba1491b3 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -97,7 +97,7 @@ def __iter__(self): for name in self.names: if name in self.part_of.bullet_world_object.link_name_to_id.keys(): yield self.Object(name, self.type, self.part_of.bullet_world_object, - self.part_of.bullet_world_object.get_link_pose(name)) + self.part_of.bullet_world_object.links[name].pose) class LocatedObject(ObjectDesignatorDescription): diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 8a5dc60bb..f0aeaac94 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -45,7 +45,7 @@ def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_ob :return: A moveit_msgs/PositionIKRequest message containing all the information from the parameter """ local_transformer = LocalTransformer() - target_pose = local_transformer.transform_pose_to_target_frame(target_pose, robot_object.get_link_tf_frame(root_link)) + target_pose = local_transformer.transform_pose_to_target_frame(target_pose, robot_object.links[root_link].tf_frame) robot_state = RobotState() joint_state = JointState() @@ -124,7 +124,7 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str # Get link after last joint in chain end_effector = robot_description.get_child(joints[-1]) - target_torso = local_transformer.transform_pose_to_target_frame(target_pose, robot.get_link_tf_frame(base_link)) + target_torso = local_transformer.transform_pose_to_target_frame(target_pose, robot.links[base_link].tf_frame) # target_torso = _transform_to_torso(pose, shadow_robot) diff = calculate_wrist_tool_offset(end_effector, gripper, robot) diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index bbfcadd50..b2574b55b 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -125,7 +125,7 @@ def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescripti for i in range(0, len(query_result.res[0].pose)): pose = Pose.from_pose_stamped(query_result.res[0].pose[i]) - pose.frame = BulletWorld.current_world.robot.get_link_tf_frame(pose.frame) + pose.frame = BulletWorld.current_world.robot.links[pose.frame].tf_frame # TODO: pose.frame is a link name? source = query_result.res[0].poseSource[i] lt = LocalTransformer() diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 719fd040c..c098eca99 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -61,9 +61,7 @@ def _apply_ik(robot: BulletWorldObject, joint_poses: List[float], joints: List[s def _transform_to_torso(pose_and_rotation: Tuple[List[float], List[float]], robot: BulletWorldObject) -> Tuple[ List[float], List[float]]: - # map_T_torso = robot.get_link_position_and_orientation("base_footprint") - # map_T_torso = robot.get_position_and_orientation() - map_T_torso = robot.get_link_pose(robot_description.torso_link).to_list() + map_T_torso = robot.links[robot_description.torso_link].pose_as_list torso_T_map = p.invertTransform(map_T_torso[0], map_T_torso[1]) map_T_target = pose_and_rotation torso_T_target = p.multiplyTransforms(torso_T_map[0], torso_T_map[1], map_T_target[0], map_T_target[1]) @@ -71,10 +69,7 @@ def _transform_to_torso(pose_and_rotation: Tuple[List[float], List[float]], robo def calculate_wrist_tool_offset(wrist_frame: str, tool_frame: str, robot: BulletWorldObject) -> Transform: - local_transformer = LocalTransformer() - tool_pose = robot.get_link_pose(tool_frame) - wrist_to_tool = local_transformer.transform_pose_to_target_frame(tool_pose, robot.get_link_tf_frame(wrist_frame)) - return wrist_to_tool.to_transform(robot.get_link_tf_frame(tool_frame)) + return robot.links[wrist_frame].get_transform_to_link(robot.links[tool_frame]) def inverseTimes(transform1: Tuple[List[float], List[float]], transform2: Tuple[List[float], List[float]]) -> Tuple[ diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 2ddbbfe7c..e707aaa61 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -87,7 +87,7 @@ def frame(self, value: str) -> None: self.header.frame_id = value @property - def position(self) -> Union[Point,GeoPose]: + def position(self) -> GeoPose: """ Property that points to the position of this pose """ diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 5ae4b0b9d..e94805a29 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -97,14 +97,14 @@ def visibility_validator(pose: Pose, robot_pose = robot.get_pose() if type(object_or_pose) == Object: robot.set_pose(pose) - camera_pose = robot.get_link_pose(robot_description.get_camera_frame()) + camera_pose = robot.links[robot_description.get_camera_frame()].pose robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) ray = p.rayTest(camera_pose.position_as_list(), object_or_pose.get_pose().position_as_list(), physicsClientId=world.client_id) res = ray[0][0] == object_or_pose.id else: robot.set_pose(pose) - camera_pose = robot.get_link_pose(robot_description.get_camera_frame()) + camera_pose = robot.links[robot_description.get_camera_frame()].pose robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) ray = p.rayTest(camera_pose.position_as_list(), object_or_pose, physicsClientId=world.client_id) res = ray[0][0] == -1 @@ -155,7 +155,6 @@ def reachability_validator(pose: Pose, try: # resp = request_ik(base_link, end_effector, target_diff, robot, left_joints) resp = request_ik(target, robot, left_joints, left_gripper) - _apply_ik(robot, resp, left_joints) for obj in BulletWorld.current_world.objects: @@ -166,8 +165,7 @@ def reachability_validator(pose: Pose, if in_contact: for link in contact_links: - - if link[0] in allowed_robot_links or link[1] in allowed_links: + if link[0].name in allowed_robot_links or link[1].name in allowed_links: in_contact = False if not in_contact: @@ -179,7 +177,6 @@ def reachability_validator(pose: Pose, try: # resp = request_ik(base_link, end_effector, target_diff, robot, right_joints) resp = request_ik(target, robot, right_joints, right_gripper) - _apply_ik(robot, resp, right_joints) for obj in BulletWorld.current_world.objects: @@ -190,8 +187,7 @@ def reachability_validator(pose: Pose, if in_contact: for link in contact_links: - - if link[0] in allowed_robot_links or link[1] in allowed_links: + if link[0].name in allowed_robot_links or link[1].name in allowed_links: in_contact = False if not in_contact: diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 2e8fc74f3..ad2932322 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -113,16 +113,14 @@ def _execute(self, desig): robot.set_joint_position(robot_description.torso_joint, -0.1) arm = "left" if solution['gripper'] == robot_description.get_tool_frame("left") else "right" joints = robot_description._safely_access_chains(arm).joints - #inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), kitchen.get_link_position(drawer_handle)) - target = helper._transform_to_torso(kitchen.get_link_position_and_orientation(drawer_handle), robot) - #target = (target[0], [0, 0, 0, 1]) + target = helper._transform_to_torso(kitchen.links[drawer_handle].pose, robot) inv = request_ik(robot_description.base_frame, gripper, target , robot, joints ) helper._apply_ik(robot, inv, gripper) time.sleep(0.2) cur_pose = robot.get_pose() robot.set_position([cur_pose[0]-dis, cur_pose[1], cur_pose[2]]) - han_pose = kitchen.get_link_position(drawer_handle) - new_p = [[han_pose[0] - dis, han_pose[1], han_pose[2]], kitchen.get_link_orientation(drawer_handle)] + han_pose = kitchen.links[drawer_handle].position + new_p = [[han_pose[0] - dis, han_pose[1], han_pose[2]], kitchen.links[drawer_handle].orientation] new_p = helper._transform_to_torso(new_p, robot) inv = request_ik(robot_description.base_frame, gripper, new_p, robot, joints) helper._apply_ik(robot, inv, gripper) @@ -227,7 +225,7 @@ def _execute(self, desig): objects = BulletWorld.current_world.get_objects_by_type(object_type) for obj in objects: - if btr.visible(obj, robot.get_link_position_and_orientation(cam_frame_name), front_facing_axis, 0.5): + if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis, 0.5): return obj diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index dfc5dc7ed..fd103e5eb 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -95,10 +95,10 @@ def _execute(self, desig): drawer_joint = solution['drawer_joint'] dis = solution['distance'] inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), - kitchen.get_link_position(drawer_handle)) + kitchen.links[drawer_handle].position) helper._apply_ik(robot, inv) time.sleep(0.2) - han_pose = kitchen.get_link_position(drawer_handle) + han_pose = kitchen.links[drawer_handle].position new_p = [han_pose[0] - dis, han_pose[1], han_pose[2]] inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), new_p) helper._apply_ik(robot, inv) @@ -190,7 +190,7 @@ def _execute(self, desig): objects = BulletWorld.current_world.get_objects_by_type(object_type) for obj in objects: - if btr.visible(obj, robot.get_link_position_and_orientation(cam_frame_name), front_facing_axis, 0.5): + if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis, 0.5): return obj diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 974ef8ac3..02ca0edcf 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -89,10 +89,10 @@ def _execute(self, desig): drawer_joint = solution['drawer_joint'] dis = solution['distance'] inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), - kitchen.get_link_position(drawer_handle)) + kitchen.links[drawer_handle].position) _apply_ik(robot, inv) time.sleep(0.2) - han_pose = kitchen.get_link_position(drawer_handle) + han_pose = kitchen.links[drawer_handle].position new_p = [han_pose[0] - dis, han_pose[1], han_pose[2]] inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), new_p) _apply_ik(robot, inv) @@ -163,7 +163,7 @@ def _execute(self, desig): objects = BulletWorld.current_world.get_objects_by_type(object_type) for obj in objects: - if btr.visible(obj, robot.get_link_position_and_orientation(cam_frame_name), front_facing_axis, 0.5): + if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis, 0.5): return obj diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 253d37df1..8c96af6e7 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -97,7 +97,8 @@ def _execute(self, desig: PlaceMotion.Motion): # Transformations such that the target position is the position of the object and not the tcp object_pose = object.get_pose() local_tf = LocalTransformer() - tcp_to_object = local_tf.transform_pose_to_target_frame(object_pose, robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) + tool_name = robot_description.get_tool_frame(arm) + tcp_to_object = local_tf.transform_pose_to_target_frame(object_pose, robot.links[tool_name].tf_frame) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, arm) @@ -115,8 +116,8 @@ def _execute(self, desig: LookingMotion.Motion): robot = BulletWorld.robot local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose_to_target_frame(target, robot.get_link_tf_frame("head_pan_link")) - pose_in_tilt = local_transformer.transform_pose_to_target_frame(target, robot.get_link_tf_frame("head_tilt_link")) + pose_in_pan = local_transformer.transform_pose_to_target_frame(target, robot.links["head_pan_link"].tf_frame) + pose_in_tilt = local_transformer.transform_pose_to_target_frame(target, robot.links["head_tilt_link"].tf_frame) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 @@ -158,7 +159,7 @@ def _execute(self, desig: DetectingMotion.Motion): objects = BulletWorld.current_world.get_objects_by_type(object_type) for obj in objects: - if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): + if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis): return obj @@ -295,8 +296,8 @@ def _execute(self, desig: LookingMotion.Motion): robot = BulletWorld.robot local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose_to_target_frame(target, robot.get_link_tf_frame("head_pan_link")) - pose_in_tilt = local_transformer.transform_pose_to_target_frame(target, robot.get_link_tf_frame("head_tilt_link")) + pose_in_pan = local_transformer.transform_pose_to_target_frame(target, robot.links["head_pan_link"].tf_frame) + pose_in_tilt = local_transformer.transform_pose_to_target_frame(target, robot.links["head_tilt_link"].tf_frame) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 @@ -321,7 +322,7 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: obj_pose = query_result["ClusterPoseBBAnnotator"] lt = LocalTransformer() - obj_pose = lt.transform_pose_to_target_frame(obj_pose, BulletWorld.robot.get_link_tf_frame("torso_lift_link")) + obj_pose = lt.transform_pose_to_target_frame(obj_pose, BulletWorld.robot.links["torso_lift_link"].tf_frame) obj_pose.orientation = [0, 0, 0, 1] obj_pose.position.x += 0.05 diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index 1eb0e5943..cde266ef1 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -54,8 +54,8 @@ def _update_objects(self) -> None: pose = obj.get_pose() self._publish_pose(obj.tf_frame, pose) for link in obj.link_name_to_id.keys(): - link_pose = obj.get_link_pose(link) - self._publish_pose(obj.get_link_tf_frame(link), link_pose) + link_pose = obj.links[link].pose + self._publish_pose(obj.links[link].tf_frame, link_pose) def _update_static_odom(self) -> None: """ diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index b91ed879a..9d629aa52 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -60,7 +60,7 @@ def _make_marker_array(self) -> MarkerArray: if obj.name == "floor": continue for link in obj.link_name_to_id.keys(): - geom = obj.link_to_geometry[link] + geom = obj.links[link].get_geometry() if not geom: continue msg = Marker() @@ -69,10 +69,9 @@ def _make_marker_array(self) -> MarkerArray: msg.id = obj.link_name_to_id[link] msg.type = Marker.MESH_RESOURCE msg.action = Marker.ADD - link_pose = obj.get_link_pose(link).to_transform(link) - if obj.urdf_object.link_map[link].collision.origin: - link_origin = Transform(obj.urdf_object.link_map[link].collision.origin.xyz, - list(quaternion_from_euler(*obj.urdf_object.link_map[link].collision.origin.rpy))) + link_pose = obj.links[link].transform + if obj.links[link].origin: + link_origin = obj.links[link].get_origin_transform() else: link_origin = Transform() link_pose_with_origin = link_pose * link_origin diff --git a/src/pycram/world.py b/src/pycram/world.py index 7e4afdd55..671fbfaf3 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -10,6 +10,7 @@ import xml.etree.ElementTree from queue import Queue import tf +from tf.transformations import quaternion_from_euler from typing import List, Optional, Dict, Tuple, Callable from typing import Union @@ -993,26 +994,34 @@ def transform(self) -> Transform: """ return self.pose.to_transform(self.tf_frame) - def calc_transform_to_link(self, link: Link): - return link.calc_transform_from_link(self) + def get_transform_to_link(self, link: Link): + return link.get_transform_from_link(self) - def calc_transform_from_link(self, link: Link) -> Transform: - return self.calc_pose_wrt_link(link).to_transform(self.tf_frame) + def get_transform_from_link(self, link: Link) -> Transform: + return self.get_pose_wrt_link(link).to_transform(self.tf_frame) - def calc_pose_wrt_link(self, link: Link) -> Pose: + def get_pose_wrt_link(self, link: Link) -> Pose: return self.local_transformer.transform_pose_to_target_frame(self.pose, link.tf_frame) - def calc_aabb(self) -> AxisAlignedBoundingBox: + def get_aabb(self) -> AxisAlignedBoundingBox: return self.world.get_object_link_aabb(self.object.id, self.id) @property def position(self) -> Point: return self.pose.position + @property + def position_as_list(self) -> List[float]: + return self.pose.position_as_list() + @property def orientation(self) -> Quaternion: return self.pose.orientation + @property + def orientation_as_list(self) -> List[float]: + return self.pose.orientation_as_list() + @property def pose(self) -> Pose: """ @@ -1022,6 +1031,10 @@ def pose(self) -> Pose: return self.object.get_pose() return self.world.get_object_link_pose(self.object.id, self.id) + @property + def pose_as_list(self) -> List[List[float]]: + return self.pose.to_list() + @property def name(self) -> str: return self.urdf_link.name @@ -1029,6 +1042,13 @@ def name(self) -> str: def get_geometry(self) -> GeometricType: return None if not self.collision else self.collision.geometry + def get_origin_transform(self): + return Transform(self.origin.xyz, list(quaternion_from_euler(*self.origin.rpy))) + + @property + def origin(self): + return self.collision.origin + @property def collision(self) -> Collision: return self.urdf_link.collision @@ -1072,6 +1092,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignore_cached_files) self.joint_name_to_id: Dict[str, int] = self._get_joint_name_to_id_map() self.link_name_to_id: Dict[str, int] = self._get_link_name_to_id_map() + self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) self.attachments: Dict[Object, Attachment] = {} self.cids: Dict[Object, int] = {} self.original_pose = pose_in_map @@ -1088,17 +1109,13 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.world.set_robot_if_not_set(self) self.link_name_to_id[self.urdf_object.get_root()] = -1 + self.link_id_to_name[-1] = self.urdf_object.get_root() self.links = self._init_links() self._current_pose = pose_in_map - self._current_link_poses: Dict[str, Pose] = {} - self._current_link_transforms: Dict[str, Transform] = {} self._current_joints_positions = {} self._init_current_positions_of_joint() - - self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) self.update_link_transforms() - self.link_to_geometry = self.get_geometry_for_link() self.world.objects.append(self) @@ -1117,9 +1134,15 @@ def _init_current_positions_of_joint(self) -> None: for joint_name in self.joint_name_to_id.keys(): self._current_joints_positions[joint_name] = self.world.get_object_joint_position(self, joint_name) + @property + def base_origin_shift(self) -> np.ndarray: + """ + The shift between the base of the object and the origin of the object. + """ + return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) + def __repr__(self): - skip_attr = ["links", "joints", "urdf_object", "attachments", "cids", "_current_link_poses", - "_current_link_transforms", "link_to_geometry"] + skip_attr = ["links", "joints", "urdf_object", "attachments", "cids"] return self.__class__.__qualname__ + f"(" + ', \n'.join( [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" @@ -1266,18 +1289,6 @@ def _set_attached_objects(self, prev_object: List[Object]) -> None: """ self.world._set_attached_objects(self, prev_object) - def _calculate_transform_from_other_object_base_to_this_object_base(self, other_object: Object): - pose_wrt_this_object_base = self.local_transformer.transform_pose_to_target_frame(other_object.pose, - self.tf_frame) - return Transform.from_pose_and_child_frame(pose_wrt_this_object_base, other_object.tf_frame) - - def transform_pose_to_link_frame(self, pose: Pose, link_name: str) -> Union[Pose, None]: - """ - :return: The new pose transformed to be relative to the link coordinate frame. - """ - target_frame = self.get_link_tf_frame(link_name) - return self.local_transformer.transform_pose_to_target_frame(pose, target_frame) - def set_position(self, position: Union[Pose, Point], base=False) -> None: """ Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position @@ -1341,25 +1352,22 @@ def get_joint_id(self, name: str) -> int: """ return self.joint_name_to_id[name] - def get_link_id(self, name: str) -> int: + def get_link_id(self, link_name: str) -> int: """ - Returns a unique id for a link name. If the name is None return -1. - - :param name: The link name - :return: The unique id + Returns a unique id for a link name. """ - if name is None: - return -1 - return self.link_name_to_id[name] + if link_name is None: + return self.get_root_link_id() + return self.link_name_to_id[link_name] - def get_link_by_id(self, id: int) -> str: - """ - Returns the name of a link for a given unique link id + def get_root_link_id(self) -> int: + return self.get_link_id(self.urdf_object.get_root()) - :param id: id for link - :return: The link name + def get_link_by_id(self, link_id: int) -> Link: + """ + Returns the link for a given unique link id """ - return dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys()))[id] + return self.links[self.link_id_to_name[link_id]] def get_joint_by_id(self, joint_id: int) -> str: """ @@ -1370,57 +1378,6 @@ def get_joint_by_id(self, joint_id: int) -> str: """ return dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys()))[joint_id] - def get_other_object_link_pose_relative_to_my_link(self, - my_link_id: int, - other_link_id: int, - other_object: Object) -> Pose: - """ - Calculates the pose of a link (child_link) in the coordinate frame of another link (parent_link). - :return: The pose of the source frame in the target frame - """ - - child_link_pose = other_object.get_link_pose(other_object.get_link_by_id(other_link_id)) - parent_link_frame = self.get_link_tf_frame(self.get_link_by_id(my_link_id)) - return self.local_transformer.transform_pose_to_target_frame(child_link_pose, parent_link_frame) - - def get_transform_from_my_link_to_other_object_link(self, - my_link_id: int, - other_link_id: int, - other_object: Object) -> Transform: - pose = self.get_other_object_link_pose_relative_to_my_link(my_link_id, other_link_id, other_object) - return pose.to_transform(other_object.get_link_tf_frame_by_id(other_link_id)) - - def get_link_position(self, name: str) -> Pose.position: - """ - Returns the position of a link of this Object. Position is returned as a list of xyz. - - :param name: The link name - :return: The link position as xyz - """ - return self.get_link_pose(name).position - - def get_link_orientation(self, name: str) -> Quaternion: - """ - Returns the orientation of a link of this Object. Orientation is returned as a quaternion. - - :param name: The name of the link - :return: The orientation of the link as a quaternion - """ - return self.get_link_pose(name).orientation - - def get_link_pose(self, name: str) -> Pose: - """ - Returns a Pose of the link corresponding to the given name. The returned Pose will be in world coordinate frame. - - :param name: Link name for which a Pose should be returned - :return: The pose of the link - """ - link = self.links[name] - return link.pose if not link.is_root else self.get_pose() - - def get_link_pose_by_id(self, link_id: int) -> Pose: - return self.get_link_pose(self.get_link_by_id(link_id)) - 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 @@ -1546,16 +1503,13 @@ def get_color(self, link: Optional[str] = None) -> Union[Color, Dict[str, Color] def get_aabb(self) -> AxisAlignedBoundingBox: return self.world.get_object_aabb(self) - def get_link_aabb(self, link_id: int) -> AxisAlignedBoundingBox: - return self.world.get_object_link_aabb(self.id, link_id) - def get_base_origin(self) -> Pose: """ Returns the origin of the base/bottom of an object :return: The position of the bottom of this Object """ - aabb = self.get_link_aabb(-1) + aabb = self.get_link_by_id(-1).get_aabb() base_width = np.absolute(aabb.min_x - aabb.max_x) base_length = np.absolute(aabb.min_y - aabb.max_y) return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], @@ -1621,36 +1575,6 @@ def get_positions_of_all_joints(self) -> Dict[str: float]: """ return self._current_joints_positions - def get_link_tf_frame(self, link_name: str) -> str: - """ - Returns the name of the tf frame for the given link name. This method does not check if the given name is - actually a link of this object. - - :param link_name: Name of a link for which the tf frame should be returned - :return: A TF frame name for a specific link - """ - if link_name == self.urdf_object.get_root(): - return self.tf_frame - return self.tf_frame + "/" + link_name - - def get_link_tf_frame_by_id(self, link_id: int) -> str: - return self.get_link_tf_frame(self.get_link_by_id(link_id)) - - def get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.GeometricType]: - """ - Extracts the geometry information for each collision of each link and links them to the respective link. - - :return: A dictionary with link name as key and geometry information as value - """ - link_to_geometry = {} - for link in self.link_name_to_id.keys(): - link_obj = self.urdf_object.link_map[link] - if not link_obj.collision: - link_to_geometry[link] = None - else: - link_to_geometry[link] = link_obj.collision.geometry - return link_to_geometry - def update_link_transforms(self, transform_time: Optional[rospy.Time] = None): for link in self.links.values(): link.update_transform(transform_time) @@ -1711,10 +1635,10 @@ def __init__(self, """ self.parent_object = parent_object self.child_object = child_object - self.parent_link_id = parent_link_id - self.child_link_id = child_link_id + self.parent_link = parent_object.get_link_by_id(parent_link_id) + self.child_link = child_object.get_link_by_id(child_link_id) self.bidirectional = bidirectional - self._loose = False and not self.bidirectional + self._loose = False and not bidirectional self.child_to_parent_transform = child_to_parent_transform if self.child_to_parent_transform is None: @@ -1736,9 +1660,7 @@ def update_constraint(self): self.add_constraint_and_update_objects_constraints_collection() def calculate_transform(self): - return self.parent_object.get_transform_from_my_link_to_other_object_link(self.parent_link_id, - self.child_link_id, - self.child_object) + return self.parent_link.get_transform_to_link(self.child_link) def remove_constraint_if_exists(self): if self.constraint_id is not None: @@ -1752,8 +1674,8 @@ def add_fixed_constraint(self): constraint_id = self.parent_object.world.add_fixed_constraint(self.parent_object.id, self.child_object.id, self.child_to_parent_transform, - self.parent_link_id, - self.child_link_id) + self.parent_link.id, + self.child_link.id) return constraint_id def update_objects_constraints_collection(self): @@ -1761,8 +1683,8 @@ def update_objects_constraints_collection(self): self.child_object.cids[self.parent_object] = self.constraint_id def get_inverse(self): - attachment = Attachment(self.child_object, self.parent_object, self.child_link_id, - self.parent_link_id, self.bidirectional, self.child_to_parent_transform.invert(), + attachment = Attachment(self.child_object, self.parent_object, self.child_link.id, + self.parent_link.id, self.bidirectional, self.child_to_parent_transform.invert(), self.constraint_id) attachment.loose = False if self.loose else True return attachment diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 9847100ba..c6e696ca5 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -279,7 +279,7 @@ def reachable(pose: Union[Object, Pose], _apply_ik(shadow_robot, inv, joints) - diff = pose.dist(shadow_robot.get_link_pose(gripper_name)) + diff = pose.dist(shadow_robot.links[gripper_name].pose) return diff < threshold @@ -362,4 +362,4 @@ def link_pose_for_joint_config(object: Object, joint_config: Dict[str, float], l with UseProspectionWorld(): for joint, pose in joint_config.items(): shadow_object.set_joint_position(joint, pose) - return shadow_object.get_link_pose(link_name) + return shadow_object.links[link_name].pose diff --git a/test/test_action_designator.py b/test/test_action_designator.py index e5bf75c46..8a90f76ef 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -130,7 +130,7 @@ def test_grasping(self): with simulated_robot: description.resolve().perform() dist = np.linalg.norm( - np.array(self.robot.get_link_pose(robot_description.get_tool_frame("right")).position_as_list()) - + np.array(self.robot.links[robot_description.get_tool_frame("right")].position_as_list) - np.array(self.milk.get_pose().position_as_list())) self.assertTrue(dist < 0.01) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index b32256c8c..b0fea4b38 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -34,10 +34,10 @@ def test_object_movement(self): def test_robot_orientation(self): self.robot.set_pose(Pose([0, 1, 1])) - head_position = self.robot.get_link_position('head_pan_link').z + head_position = self.robot.links['head_pan_link'].position.z #self.robot.set_orientation(list(2*tf.transformations.quaternion_from_euler(0, 0, np.pi, axes="sxyz"))) self.robot.set_orientation(Pose(orientation=[0, 0, 1, 1])) - self.assertEqual(self.robot.get_link_position('head_pan_link').z, head_position) + self.assertEqual(self.robot.links['head_pan_link'].position.z, head_position) def tearDown(self): self.world.reset_world() diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index 085b15d47..2fc1055cd 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -18,13 +18,13 @@ def test_visible(self): self.milk.set_pose(Pose([1.5, 0, 1.2])) self.robot.set_pose(Pose()) time.sleep(1) - self.assertTrue(btr.visible(self.milk, self.robot.get_link_pose(robot_description.get_camera_frame()), + self.assertTrue(btr.visible(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, robot_description.front_facing_axis)) def test_occluding(self): self.milk.set_pose(Pose([3, 0, 1.2])) self.robot.set_pose(Pose()) - self.assertTrue(btr.occluding(self.milk, self.robot.get_link_pose(robot_description.get_camera_frame()), + self.assertTrue(btr.occluding(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, robot_description.front_facing_axis) != []) def test_reachable(self): From 417c17627ad0dedf048b163f649510d656bc597f Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Fri, 22 Dec 2023 11:59:04 +0100 Subject: [PATCH 22/69] [AbstractWorld] Refactoring of link related methods. Removal of redundant methods and/or properties --- src/pycram/bullet_world.py | 20 +++--- src/pycram/world.py | 115 +++++++++++++++------------------- src/pycram/world_reasoning.py | 4 +- test/test_bullet_world.py | 2 +- 4 files changed, 60 insertions(+), 81 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index af139e7ed..33bf00967 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -195,15 +195,6 @@ def get_object_number_of_joints(self, obj_id: int) -> int: """ return p.getNumJoints(obj_id, self.client_id) - def get_object_link_id(self, obj_id: int, link_name: str) -> int: - """ - Get the ID of a link in an articulated object. - - :param obj: The object - :param link_name: The name of the link - """ - return self.get_object_by_id(obj_id).link_name_to_id[link_name] - def reset_object_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ Reset the joint position instantly without physics simulation @@ -242,7 +233,10 @@ def set_object_link_color(self, obj: Object, link_id: int, rgba_color: Color): """ p.changeVisualShape(obj.id, link_id, rgbaColor=rgba_color, physicsClientId=self.client_id) - def get_object_colors(self, obj: Object) -> Dict[str, Color]: + def get_color_of_object_link(self, obj: Object, link_name: str) -> Color: + return self.get_colors_of_all_links_of_object(obj)[link_name] + + def get_colors_of_all_links_of_object(self, obj: Object) -> Dict[str, Color]: """ Get the RGBA colors of each link in the object as a dictionary from link name to color. @@ -250,9 +244,9 @@ def get_object_colors(self, obj: Object) -> Dict[str, Color]: :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. """ visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) - swap = {v: k for k, v in obj.link_name_to_id.items()} - links = list(map(lambda x: swap[x[1]] if x[1] != -1 else "base", visual_data)) - colors = Color.from_rgba(list(map(lambda x: x[7], visual_data))) + link_id_to_name = {v: k for k, v in obj.link_name_to_id.items()} + links = list(map(lambda x: link_id_to_name[x[1]] if x[1] != -1 else "base", visual_data)) + colors = list(map(lambda x: Color.from_rgba(x[7]), visual_data)) link_to_color = dict(zip(links, colors)) return link_to_color diff --git a/src/pycram/world.py b/src/pycram/world.py index 671fbfaf3..618c9d8ef 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -173,7 +173,7 @@ def remove_object(self, obj_id: int) -> None: """ pass - def _set_attached_objects(self, parent, prev_object: List[Object]) -> None: + def _set_attached_objects_poses(self, parent, prev_object: List[Object]) -> None: """ Updates the positions of all attached objects. This is done by calculating the new pose in world coordinate frame and setting the @@ -198,7 +198,7 @@ def _set_attached_objects(self, parent, prev_object: List[Object]) -> None: world_to_object.position_as_list(), world_to_object.orientation_as_list()) child._current_pose = world_to_object - self._set_attached_objects(child, prev_object + [parent]) + self._set_attached_objects_poses(child, prev_object + [parent]) def attach_objects(self, parent_object: Object, @@ -408,15 +408,6 @@ def get_object_link_names(self, obj_id: int) -> List[str]: """ pass - def get_object_link_id(self, obj: Object, link_idx: int) -> str: - """ - Get the name of a link in an articulated object. - - :param obj: The object - :param link_idx: The index of the link (would indicate order). - """ - pass - @abstractmethod def get_object_number_of_joints(self, obj_id: int) -> int: """ @@ -492,12 +483,13 @@ def set_object_link_color(self, obj: Object, link_id: int, rgba_color: Color): """ pass - def get_object_color(self, - obj: Object, - link: Optional[str] = None) -> Union[Color, Dict[str, Color], None]: + @abstractmethod + def get_color_of_object_link(self, obj: Object, link_name: str) -> Color: + pass + + def get_color_of_object(self, obj: Object) -> Union[Color, Dict[str, Color]]: """ - This method returns the color of this object or a link of this object. If no link is given then the - return is either: + This method returns the color of this object. The return is either: 1. A Color object with RGBA values, this is the case if the object only has one link (this happens for example if the object is spawned from a .obj or .stl file) @@ -505,24 +497,9 @@ def get_object_color(self, Please keep in mind that not every link may have a color. This is dependent on the URDF from which the object is spawned. - If a link is specified then the return is a list with RGBA values representing the color of this link. - It may be that this link has no color, in this case the return is None as well as an error message. - :param obj: The object for which the color should be returned. - :param link: the link name for which the color should be returned. - :return: The color of the object or link, or a dictionary containing every colored link with its color """ - link_to_color_dict = self.get_object_colors(obj) - - if link: - if link in link_to_color_dict.keys(): - return link_to_color_dict[link] - elif link not in obj.link_name_to_id.keys(): - rospy.logerr(f"The link '{link}' is not part of this obejct") - return None - else: - rospy.logerr(f"The link '{link}' has no color") - return None + link_to_color_dict = self.get_colors_of_all_links_of_object(obj) if len(link_to_color_dict) == 1: return list(link_to_color_dict.values())[0] @@ -530,7 +507,7 @@ def get_object_color(self, return link_to_color_dict @abstractmethod - def get_object_colors(self, obj: Object) -> Dict[str, Color]: + def get_colors_of_all_links_of_object(self, obj: Object) -> Dict[str, Color]: """ Get the RGBA colors of each link in the object as a dictionary from link name to color. @@ -982,7 +959,7 @@ def __init__(self, @property def is_root(self) -> bool: - return self.object.urdf_object.get_root() == self.urdf_link.name + return self.object.get_root_link_id() == self.id def update_transform(self, transform_time: Optional[rospy.Time] = None): self.local_transformer.update_transforms([self.transform], transform_time) @@ -1007,7 +984,7 @@ def get_aabb(self) -> AxisAlignedBoundingBox: return self.world.get_object_link_aabb(self.object.id, self.id) @property - def position(self) -> Point: + def position(self) -> Pose.position: return self.pose.position @property @@ -1029,7 +1006,8 @@ def pose(self) -> Pose: """ if self.is_root: return self.object.get_pose() - return self.world.get_object_link_pose(self.object.id, self.id) + else: + return self.world.get_object_link_pose(self.object.id, self.id) @property def pose_as_list(self) -> List[List[float]]: @@ -1053,6 +1031,14 @@ def origin(self): def collision(self) -> Collision: return self.urdf_link.collision + @property + def color(self) -> Color: + return self.world.get_color_of_object_link(self.object, self.name) + + @color.setter + def color(self, color: Color) -> None: + self.world.set_object_link_color(self.object, self.id, color) + class Object: """ @@ -1188,6 +1174,8 @@ def attach(self, :param child_link: The link name of the other object. :param bidirectional: If the attachment should be a loose attachment. """ + parent_link = self.urdf_object.get_root() if parent_link is None else parent_link + child_link = child_object.urdf_object.get_root() if child_link is None else child_link self.world.attach_objects(self, child_object, self.get_link_id(parent_link), @@ -1229,6 +1217,22 @@ def get_orientation(self) -> Pose.orientation: """ return self.get_pose().orientation + def get_position_as_list(self) -> List[float]: + """ + Returns the position of this Object as a list of xyz. + + :return: The current position of this object + """ + return self.get_pose().position_as_list() + + def get_orientation_as_list(self) -> List[float]: + """ + Returns the orientation of this object as a list of xyzw, representing a quaternion. + + :return: A list of xyzw + """ + return self.get_pose().orientation_as_list() + def get_pose(self) -> Pose: """ Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. @@ -1250,25 +1254,7 @@ def set_pose(self, pose: Pose, base: bool = False) -> None: position = np.array(position) + self.base_origin_shift self.world.reset_object_base_pose(self, position, orientation) self._current_pose = pose_in_map - self.world._set_attached_objects(self, [self]) - - @property - def pose(self) -> Pose: - """ - Property that returns the current position of this Object. - - :return: The position as a list of xyz - """ - return self.get_pose() - - @pose.setter - def pose(self, value: Pose) -> None: - """ - Sets the Pose of the Object to the given value. Function for attribute use. - - :param value: New Pose of the Object - """ - self.set_pose(value) + self._set_attached_objects_poses() def move_base_to_origin_pos(self) -> None: """ @@ -1277,7 +1263,7 @@ def move_base_to_origin_pos(self) -> None: """ self.set_pose(self.get_pose(), base=True) - def _set_attached_objects(self, prev_object: List[Object]) -> None: + def _set_attached_objects_poses(self) -> None: """ Updates the positions of all attached objects. This is done by calculating the new pose in world coordinate frame and setting the @@ -1287,7 +1273,7 @@ def _set_attached_objects(self, prev_object: List[Object]) -> None: :param prev_object: A list of Objects that were already moved, these will be excluded to prevent loops in the update. """ - self.world._set_attached_objects(self, prev_object) + self.world._set_attached_objects_poses(self, [self]) def set_position(self, position: Union[Pose, Point], base=False) -> None: """ @@ -1352,17 +1338,16 @@ def get_joint_id(self, name: str) -> int: """ return self.joint_name_to_id[name] + def get_root_link_id(self) -> int: + return self.get_link_id(self.urdf_object.get_root()) + def get_link_id(self, link_name: str) -> int: """ Returns a unique id for a link name. """ - if link_name is None: - return self.get_root_link_id() + assert link_name is not None return self.link_name_to_id[link_name] - def get_root_link_id(self) -> int: - return self.get_link_id(self.urdf_object.get_root()) - def get_link_by_id(self, link_id: int) -> Link: """ Returns the link for a given unique link id @@ -1397,7 +1382,7 @@ def set_positions_of_all_joints(self, joint_poses: dict) -> None: for joint_name, joint_position in joint_poses.items(): self.world.reset_object_joint_position(self, joint_name, joint_position) self._current_joints_positions[joint_name] = joint_position - self.world._set_attached_objects(self, [self]) + self._set_attached_objects_poses() def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ @@ -1420,7 +1405,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: # return self.world.reset_object_joint_position(self, joint_name, joint_position) self._current_joints_positions[joint_name] = joint_position - self.world._set_attached_objects(self, [self]) + self._set_attached_objects_poses() def get_joint_position(self, joint_name: str) -> float: """ @@ -1498,7 +1483,7 @@ def set_color(self, color: Color, link: Optional[str] = "") -> None: self.world.set_object_color(self, color, link) def get_color(self, link: Optional[str] = None) -> Union[Color, Dict[str, Color], None]: - return self.world.get_object_color(self, link) + return self.world.get_color_of_object(self, link) def get_aabb(self) -> AxisAlignedBoundingBox: return self.world.get_object_aabb(self) diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index c6e696ca5..1e5f48c3e 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -100,14 +100,14 @@ def stable(object: Object, world, world_id = _world_and_id(world) shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) with UseProspectionWorld(): - coords_prev = shadow_obj.pose.position_as_list() + coords_prev = shadow_obj.get_position_as_list() state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) p.setGravity(0, 0, -9.8, BulletWorld.current_world.client_id) # one Step is approximately 1/240 seconds BulletWorld.current_world.simulate(2) # coords_past = p.getBasePositionAndOrientation(object.id, physicsClientId=world_id)[0] - coords_past = shadow_obj.pose.position_as_list() + coords_past = shadow_obj.get_position_as_list() # p.restoreState(state, physicsClientId=BulletWorld.current_bullet_world.client_id) coords_prev = list(map(lambda n: round(n, 3), coords_prev)) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index b0fea4b38..6e62cd458 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -30,7 +30,7 @@ def setUp(self): def test_object_movement(self): self.milk.set_position(Pose([0, 1, 1])) - self.assertEqual(self.milk.get_pose().position_as_list(), [0, 1, 1]) + self.assertEqual(self.milk.get_position_as_list(), [0, 1, 1]) def test_robot_orientation(self): self.robot.set_pose(Pose([0, 1, 1])) From fae2d8fdc2155ab59ba8c6dec2d7a0bc12d78bab Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 24 Dec 2023 12:42:40 +0100 Subject: [PATCH 23/69] [AbstractWorld] Objects are responsible for attachments not World. Links are responsible for constraints. --- demos/pycram_bullet_world_demo/demo.py | 10 +- src/pycram/bullet_world.py | 10 +- src/pycram/world.py | 198 ++++++++++++------------- src/pycram/world_dataclasses.py | 4 +- 4 files changed, 111 insertions(+), 111 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 837c3a359..eabdd2ac2 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -2,7 +2,9 @@ from pycram.designators.location_designator import * from pycram.designators.object_designator import * from pycram.pose import Pose -from pycram.bullet_world import BulletWorld, Object +from pycram.bullet_world import BulletWorld +from pycram.world import Object +from pycram.world_dataclasses import Color from pycram.process_module import simulated_robot, with_simulated_robot from pycram.enums import ObjectType @@ -10,11 +12,11 @@ robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf", pose=Pose([1, 2, 0])) apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]), color=[1, 0, 0, 1]) +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]), color=Color(1, 0, 0, 1)) cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]), color=[0, 1, 0, 1]) -spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=[0, 0, 1, 1]) -bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=[1, 1, 0, 1]) +spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=Color(0, 0, 1, 1)) +bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=Color(1, 1, 0, 1)) apartment.attach(spoon, 'cabinet10_drawer_top') pick_pose = Pose([2.7, 2.15, 1]) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 33bf00967..ad1cc7abe 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -73,10 +73,16 @@ def add_constraint(self, constraint: Constraint) -> int: """ Add a constraint between two objects so that attachment they become attached """ + + parent_obj = self.get_object_by_id(constraint.parent_obj_id) + parent_link_id = parent_obj.link_name_to_id[constraint.parent_link_name] + child_obj = self.get_object_by_id(constraint.child_obj_id) + child_link_id = child_obj.link_name_to_id[constraint.child_link_name] + constraint_id = p.createConstraint(constraint.parent_obj_id, - constraint.parent_link_id, + parent_link_id, constraint.child_obj_id, - constraint.child_link_id, + child_link_id, constraint.joint_type.as_int(), constraint.joint_axis_in_child_link_frame, constraint.joint_frame_position_wrt_parent_origin, diff --git a/src/pycram/world.py b/src/pycram/world.py index 618c9d8ef..6e9aec3f1 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -188,8 +188,8 @@ def _set_attached_objects_poses(self, parent, prev_object: List[Object]) -> None if child in prev_object: continue if not parent.attachments[child].bidirectional: - parent.attachments[child].update_attachment() - child.attachments[parent].update_attachment() + parent.attachments[child].update_transform_and_constraint() + child.attachments[parent].update_transform_and_constraint() else: link_to_object = parent.attachments[child].child_to_parent_transform @@ -200,37 +200,7 @@ def _set_attached_objects_poses(self, parent, prev_object: List[Object]) -> None child._current_pose = world_to_object self._set_attached_objects_poses(child, prev_object + [parent]) - def attach_objects(self, - parent_object: Object, - child_object: Object, - parent_link_id: Optional[int] = -1, - child_link_id: Optional[int] = -1, - bidirectional: Optional[bool] = True) -> None: - """ - Attaches two objects together by saving the current transformation between the links coordinate frames. - Furthermore, a constraint will be created so the attachment also works while in simulation. - - :param bidirectional: If the parent should also follow the child. - """ - - # Add the attachment to the attachment dictionary of both objects - attachment = Attachment(parent_object, child_object, parent_link_id, child_link_id, bidirectional) - self.update_objects_attachments_collection(parent_object, child_object, attachment) - self.attachment_event(parent_object, [parent_object, child_object]) - - def update_objects_attachments_collection(self, - parent_object: Object, - child_object: Object, - attachment: Attachment) -> None: - parent_object.attachments[child_object] = attachment - child_object.attachments[parent_object] = attachment.get_inverse() - - def add_fixed_constraint(self, - parent_object_id: int, - child_object_id: int, - child_to_parent_transform: Transform, - parent_link_id: Optional[int] = -1, - child_link_id: Optional[int] = -1) -> int: + def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: """ Creates a fixed joint constraint between the given parent and child links, the joint frame will be at the origin of the child link frame, and would have the same orientation @@ -238,11 +208,11 @@ def add_fixed_constraint(self, returns the constraint id """ - # -1 in link id means use the base link of the object - constraint = Constraint(parent_obj_id=parent_object_id, - parent_link_id=parent_link_id, - child_obj_id=child_object_id, - child_link_id=child_link_id, + + constraint = Constraint(parent_obj_id=parent_link.get_object_id(), + parent_link_name=parent_link.name, + child_obj_id=child_link.get_object_id(), + child_link_name=child_link.name, joint_type=JointType.FIXED, joint_axis_in_child_link_frame=[0, 0, 0], joint_frame_position_wrt_parent_origin=child_to_parent_transform.translation_as_list(), @@ -259,24 +229,6 @@ def add_constraint(self, constraint: Constraint) -> int: """ pass - @staticmethod - def detach_objects(obj1: Object, obj2: Object) -> None: - """ - Detaches obj2 from obj1. This is done by - deleting the attachment from the attachments dictionary of both objects - and deleting the constraint of pybullet. - Afterward the detachment event of the corresponding BulletWorld will be fired. - - :param obj1: The object from which an object should be detached. - :param obj2: The object which should be detached. - """ - del obj1.attachments[obj2] - del obj2.attachments[obj1] - del obj1.cids[obj2] - del obj2.cids[obj1] - - obj1.world.detachment_event(obj1, [obj1, obj2]) - @abstractmethod def remove_constraint(self, constraint_id): pass @@ -954,8 +906,22 @@ def __init__(self, self.urdf_link = urdf_link self.object = obj self.world = obj.world - self.tf_frame = f"{obj.tf_frame}/{urdf_link.name}" if not self.is_root else obj.tf_frame self.local_transformer = LocalTransformer() + self.constraint_ids: Dict[Link, int] = {} + + def add_fixed_constraint_with_link(self, child_link: Link) -> None: + constraint_id = self.world.add_fixed_constraint(self, + child_link, + child_link.get_transform_from_link(self)) + self.constraint_ids[child_link] = constraint_id + child_link.constraint_ids[self] = constraint_id + + def get_object_id(self) -> int: + return self.object.id + + @property + def tf_frame(self) -> str: + return f"{self.object.tf_frame}/{self.urdf_link.name}" @property def is_root(self) -> bool: @@ -1004,10 +970,7 @@ def pose(self) -> Pose: """ The pose of the link relative to the world frame. """ - if self.is_root: - return self.object.get_pose() - else: - return self.world.get_object_link_pose(self.object.id, self.id) + return self.world.get_object_link_pose(self.object.id, self.id) @property def pose_as_list(self) -> List[List[float]]: @@ -1039,6 +1002,25 @@ def color(self) -> Color: def color(self, color: Color) -> None: self.world.set_object_link_color(self.object, self.id, color) + def remove_constraint_with(self, child_link: Link) -> None: + self.world.remove_constraint(self.constraint_ids[child_link]) + del self.constraint_ids[child_link] + del child_link.constraint_ids[self] + + +class RootLink(Link): + + def __init__(self, obj: Object): + super().__init__(obj.get_root_link_id(), obj.get_root_urdf_link(), obj) + + @property + def tf_frame(self) -> str: + return self.object.tf_frame + + @property + def pose(self) -> Pose: + return self.object.get_pose() + class Object: """ @@ -1110,7 +1092,10 @@ def _init_links(self): for urdf_link in self.urdf_object.links: link_name = urdf_link.name link_id = self.link_name_to_id[link_name] - links[link_name] = Link(link_id, urdf_link, self) + if link_name == self.urdf_object.get_root(): + links[link_name] = RootLink(self) + else: + links[link_name] = Link(link_id, urdf_link, self) return links def _init_current_positions_of_joint(self) -> None: @@ -1154,6 +1139,9 @@ def remove(self) -> None: if World.robot == self: World.robot = None + def remove_constraint_with(self, obj: Object) -> None: + self.world.remove_constraint(self.cids[obj]) + def attach(self, child_object: Object, parent_link: Optional[str] = None, @@ -1174,13 +1162,15 @@ def attach(self, :param child_link: The link name of the other object. :param bidirectional: If the attachment should be a loose attachment. """ - parent_link = self.urdf_object.get_root() if parent_link is None else parent_link - child_link = child_object.urdf_object.get_root() if child_link is None else child_link - self.world.attach_objects(self, - child_object, - self.get_link_id(parent_link), - child_object.get_link_id(child_link), - bidirectional) + parent_link = self.links[parent_link] if parent_link else self.get_root_link() + child_link = child_object.links[child_link] if child_link else child_object.get_root_link() + + attachment = Attachment(parent_link, child_link, bidirectional) + + self.attachments[child_object] = attachment + child_object.attachments[self] = attachment.get_inverse() + + self.world.attachment_event(self, [self, child_object]) def detach(self, child_object: Object) -> None: """ @@ -1191,7 +1181,10 @@ def detach(self, child_object: Object) -> None: :param child_object: The object which should be detached """ - self.world.detach_objects(self, child_object) + del self.attachments[child_object] + del child_object.attachments[self] + + self.world.detachment_event(self, [self, child_object]) def detach_all(self) -> None: """ @@ -1199,7 +1192,7 @@ def detach_all(self) -> None: """ attachments = self.attachments.copy() for att in attachments.keys(): - self.world.detach_objects(self, att) + self.detach(att) def get_position(self) -> Pose.position: """ @@ -1338,6 +1331,21 @@ def get_joint_id(self, name: str) -> int: """ return self.joint_name_to_id[name] + def get_root_urdf_link(self) -> urdf_parser_py.urdf.Link: + """ + Returns the root link of the URDF of this object. + """ + link_name = self.urdf_object.get_root() + for link in self.urdf_object.links: + if link.name == link_name: + return link + + def get_root_link(self) -> Link: + """ + Returns the root link of this object. + """ + return self.links[self.urdf_object.get_root()] + def get_root_link_id(self) -> int: return self.get_link_id(self.urdf_object.get_root()) @@ -1608,20 +1616,18 @@ def _get_robot_name_from_urdf(urdf_string: str) -> str: class Attachment: def __init__(self, - parent_object: Object, - child_object: Object, - parent_link_id: Optional[int] = -1, # -1 means base link - child_link_id: Optional[int] = -1, + parent_link: Link, + child_link: Link, bidirectional: Optional[bool] = False, child_to_parent_transform: Optional[Transform] = None, constraint_id: Optional[int] = None): """ Creates an attachment between the parent object and the child object. """ - self.parent_object = parent_object - self.child_object = child_object - self.parent_link = parent_object.get_link_by_id(parent_link_id) - self.child_link = child_object.get_link_by_id(child_link_id) + self.parent_link = parent_link + self.child_link = child_link + self.parent_object = parent_link.object + self.child_object = child_link.object self.bidirectional = bidirectional self._loose = False and not bidirectional @@ -1631,9 +1637,9 @@ def __init__(self, self.constraint_id = constraint_id if self.constraint_id is None: - self.add_constraint_and_update_objects_constraints_collection() + self.add_fixed_constraint() - def update_attachment(self): + def update_transform_and_constraint(self): self.update_transform() self.update_constraint() @@ -1642,35 +1648,21 @@ def update_transform(self): def update_constraint(self): self.remove_constraint_if_exists() - self.add_constraint_and_update_objects_constraints_collection() + self.add_fixed_constraint() + + def add_fixed_constraint(self): + self.parent_link.add_fixed_constraint_with_link(self.child_link) def calculate_transform(self): return self.parent_link.get_transform_to_link(self.child_link) def remove_constraint_if_exists(self): - if self.constraint_id is not None: - self.parent_object.world.remove_constraint(self.constraint_id) - - def add_constraint_and_update_objects_constraints_collection(self): - self.constraint_id = self.add_fixed_constraint() - self.update_objects_constraints_collection() - - def add_fixed_constraint(self): - constraint_id = self.parent_object.world.add_fixed_constraint(self.parent_object.id, - self.child_object.id, - self.child_to_parent_transform, - self.parent_link.id, - self.child_link.id) - return constraint_id - - def update_objects_constraints_collection(self): - self.parent_object.cids[self.child_object] = self.constraint_id - self.child_object.cids[self.parent_object] = self.constraint_id + if self.child_link in self.parent_link.constraint_ids: + self.parent_link.remove_constraint_with(self.child_link) def get_inverse(self): - attachment = Attachment(self.child_object, self.parent_object, self.child_link.id, - self.parent_link.id, self.bidirectional, self.child_to_parent_transform.invert(), - self.constraint_id) + attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, + constraint_id=self.constraint_id) attachment.loose = False if self.loose else True return attachment diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 6598c511a..3910aae73 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -37,9 +37,9 @@ class Constraint: Dataclass for storing a constraint between two objects. """ parent_obj_id: int - parent_link_id: int + parent_link_name: str child_obj_id: int - child_link_id: int + child_link_name: str joint_type: JointType joint_axis_in_child_link_frame: List[int] joint_frame_position_wrt_parent_origin: List[float] From 2099bd7410ecb26cfa3bdf368f0f72a5b4b90725 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 28 Dec 2023 14:53:27 +0100 Subject: [PATCH 24/69] [AbstractWorld] _set_attached_objects_poses implementation moved to the Object class instead of World. Cleaned World init method. Added optional argument in set_pose method of the Object class to exclude moving attached objects. --- src/pycram/world.py | 124 +++++++++++++++++++++++--------------------- 1 file changed, 64 insertions(+), 60 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 6e9aec3f1..9f5bede9a 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -82,32 +82,32 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. """ - self._init_current_world_and_simulation_time_step(simulation_time_step) - # init the current world and simulation time step which are class variables. + if World.current_world is None: + World.current_world = self + World.simulation_time_step = simulation_time_step + + self.is_prospection_world: bool = is_prospection_world + self._init_and_sync_prospection_world() + + self.local_transformer = LocalTransformer() + self._update_local_transformer_worlds() self.objects: List[Object] = [] + # List of all Objects in the World + self.client_id: int = -1 + # This is used to connect to the physics server (allows multiple clients) + self.mode: str = mode + # The mode of the simulation, can be "GUI" or "DIRECT" - self._init_events() self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} - self.local_transformer = LocalTransformer() - self.is_prospection_world: bool = is_prospection_world - if is_prospection_world: # then no need to add another prospection world - self.prospection_world = None - self.world_sync = None - else: # a normal world should have a synced prospection world - self._init_and_sync_prospection_world() - self._update_local_transformer_worlds() + self._init_events() self._saved_states: Dict[int, WorldState] = {} # Different states of the world indexed by int state id. - def _init_current_world_and_simulation_time_step(self, simulation_time_step): - World.current_world = self if World.current_world is None else World.current_world - World.simulation_time_step = simulation_time_step - def _init_events(self): self.detachment_event: Event = Event() self.attachment_event: Event = Event() @@ -121,13 +121,20 @@ def _update_local_transformer_worlds(self): self.local_transformer.world = self self.local_transformer.prospection_world = self.prospection_world - @classmethod - def _init_prospection_world(cls): - cls.prospection_world: World = cls("DIRECT", True, cls.simulation_time_step) + def _init_prospection_world(self): + if self.is_prospection_world: # then no need to add another prospection world + self.prospection_world = None + else: + self.prospection_world: World = self.__class__("DIRECT", + True, + World.simulation_time_step) def _sync_prospection_world(self): - self.world_sync: WorldSync = WorldSync(self, self.prospection_world) - self.world_sync.start() + if self.is_prospection_world: # then no need to add another prospection world + self.world_sync = None + else: + self.world_sync: WorldSync = WorldSync(self, self.prospection_world) + self.world_sync.start() @property def simulation_frequency(self): @@ -155,14 +162,14 @@ def get_objects_by_type(self, obj_type: str) -> List[Object]: """ return list(filter(lambda obj: obj.obj_type == obj_type, self.objects)) - def get_object_by_id(self, id: int) -> Object: + def get_object_by_id(self, obj_id: int) -> Object: """ Returns the single Object that has the unique id. - :param id: The unique id for which the Object should be returned. + :param obj_id: The unique id for which the Object should be returned. :return: The Object with the id 'id'. """ - return list(filter(lambda obj: obj.id == id, self.objects))[0] + return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] @abstractmethod def remove_object(self, obj_id: int) -> None: @@ -173,33 +180,6 @@ def remove_object(self, obj_id: int) -> None: """ pass - def _set_attached_objects_poses(self, parent, prev_object: List[Object]) -> None: - """ - Updates the positions of all attached objects. This is done - by calculating the new pose in world coordinate frame and setting the - base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. - - :param prev_object: A list of Objects that were already moved, - these will be excluded to prevent loops in the update. - """ - for child in parent.attachments: - if child in prev_object: - continue - if not parent.attachments[child].bidirectional: - parent.attachments[child].update_transform_and_constraint() - child.attachments[parent].update_transform_and_constraint() - else: - link_to_object = parent.attachments[child].child_to_parent_transform - - world_to_object = parent.local_transformer.transform_pose_to_target_frame(link_to_object.to_pose(), "map") - self.reset_object_base_pose(child, - world_to_object.position_as_list(), - world_to_object.orientation_as_list()) - child._current_pose = world_to_object - self._set_attached_objects_poses(child, prev_object + [parent]) - def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: """ Creates a fixed joint constraint between the given parent and child links, @@ -697,7 +677,9 @@ def _copy(self) -> World: o.set_joint_position(joint, obj.get_joint_position(joint)) return world - def register_two_objects_collision_callbacks(self, object_a: Object, object_b: Object, + def register_two_objects_collision_callbacks(self, + object_a: Object, + object_b: Object, on_collision_callback: Callable, on_collision_removal_callback: Optional[Callable] = None) -> None: """ @@ -1194,6 +1176,9 @@ def detach_all(self) -> None: for att in attachments.keys(): self.detach(att) + def update_attachment_with_object(self, child_object: Object): + self.attachments[child_object].update_transform_and_constraint() + def get_position(self) -> Pose.position: """ Returns the position of this Object as a list of xyz. @@ -1234,12 +1219,13 @@ def get_pose(self) -> Pose: """ return self._current_pose - def set_pose(self, pose: Pose, base: bool = False) -> None: + def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Optional[bool] = True) -> None: """ Sets the Pose of the object. :param pose: New Pose for the object :param base: If True places the object base instead of origin at the specified position and orientation + :param set_attachments: Whether to set the poses of the attached objects to this object or not. """ pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") position, orientation = pose_in_map.to_list() @@ -1247,7 +1233,8 @@ def set_pose(self, pose: Pose, base: bool = False) -> None: position = np.array(position) + self.base_origin_shift self.world.reset_object_base_pose(self, position, orientation) self._current_pose = pose_in_map - self._set_attached_objects_poses() + if set_attachments: + self._set_attached_objects_poses() def move_base_to_origin_pos(self) -> None: """ @@ -1256,7 +1243,7 @@ def move_base_to_origin_pos(self) -> None: """ self.set_pose(self.get_pose(), base=True) - def _set_attached_objects_poses(self) -> None: + def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: """ Updates the positions of all attached objects. This is done by calculating the new pose in world coordinate frame and setting the @@ -1264,9 +1251,26 @@ def _set_attached_objects_poses(self) -> None: After this the _set_attached_objects method of all attached objects will be called. - :param prev_object: A list of Objects that were already moved, these will be excluded to prevent loops in the update. + :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops in the update. """ - self.world._set_attached_objects_poses(self, [self]) + + if already_moved_objects is None: + already_moved_objects = [] + + for child in self.attachments: + + if child in already_moved_objects: + continue + + attachment = self.attachments[child] + if not attachment.bidirectional: + self.update_attachment_with_object(child) + 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_attached_objects_poses(already_moved_objects + [self]) def set_position(self, position: Union[Pose, Point], base=False) -> None: """ @@ -1619,7 +1623,7 @@ def __init__(self, parent_link: Link, child_link: Link, bidirectional: Optional[bool] = False, - child_to_parent_transform: Optional[Transform] = None, + parent_to_child_transform: Optional[Transform] = None, constraint_id: Optional[int] = None): """ Creates an attachment between the parent object and the child object. @@ -1631,8 +1635,8 @@ def __init__(self, self.bidirectional = bidirectional self._loose = False and not bidirectional - self.child_to_parent_transform = child_to_parent_transform - if self.child_to_parent_transform is None: + self.parent_to_child_transform = parent_to_child_transform + if self.parent_to_child_transform is None: self.update_transform() self.constraint_id = constraint_id @@ -1644,7 +1648,7 @@ def update_transform_and_constraint(self): self.update_constraint() def update_transform(self): - self.child_to_parent_transform = self.calculate_transform() + self.parent_to_child_transform = self.calculate_transform() def update_constraint(self): self.remove_constraint_if_exists() From 38516ffa77f9a058cfad04870c534f8eaff263d3 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 1 Jan 2024 08:24:25 +0100 Subject: [PATCH 25/69] [AbstractWorld] Implemented a CollisionsCallbacks dataclass and used it to define the self.coll_callbacks attribute. --- src/pycram/world.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 9f5bede9a..bef1c3341 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -42,6 +42,14 @@ class WorldState: constraint_ids: Dict[Object, Dict[Object, int]] +@dataclass +class CollisionCallbacks: + obj_1: Object + obj_2: Object + on_collision_cb: Callable + no_collision_cb: Optional[Callable] = None + + class World(ABC): """ The World Class represents the physics Simulation and belief state. @@ -101,7 +109,7 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: self.mode: str = mode # The mode of the simulation, can be "GUI" or "DIRECT" - self.coll_callbacks: Dict[Tuple[Object, Object], Tuple[Callable, Callable]] = {} + self.coll_callbacks: Dict[Tuple[Object, Object], CollisionCallbacks] = {} self._init_events() @@ -295,12 +303,12 @@ def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: """ for i in range(0, int(seconds * self.simulation_frequency)): self.step() - for objects, callback in self.coll_callbacks.items(): + for objects, callbacks in self.coll_callbacks.items(): contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1]) if contact_points != (): - callback[0]() - elif callback[1] is not None: # Call no collision callback - callback[1]() + callbacks.on_collision_cb() + elif callbacks.no_collision_cb is not None: + callbacks.no_collision_cb() if real_time: # Simulation runs at 240 Hz time.sleep(self.simulation_time_step) From 9a9d1a63f2954dacc4a119198d172eb381d3fe39 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 1 Jan 2024 09:03:47 +0100 Subject: [PATCH 26/69] [AbstractWorld] Object Class does not manage constraints, they are managed by the Link Class instead. --- src/pycram/world.py | 57 +++++++++++---------------------------------- 1 file changed, 14 insertions(+), 43 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index bef1c3341..4b00f1f73 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -39,8 +39,6 @@ class WorldState: state_id: int attachments: Dict[Object, Dict[Object, Attachment]] - constraint_ids: Dict[Object, Dict[Object, int]] - @dataclass class CollisionCallbacks: @@ -596,9 +594,7 @@ def save_state(self) -> int: :return: A unique id of the state """ state_id = self.save_physics_simulator_state() - self._saved_states[state_id] = WorldState(state_id, - self.get_objects_attachments(), - self.get_objects_constraint_ids()) + self._saved_states[state_id] = WorldState(state_id, self.get_objects_attachments()) return state_id def restore_state(self, state_id) -> None: @@ -610,7 +606,7 @@ def restore_state(self, state_id) -> None: :param state_id: The unique id representing the state, as returned by :func:`~save_state` """ self.restore_physics_simulator_state(state_id) - self.restore_attachments_and_constraints_from_saved_world_state(state_id) + self.restore_attachments_from_saved_world_state(state_id) @abstractmethod def save_physics_simulator_state(self) -> int: @@ -628,15 +624,6 @@ def get_objects_attachments(self) -> Dict[Object, Dict[Object, Attachment]]: attachments[o] = o.attachments.copy() return attachments - def get_objects_constraint_ids(self) -> Dict[Object, Dict[Object, int]]: - """ - Get the constraint ids collection that is stored in each object. - """ - constraint_ids = {} - for o in self.objects: - constraint_ids[o] = o.cids.copy() - return constraint_ids - @abstractmethod def restore_physics_simulator_state(self, state_id): """ @@ -645,14 +632,6 @@ def restore_physics_simulator_state(self, state_id): """ pass - def restore_attachments_and_constraints_from_saved_world_state(self, state_id: int): - """ - Restores the attachments and constraints of the objects in the World. This is done by setting the attachments, - and the cids attributes of each object in the World to the given attachments and constraint_ids. - """ - self.restore_attachments_from_saved_world_state(state_id) - self.restore_constraints_from_saved_world_state(state_id) - def restore_attachments_from_saved_world_state(self, state_id: int): attachments = self._saved_states[state_id].attachments for obj in self.objects: @@ -661,14 +640,6 @@ def restore_attachments_from_saved_world_state(self, state_id: int): except KeyError: continue - def restore_constraints_from_saved_world_state(self, state_id: int): - constraint_ids = self._saved_states[state_id].constraint_ids - for obj in self.objects: - try: - obj.cids = constraint_ids[obj] - except KeyError: - continue - def _copy(self) -> World: """ Copies this World into another and returns it. The other World @@ -1038,22 +1009,27 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, :param color: The color with which the object should be spawned. :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ + if pose is None: pose = Pose() + self.world: World = world if world is not None else World.current_world - self.local_transformer = LocalTransformer() + self.name: str = name self.obj_type: Union[str, ObjectType] = obj_type self.color: Color = color - pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") - position, orientation = pose_in_map.to_list() + + self.local_transformer = LocalTransformer() + self.original_pose = self.local_transformer.transform_pose_to_target_frame(pose, "map") + position, orientation = self.original_pose.to_list() self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignore_cached_files) + self._current_pose = self.original_pose + self.joint_name_to_id: Dict[str, int] = self._get_joint_name_to_id_map() self.link_name_to_id: Dict[str, int] = self._get_link_name_to_id_map() self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) + self.attachments: Dict[Object, Attachment] = {} - self.cids: Dict[Object, int] = {} - self.original_pose = pose_in_map self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) @@ -1065,15 +1041,13 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.urdf_object = URDF.from_xml_string(f.read()) if self.urdf_object.name == robot_description.name: self.world.set_robot_if_not_set(self) - self.link_name_to_id[self.urdf_object.get_root()] = -1 self.link_id_to_name[-1] = self.urdf_object.get_root() self.links = self._init_links() + self.update_link_transforms() - self._current_pose = pose_in_map self._current_joints_positions = {} self._init_current_positions_of_joint() - self.update_link_transforms() self.world.objects.append(self) @@ -1103,7 +1077,7 @@ def base_origin_shift(self) -> np.ndarray: return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) def __repr__(self): - skip_attr = ["links", "joints", "urdf_object", "attachments", "cids"] + skip_attr = ["links", "joints", "urdf_object", "attachments"] return self.__class__.__qualname__ + f"(" + ', \n'.join( [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" @@ -1129,9 +1103,6 @@ def remove(self) -> None: if World.robot == self: World.robot = None - def remove_constraint_with(self, obj: Object) -> None: - self.world.remove_constraint(self.cids[obj]) - def attach(self, child_object: Object, parent_link: Optional[str] = None, From 34fd070f39f2e2ad3bd0539ae05868fb4b79daf0 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Thu, 18 Jan 2024 17:06:34 +0100 Subject: [PATCH 27/69] [AbstractWorld] Implemented save and restore state for World, Object, and Link classes. Added tests for Object, Attachment, and Link Classes. --- src/pycram/pose.py | 8 +-- src/pycram/world.py | 110 ++++++++++++++++++++++++++++---------- test/test_attachment.py | 42 +++++++++++++++ test/test_bullet_world.py | 14 +++++ test/test_links.py | 27 ++++++++++ test/test_object.py | 42 +++++++++++++++ 6 files changed, 212 insertions(+), 31 deletions(-) create mode 100644 test/test_attachment.py create mode 100644 test/test_links.py create mode 100644 test/test_object.py diff --git a/src/pycram/pose.py b/src/pycram/pose.py index e707aaa61..cd30c2d4b 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -9,7 +9,7 @@ import numpy as np import rospy import sqlalchemy.orm -from geometry_msgs.msg import PoseStamped, TransformStamped, Vector3 +from geometry_msgs.msg import PoseStamped, TransformStamped, Vector3, Point from geometry_msgs.msg import (Pose as GeoPose, Quaternion as GeoQuaternion) from std_msgs.msg import Header from tf import transformations @@ -87,7 +87,7 @@ def frame(self, value: str) -> None: self.header.frame_id = value @property - def position(self) -> GeoPose: + def position(self) -> Point: """ Property that points to the position of this pose """ @@ -168,6 +168,8 @@ def copy(self) -> Pose: :return: A copy of this pose """ + if isinstance(self.position, list): + print("Position is list") p = Pose(self.position_as_list(), self.orientation_as_list(), self.frame, self.header.stamp) p.header.frame_id = self.header.frame_id # p.header.stamp = self.header.stamp @@ -179,7 +181,7 @@ def position_as_list(self) -> List[float]: :return: The position as a list """ - return [self.pose.position.x, self.pose.position.y, self.pose.position.z] + return [self.position.x, self.position.y, self.position.z] def orientation_as_list(self) -> List[float]: """ diff --git a/src/pycram/world.py b/src/pycram/world.py index 4b00f1f73..e3eb38116 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -13,6 +13,7 @@ from tf.transformations import quaternion_from_euler from typing import List, Optional, Dict, Tuple, Callable from typing import Union +from copy import deepcopy import numpy as np import rospkg @@ -40,6 +41,7 @@ class WorldState: state_id: int attachments: Dict[Object, Dict[Object, Attachment]] + @dataclass class CollisionCallbacks: obj_1: Object @@ -594,9 +596,13 @@ def save_state(self) -> int: :return: A unique id of the state """ state_id = self.save_physics_simulator_state() - self._saved_states[state_id] = WorldState(state_id, self.get_objects_attachments()) + self.save_objects_state(state_id) return state_id + def save_objects_state(self, state_id: int): + for obj in self.objects: + obj.save_state(state_id) + def restore_state(self, state_id) -> None: """ Restores the state of the World according to the given state using the unique state id. This includes position, @@ -606,7 +612,7 @@ def restore_state(self, state_id) -> None: :param state_id: The unique id representing the state, as returned by :func:`~save_state` """ self.restore_physics_simulator_state(state_id) - self.restore_attachments_from_saved_world_state(state_id) + self.restore_objects_states(state_id) @abstractmethod def save_physics_simulator_state(self) -> int: @@ -615,30 +621,17 @@ def save_physics_simulator_state(self) -> int: """ pass - def get_objects_attachments(self) -> Dict[Object, Dict[Object, Attachment]]: - """ - Get The attachments collections that is stored in each object. - """ - attachments = {} - for o in self.objects: - attachments[o] = o.attachments.copy() - return attachments - @abstractmethod - def restore_physics_simulator_state(self, state_id): + def restore_physics_simulator_state(self, state_id: int): """ Restores the objects and environment state in the physics simulator according to the given state using the unique state id. """ pass - def restore_attachments_from_saved_world_state(self, state_id: int): - attachments = self._saved_states[state_id].attachments + def restore_objects_states(self, state_id: int): for obj in self.objects: - try: - obj.attachments = attachments[obj] - except KeyError: - continue + obj.restore_state(state_id) def _copy(self) -> World: """ @@ -857,6 +850,11 @@ def check_for_equal(self) -> None: self.equal_states = eql +@dataclass +class LinkState: + constraint_ids: Dict[Link, int] + + class Link: def __init__(self, @@ -869,13 +867,35 @@ def __init__(self, self.world = obj.world self.local_transformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} + self.saved_states: Dict[int, LinkState] = {} + + def save_state(self, state_id: int) -> None: + """ + Saves the state of this link, this includes the pose of the link. + """ + self.saved_states[state_id] = self.get_current_state() + + def restore_state(self, state_id: int) -> None: + """ + Restores the state of this link, this includes the pose of the link. + """ + self.constraint_ids = self.saved_states[state_id].constraint_ids + + def get_current_state(self): + return LinkState(self.constraint_ids.copy()) - def add_fixed_constraint_with_link(self, child_link: Link) -> None: + def add_fixed_constraint_with_link(self, child_link: Link) -> int: constraint_id = self.world.add_fixed_constraint(self, child_link, child_link.get_transform_from_link(self)) self.constraint_ids[child_link] = constraint_id child_link.constraint_ids[self] = constraint_id + return constraint_id + + def remove_constraint_with_link(self, child_link: Link) -> None: + self.world.remove_constraint(self.constraint_ids[child_link]) + del self.constraint_ids[child_link] + del child_link.constraint_ids[self] def get_object_id(self) -> int: return self.object.id @@ -963,11 +983,6 @@ def color(self) -> Color: def color(self, color: Color) -> None: self.world.set_object_link_color(self.object, self.id, color) - def remove_constraint_with(self, child_link: Link) -> None: - self.world.remove_constraint(self.constraint_ids[child_link]) - del self.constraint_ids[child_link] - del child_link.constraint_ids[self] - class RootLink(Link): @@ -983,6 +998,12 @@ def pose(self) -> Pose: return self.object.get_pose() +@dataclass +class ObjectState: + state_id: int + attachments: Dict[Object, Attachment] + + class Object: """ Represents a spawned Object in the World. @@ -1030,6 +1051,8 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) self.attachments: Dict[Object, Attachment] = {} + self.saved_states: Dict[int, ObjectState] = {} + # takes the state id as key and returns the attachments of the object at that state self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) @@ -1158,7 +1181,7 @@ def detach_all(self) -> None: def update_attachment_with_object(self, child_object: Object): self.attachments[child_object].update_transform_and_constraint() - def get_position(self) -> Pose.position: + def get_position(self) -> Point: """ Returns the position of this Object as a list of xyz. @@ -1207,11 +1230,14 @@ def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Op :param set_attachments: Whether to set the poses of the attached objects to this object or not. """ pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") + position, orientation = pose_in_map.to_list() if base: position = np.array(position) + self.base_origin_shift + self.world.reset_object_base_pose(self, position, orientation) self._current_pose = pose_in_map + if set_attachments: self._set_attached_objects_poses() @@ -1222,6 +1248,31 @@ def move_base_to_origin_pos(self) -> None: """ self.set_pose(self.get_pose(), base=True) + def save_state(self, state_id): + self.save_links_states(state_id) + self.saved_states[state_id] = ObjectState(state_id, self.attachments.copy()) + + def save_links_states(self, state_id: int): + for link in self.links.values(): + link.save_state(state_id) + + def restore_state(self, state_id: int): + self.restore_links_states(state_id) + self.restore_attachments(state_id) + + def restore_attachments(self, state_id): + self.attachments = self.saved_states[state_id].attachments + + def restore_links_states(self, state_id): + for link in self.links.values(): + link.restore_state(state_id) + + def set_object_state(self, obj_state:ObjectState): + self.set_attachments(obj_state.attachments) + + def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: + self.attachments = attachments + def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: """ Updates the positions of all attached objects. This is done @@ -1264,8 +1315,10 @@ def set_position(self, position: Union[Pose, Point], base=False) -> None: if isinstance(position, Pose): target_position = position.position pose.frame = position.frame - else: + elif isinstance(position, Point): target_position = position + else: + raise TypeError("The position has to be either a Pose or a Point") pose.pose.position = target_position pose.pose.orientation = self.get_orientation() @@ -1634,14 +1687,15 @@ def update_constraint(self): self.add_fixed_constraint() def add_fixed_constraint(self): - self.parent_link.add_fixed_constraint_with_link(self.child_link) + cid = self.parent_link.add_fixed_constraint_with_link(self.child_link) + self.constraint_id = cid def calculate_transform(self): return self.parent_link.get_transform_to_link(self.child_link) def remove_constraint_if_exists(self): if self.child_link in self.parent_link.constraint_ids: - self.parent_link.remove_constraint_with(self.child_link) + self.parent_link.remove_constraint_with_link(self.child_link) def get_inverse(self): attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, diff --git a/test/test_attachment.py b/test/test_attachment.py new file mode 100644 index 000000000..d99cb05fc --- /dev/null +++ b/test/test_attachment.py @@ -0,0 +1,42 @@ +import test_bullet_world + + +class TestAttachment(test_bullet_world.BulletWorldTest): + + def test_attach(self): + self.milk.attach(self.robot) + self.assertTrue(self.milk.attachments[self.robot]) + self.assertTrue(self.robot.attachments[self.milk]) + + def test_detach(self): + self.milk.attach(self.robot) + self.milk.detach(self.robot) + self.assertTrue(self.robot not in self.milk.attachments) + self.assertTrue(self.milk not in self.robot.attachments) + + def test_attachment_behavior(self): + self.robot.attach(self.milk) + + milk_pos = self.milk.get_position() + rob_pos = self.robot.get_position() + + rob_pos.x += 1 + self.robot.set_position(rob_pos) + + new_milk_pos = self.milk.get_position() + self.assertEqual(new_milk_pos.x, milk_pos.x + 1) + + def test_detachment_behavior(self): + self.robot.attach(self.milk) + + milk_pos = self.milk.get_position() + rob_pos = self.robot.get_position() + + self.robot.detach(self.milk) + rob_pos.x += 1 + self.robot.set_position(rob_pos) + + new_milk_pos = self.milk.get_position() + self.assertEqual(new_milk_pos.x, milk_pos.x) + + diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 6e62cd458..d847957bc 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -39,6 +39,20 @@ def test_robot_orientation(self): self.robot.set_orientation(Pose(orientation=[0, 0, 1, 1])) self.assertEqual(self.robot.links['head_pan_link'].position.z, head_position) + def test_save_and_restore_state(self): + self.robot.attach(self.milk) + state_id = self.world.save_state() + robot_link = self.robot.get_root_link() + milk_link = self.milk.get_root_link() + cid = robot_link.constraint_ids[milk_link] + self.assertTrue(cid == self.robot.attachments[self.milk].constraint_id) + self.world.remove_constraint(cid) + self.world.restore_state(state_id) + cid = robot_link.constraint_ids[milk_link] + self.assertTrue(milk_link in robot_link.constraint_ids) + self.assertTrue(self.milk in self.robot.attachments) + self.assertTrue(cid == self.robot.attachments[self.milk].constraint_id) + def tearDown(self): self.world.reset_world() diff --git a/test/test_links.py b/test/test_links.py new file mode 100644 index 000000000..0115c8c48 --- /dev/null +++ b/test/test_links.py @@ -0,0 +1,27 @@ +import test_bullet_world + + +class TestLinks(test_bullet_world.BulletWorldTest): + + def test_add_constraint(self): + milk_link = self.milk.get_root_link() + robot_link = self.robot.get_root_link() + robot_link.add_fixed_constraint_with_link(milk_link) + self.assertTrue(milk_link in robot_link.constraint_ids) + self.assertTrue(robot_link in milk_link.constraint_ids) + + def test_remove_constraint(self): + milk_link = self.milk.get_root_link() + robot_link = self.robot.get_root_link() + robot_link.add_fixed_constraint_with_link(milk_link) + robot_link.remove_constraint_with_link(milk_link) + self.assertTrue(milk_link not in robot_link.constraint_ids) + self.assertTrue(robot_link not in milk_link.constraint_ids) + + def test_transform_link(self): + milk_link = self.milk.get_root_link() + robot_link = self.robot.get_root_link() + milk_to_robot = milk_link.get_transform_to_link(robot_link) + robot_to_milk = robot_link.get_transform_to_link(milk_link) + self.assertAlmostEqual(robot_to_milk, milk_to_robot.invert()) + diff --git a/test/test_object.py b/test/test_object.py new file mode 100644 index 000000000..f04866517 --- /dev/null +++ b/test/test_object.py @@ -0,0 +1,42 @@ +import test_bullet_world +from pycram.pose import Pose +from geometry_msgs.msg import Point + + +class TestObject(test_bullet_world.BulletWorldTest): + + def test_set_position_as_point(self): + p = Point() + p.x = 1 + p.y = 2 + p.z = 3 + self.milk.set_position(p) + self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) + + def test_set_position_as_pose(self): + self.milk.set_position(Pose([1, 2, 3])) + self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) + + def test_save_state(self): + self.robot.attach(self.milk, parent_link="r_gripper_palm_link") + self.robot.save_state(1) + self.assertEqual(self.robot.saved_states[1].attachments, self.robot.attachments) + self.assertTrue(self.milk in self.robot.saved_states[1].attachments) + for link in self.robot.links.values(): + self.assertEqual(link.get_current_state(), link.saved_states[1]) + + def test_restore_state(self): + self.robot.attach(self.milk) + self.robot.save_state(1) + self.milk.save_state(1) + self.assertTrue(self.milk in self.robot.attachments) + self.robot.detach(self.milk) + self.assertTrue(self.milk not in self.robot.attachments) + self.robot.restore_state(1) + self.milk.restore_state(1) + self.assertTrue(self.milk in self.robot.attachments) + for link in self.robot.links.values(): + curr_state = link.get_current_state() + saved_state = link.saved_states[1] + self.assertEqual(curr_state, saved_state) + self.assertEqual(curr_state.constraint_ids, saved_state.constraint_ids) \ No newline at end of file From e29653713b4739ddfc1843d9e6cef21910a6ca64 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 18 Jan 2024 19:46:25 +0100 Subject: [PATCH 28/69] [WorldAbstractClass] Reseting the world now also has the option to remove all saved sates. world saved states is a list of int state ids WorldState data class moved to world_dataclasses.py added the method to remove physics simulator state add reset object method in the Object class. --- src/pycram/bullet_world.py | 7 ++++ src/pycram/world.py | 71 +++++++++++++++++++-------------- src/pycram/world_dataclasses.py | 9 ++++- test/test_object.py | 2 +- 4 files changed, 57 insertions(+), 32 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index ad1cc7abe..9445aca34 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -93,6 +93,7 @@ def add_constraint(self, constraint: Constraint) -> int: return constraint_id def remove_constraint(self, constraint_id): + print("Removing constraint with id: ", constraint_id) p.removeConstraint(constraint_id, physicsClientId=self.client_id) def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: @@ -327,6 +328,12 @@ def restore_physics_simulator_state(self, state_id): """ p.restoreState(state_id, physicsClientId=self.client_id) + def remove_physics_simulator_state(self, state_id: int): + """ + Removes the physics simulator state with the given unique state id. + """ + p.removeState(state_id, physicsClientId=self.client_id) + def add_vis_axis(self, pose: Pose, length: Optional[float] = 0.2) -> None: """ diff --git a/src/pycram/world.py b/src/pycram/world.py index e3eb38116..f4ff2c2e9 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -33,21 +33,7 @@ from abc import ABC, abstractmethod from dataclasses import dataclass -from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox - - -@dataclass -class WorldState: - state_id: int - attachments: Dict[Object, Dict[Object, Attachment]] - - -@dataclass -class CollisionCallbacks: - obj_1: Object - obj_2: Object - on_collision_cb: Callable - no_collision_cb: Optional[Callable] = None +from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox, CollisionCallbacks class World(ABC): @@ -113,8 +99,7 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: self._init_events() - self._saved_states: Dict[int, WorldState] = {} - # Different states of the world indexed by int state id. + self.saved_states: List[int] = [] def _init_events(self): self.detachment_event: Event = Event() @@ -597,6 +582,7 @@ def save_state(self) -> int: """ state_id = self.save_physics_simulator_state() self.save_objects_state(state_id) + self.saved_states.append(state_id) return state_id def save_objects_state(self, state_id: int): @@ -621,6 +607,13 @@ def save_physics_simulator_state(self) -> int: """ pass + @abstractmethod + def remove_physics_simulator_state(self, state_id: int): + """ + Removes the state of the physics simulator with the given id. + """ + pass + @abstractmethod def restore_physics_simulator_state(self, state_id: int): """ @@ -663,7 +656,8 @@ def register_two_objects_collision_callbacks(self, :param on_collision_callback: A function that should be called if the objects are in contact :param on_collision_removal_callback: A function that should be called if the objects are not in contact """ - self.coll_callbacks[(object_a, object_b)] = (on_collision_callback, on_collision_removal_callback) + self.coll_callbacks[(object_a, object_b)] = CollisionCallbacks(on_collision_callback, + on_collision_removal_callback) @classmethod def add_resource_path(cls, path: str) -> None: @@ -710,17 +704,24 @@ def get_object_from_prospection_object(self, prospection_object: Object) -> Obje except ValueError: raise ValueError("The given object is not in the shadow world.") - def reset_world(self) -> None: + def reset_world(self, remove_saved_states=True) -> None: """ Resets the World to the state it was first spawned in. All attached objects will be detached, all joints will be set to the default position of 0 and all objects will be set to the position and orientation in which they were spawned. """ + + if remove_saved_states: + self.remove_saved_states() + for obj in self.objects: - obj.detach_all() - obj.reset_all_joints_positions() - obj.set_pose(obj.original_pose) + obj.reset(remove_saved_states) + + def remove_saved_states(self): + for state_id in self.saved_states: + self.remove_physics_simulator_state(state_id) + self.saved_states = [] def update_transforms_for_objects_in_current_world(self) -> None: """ @@ -1066,7 +1067,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.world.set_robot_if_not_set(self) self.link_name_to_id[self.urdf_object.get_root()] = -1 self.link_id_to_name[-1] = self.urdf_object.get_root() - self.links = self._init_links() + self.links: Dict[str, Link] = self._init_links() self.update_link_transforms() self._current_joints_positions = {} @@ -1126,6 +1127,19 @@ def remove(self) -> None: if World.robot == self: World.robot = None + def reset(self, remove_saved_states=True) -> None: + """ + Resets the Object to the state it was first spawned in. + All attached objects will be detached, all joints will be set to the + default position of 0 and the object will be set to the position and + orientation in which it was spawned. + """ + self.detach_all() + self.reset_all_joints_positions() + self.set_pose(self.original_pose) + if remove_saved_states: + self.saved_states = {} + def attach(self, child_object: Object, parent_link: Optional[str] = None, @@ -1267,11 +1281,10 @@ def restore_links_states(self, state_id): for link in self.links.values(): link.restore_state(state_id) - def set_object_state(self, obj_state:ObjectState): - self.set_attachments(obj_state.attachments) - - def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: - self.attachments = attachments + def remove_saved_states(self): + self.saved_states = {} + for link in self.links.values(): + link.saved_states = {} def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: """ @@ -1662,8 +1675,6 @@ def __init__(self, """ self.parent_link = parent_link self.child_link = child_link - self.parent_object = parent_link.object - self.child_object = child_link.object self.bidirectional = bidirectional self._loose = False and not bidirectional diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 3910aae73..fb057aa3a 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from typing import List, Optional, Tuple +from typing import List, Optional, Tuple, Callable from .enums import JointType @@ -63,6 +63,7 @@ def from_list(cls, point: List[float]): """ return cls(point[0], point[1], point[2]) + @dataclass class AxisAlignedBoundingBox: """ @@ -132,3 +133,9 @@ def get_max(self) -> List[float]: :return: The maximum point of the axis-aligned bounding box """ return [self.max_x, self.max_y, self.max_z] + + +@dataclass +class CollisionCallbacks: + on_collision_cb: Callable + no_collision_cb: Optional[Callable] = None \ No newline at end of file diff --git a/test/test_object.py b/test/test_object.py index f04866517..254495c5f 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -18,7 +18,7 @@ def test_set_position_as_pose(self): self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) def test_save_state(self): - self.robot.attach(self.milk, parent_link="r_gripper_palm_link") + self.robot.attach(self.milk) self.robot.save_state(1) self.assertEqual(self.robot.saved_states[1].attachments, self.robot.attachments) self.assertTrue(self.milk in self.robot.saved_states[1].attachments) From 62c5309aeb98b1df3e5eaa13734ba698f4a566b1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 19 Jan 2024 17:01:35 +0100 Subject: [PATCH 29/69] [WorldAbstractClass] World reasoning is refactored to depend on abstract class. Not passing tests yet. --- doc/source/notebooks/intro.ipynb | 4 +- examples/intro.ipynb | 4 +- src/pycram/bullet_world.py | 42 +- src/pycram/costmaps.py | 12 +- src/pycram/external_interfaces/ik.py | 1 - src/pycram/helper.py | 23 +- src/pycram/old_world_reasoning.py | 365 +++++++++++ .../process_modules/boxy_process_modules.py | 20 +- .../process_modules/pr2_process_modules.py | 16 +- src/pycram/world.py | 26 +- src/pycram/world_reasoning.py | 577 +++++++++--------- test/test_bullet_world.py | 2 - test/test_bullet_world_reasoning.py | 9 +- 13 files changed, 736 insertions(+), 365 deletions(-) create mode 100644 src/pycram/old_world_reasoning.py diff --git a/doc/source/notebooks/intro.ipynb b/doc/source/notebooks/intro.ipynb index f5875cb3a..d38e92ecc 100644 --- a/doc/source/notebooks/intro.ipynb +++ b/doc/source/notebooks/intro.ipynb @@ -181,9 +181,7 @@ } ], "source": [ - "from pycram.bullet_world_reasoning import _get_images_for_target\n", - "\n", - "_get_images_for_target(Pose([1, 0, 1], [0, 0, 0, 1]), Pose([0.3, 0, 1], [0, 0, 0, 1]))" + "world.get_images_for_target(Pose([1, 0, 1], [0, 0, 0, 1]), Pose([0.3, 0, 1], [0, 0, 0, 1]))" ] }, { diff --git a/examples/intro.ipynb b/examples/intro.ipynb index f5875cb3a..d38e92ecc 100644 --- a/examples/intro.ipynb +++ b/examples/intro.ipynb @@ -181,9 +181,7 @@ } ], "source": [ - "from pycram.bullet_world_reasoning import _get_images_for_target\n", - "\n", - "_get_images_for_target(Pose([1, 0, 1], [0, 0, 0, 1]), Pose([0.3, 0, 1], [0, 0, 0, 1]))" + "world.get_images_for_target(Pose([1, 0, 1], [0, 0, 0, 1]), Pose([0.3, 0, 1], [0, 0, 0, 1]))" ] }, { diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 9445aca34..137764fc0 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -152,6 +152,12 @@ def get_object_link_pose(self, obj_id: int, link_id: int) -> Pose: """ return Pose(*p.getLinkState(obj_id, link_id, physicsClientId=self.client_id)[4:6]) + def perform_collision_detection(self) -> None: + """ + Performs collision detection and updates the contact points. + """ + p.performCollisionDetection(physicsClientId=self.client_id) + def get_object_contact_points(self, obj: Object) -> List: """l.update_transforms_for_object(self.milk) Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned @@ -160,7 +166,7 @@ def get_object_contact_points(self, obj: Object) -> List: :param obj: The object. :return: A list of all contact points with other objects """ - return p.getContactPoints(obj.id) + return p.getContactPoints(obj.id, physicsClientId=self.client_id) def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: """ @@ -170,7 +176,7 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> :param obj2: The second object. :return: A list of all contact points between the two objects. """ - return p.getContactPoints(obj1.id, obj2.id) + return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.client_id) def get_object_joint_names(self, obj_id: int) -> List[str]: """ @@ -200,7 +206,7 @@ def get_object_number_of_joints(self, obj_id: int) -> int: :param obj_id: The object """ - return p.getNumJoints(obj_id, self.client_id) + return p.getNumJoints(obj_id, physicsClientId=self.client_id) def reset_object_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ @@ -374,6 +380,36 @@ def remove_vis_axis(self) -> None: p.removeBody(id) self.vis_axis = [] + def get_images_for_target(self, + target_pose: Pose, + cam_pose: Pose, + size: Optional[int] = 256) -> List[np.ndarray]: + """ + Calculates the view and projection Matrix and returns 3 images: + + 1. An RGB image + 2. A depth image + 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object. + + From the given target_pose and cam_pose only the position is used. + + :param cam_pose: The pose of the camera + :param target_pose: The pose to which the camera should point to + :param size: The height and width of the images in pixel + :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. + """ + # TODO: Might depend on robot cameras, if so please add these camera parameters to RobotDescription object + # TODO: of your robot with a CameraDescription object. + fov = 90 + aspect = size / size + near = 0.2 + far = 100 + + view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1]) + projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) + return list(p.getCameraImage(size, size, view_matrix, projection_matrix, + physicsClientId=self.client_id))[2:5] + class Gui(threading.Thread): """ diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index b0d2f5ccb..100ecbdbf 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -10,7 +10,6 @@ import time from .bullet_world import BulletWorld from pycram.world import UseProspectionWorld, Object, Link -from .world_reasoning import _get_images_for_target from .world_dataclasses import AxisAlignedBoundingBox from nav_msgs.msg import OccupancyGrid, MapMetaData from typing import Tuple, List, Union, Optional @@ -500,22 +499,19 @@ def _create_images(self) -> List[np.ndarray]: origin_copy = self.origin.copy() origin_copy.position.y += 1 images.append( - _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_world, size=self.size)[1]) + self.world.get_images_for_target(origin_copy, camera_pose, size=self.size)[1]) origin_copy = self.origin.copy() origin_copy.position.x -= 1 - images.append( - _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_world, size=self.size)[1]) + images.append(self.world.get_images_for_target(origin_copy, camera_pose, size=self.size)[1]) origin_copy = self.origin.copy() origin_copy.position.y -= 1 - images.append( - _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_world, size=self.size)[1]) + images.append(self.world.get_images_for_target(origin_copy, camera_pose, size=self.size)[1]) origin_copy = self.origin.copy() origin_copy.position.x += 1 - images.append( - _get_images_for_target(origin_copy, camera_pose, BulletWorld.current_world, size=self.size)[1]) + images.append(self.world.get_images_for_target(origin_copy, camera_pose, size=self.size)[1]) for i in range(0, 4): images[i] = self._depth_buffer_to_meter(images[i]) diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index f0aeaac94..664d5133d 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -125,7 +125,6 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str end_effector = robot_description.get_child(joints[-1]) target_torso = local_transformer.transform_pose_to_target_frame(target_pose, robot.links[base_link].tf_frame) - # target_torso = _transform_to_torso(pose, shadow_robot) diff = calculate_wrist_tool_offset(end_effector, gripper, robot) target_diff = target_torso.to_transform("target").inverse_times(diff).to_pose() diff --git a/src/pycram/helper.py b/src/pycram/helper.py index c098eca99..090ac17ff 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -11,18 +11,15 @@ from typing import Tuple, Callable import numpy as np -import pybullet as p from pytransform3d.rotations import quaternion_wxyz_from_xyzw, quaternion_xyzw_from_wxyz from pytransform3d.transformations import transform_from_pq, transform_from, pq_from_transform from macropy.core.quotes import ast_literal, q from .world import Object as BulletWorldObject -from .local_transformer import LocalTransformer from .pose import Transform, Pose -from .robot_descriptions import robot_description -import os import math + class bcolors: """ Color codes which can be used to highlight Text in the Terminal. For example, @@ -59,28 +56,10 @@ def _apply_ik(robot: BulletWorldObject, joint_poses: List[float], joints: List[s # robot.set_joint_state(joints[i], joint_poses[i]) -def _transform_to_torso(pose_and_rotation: Tuple[List[float], List[float]], robot: BulletWorldObject) -> Tuple[ - List[float], List[float]]: - map_T_torso = robot.links[robot_description.torso_link].pose_as_list - torso_T_map = p.invertTransform(map_T_torso[0], map_T_torso[1]) - map_T_target = pose_and_rotation - torso_T_target = p.multiplyTransforms(torso_T_map[0], torso_T_map[1], map_T_target[0], map_T_target[1]) - return torso_T_target - - def calculate_wrist_tool_offset(wrist_frame: str, tool_frame: str, robot: BulletWorldObject) -> Transform: return robot.links[wrist_frame].get_transform_to_link(robot.links[tool_frame]) -def inverseTimes(transform1: Tuple[List[float], List[float]], transform2: Tuple[List[float], List[float]]) -> Tuple[ - List[float], List[float]]: - """ - Like a Minus for Transforms, this subtracts the second transform from the first. - """ - inv = p.invertTransform(transform2[0], transform2[1]) - return p.multiplyTransforms(transform1[0], transform1[1], inv[0], inv[1]) - - def transform(pose: List[float], transformation: List[float], local_coords=False): # TODO: if pose is a list of position and orientation calculate new pose w/ orientation too diff --git a/src/pycram/old_world_reasoning.py b/src/pycram/old_world_reasoning.py new file mode 100644 index 000000000..452cce72c --- /dev/null +++ b/src/pycram/old_world_reasoning.py @@ -0,0 +1,365 @@ +import pybullet as p +import itertools +import numpy as np +import rospy + +from .world import _world_and_id, Object, UseProspectionWorld +from .bullet_world import BulletWorld +from .external_interfaces.ik import request_ik +from .local_transformer import LocalTransformer +from .plan_failures import IKError +from .robot_descriptions import robot_description +from .helper import _apply_ik +from .pose import Pose, Transform +from typing import List, Tuple, Optional, Union, Dict + + +class ReasoningError(Exception): + def __init__(self, *args, **kwargs): + Exception.__init__(self, *args, **kwargs) + + +class CollisionError(Exception): + def __init__(self, *args, **kwargs): + Exception.__init__(self, *args, **kwargs) + + +def _get_joint_names(robot: Object, tip_link: str) -> List[str]: + res = [] + for i in range(p.getNumJoints(robot.id)): + info = p.getJointInfo(robot.id, i) + if info[2] != p.JOINT_FIXED: + res.append(info[1]) + return res + + +def _get_images_for_target(target_pose: Pose, + cam_pose: Pose, + world: Optional[BulletWorld] = None, + size: Optional[int] = 256) -> List[np.ndarray]: + """ + Calculates the view and projection Matrix and returns 3 images: + + 1. An RGB image + 2. A depth image + 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object. + + From the given target_pose and cam_pose only the position is used. + + :param cam_pose: The pose of the camera + :param target_pose: The pose to which the camera should point to + :param size: The height and width of the images in pixel + :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. + """ + world, world_id = _world_and_id(world) + # TODO: Might depend on robot cameras, if so please add these camera parameters to RobotDescription object + # TODO: of your robot with a CameraDescription object. + fov = 90 + aspect = size / size + near = 0.2 + far = 100 + + view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1]) + projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) + return list(p.getCameraImage(size, size, view_matrix, projection_matrix, physicsClientId=world_id))[2:5] + + +def _get_joint_ranges(robot: Object) -> Tuple[List, List, List, List, List]: + """ + Calculates the lower and upper limits, the joint ranges and the joint damping. For a given robot Object. + Fixed joints will be skipped because they don't have limits or ranges. + + :param robot: The robot for whom the values should be calculated + :return: The lists for the upper and lower limits, joint ranges, rest poses and joint damping + """ + ll, ul, jr, rp, jd = [], [], [], [], [] + + for i in range(0, p.getNumJoints(robot.id)): + info = p.getJointInfo(robot.id, i) + if info[3] > -1: + ll.append(info[8]) + ul.append(info[9]) + jr.append(info[9] - info[8]) + rp.append(p.getJointState(robot.id, i)[0]) + jd.append(info[6]) + + return ll, ul, jr, rp, jd + + +def stable(object: Object, + world: Optional[BulletWorld] = None) -> bool: + """ + Checks if an object is stable in the world. Stable meaning that it's position will not change after simulating physics + in the BulletWorld. This will be done by simulating the world for 10 seconds and compare the previous coordinates + with the coordinates after the simulation. + + :param object: The object which should be checked + :param world: The BulletWorld if more than one BulletWorld is active + :return: True if the given object is stable in the world False else + """ + world, world_id = _world_and_id(world) + shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) + with UseProspectionWorld(): + coords_prev = shadow_obj.get_position_as_list() + state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) + p.setGravity(0, 0, -9.8, BulletWorld.current_world.client_id) + + # one Step is approximately 1/240 seconds + BulletWorld.current_world.simulate(2) + # coords_past = p.getBasePositionAndOrientation(object.id, physicsClientId=world_id)[0] + coords_past = shadow_obj.get_position_as_list() + + # p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) + coords_prev = list(map(lambda n: round(n, 3), coords_prev)) + coords_past = list(map(lambda n: round(n, 3), coords_past)) + return coords_past == coords_prev + + +def contact(object1: Object, + object2: Object, + return_links: bool = False) -> Union[bool, Tuple[bool, List]]: + """ + Checks if two objects are in contact or not. If the links should be returned then the output will also contain a + list of tuples where the first element is the link name of 'object1' and the second element is the link name of + 'object2'. + + :param object1: The first object + :param object2: The second object + :param return_links: If the respective links on the objects that are in contact should be returned. + :return: True if the two objects are in contact False else. If links should be returned a list of links in contact + """ + + with UseProspectionWorld(): + shadow_obj1 = BulletWorld.current_world.get_prospection_object_from_object(object1) + shadow_obj2 = BulletWorld.current_world.get_prospection_object_from_object(object2) + p.performCollisionDetection(BulletWorld.current_world.client_id) + con_points = p.getContactPoints(shadow_obj1.id, shadow_obj2.id, + physicsClientId=BulletWorld.current_world.client_id) + + if return_links: + contact_links = [] + for point in con_points: + contact_links.append((shadow_obj1.get_link_by_id(point[3]), shadow_obj2.get_link_by_id(point[4]))) + return con_points != (), contact_links + + else: + return con_points != () + + +def visible(object: Object, + camera_pose: Pose, + front_facing_axis: Optional[List[float]] = None, + threshold: float = 0.8, + world: Optional[BulletWorld] = None) -> bool: + """ + Checks if an object is visible from a given position. This will be achieved by rendering the object + alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the + absolut count of pixels. + + :param object: The object for which the visibility should be checked + :param camera_pose: The pose of the camera in map frame + :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :param threshold: The minimum percentage of the object that needs to be visible for this method to return true + :param world: The BulletWorld if more than one BulletWorld is active + :return: True if the object is visible from the camera_position False if not + """ + front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis + with UseProspectionWorld(): + shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) + if BulletWorld.robot: + shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(BulletWorld.robot) + state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) + for obj in BulletWorld.current_world.objects: + if obj == shadow_obj or BulletWorld.robot and obj == shadow_robot: + continue + else: + obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) + + world_to_cam = camera_pose.to_transform("camera") + cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", "point") + target_point = (world_to_cam * cam_to_point).to_pose() + # target_point = p.multiplyTransforms(world_to_cam.translation_as_list(), world_to_cam.rotation_as_list(), cam_to_point.translation_as_list(), [0, 0, 0, 1]) + # print(target_point) + + seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] + flat_list = list(itertools.chain.from_iterable(seg_mask)) + max_pixel = sum(list(map(lambda x: 1 if x == shadow_obj.id else 0, flat_list))) + p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) + if max_pixel == 0: + # Object is not visible + return False + + seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] + flat_list = list(itertools.chain.from_iterable(seg_mask)) + real_pixel = sum(list(map(lambda x: 1 if x == shadow_obj.id else 0, flat_list))) + + return real_pixel / max_pixel > threshold > 0 + + +def occluding(object: Object, + camera_pose: Pose, + front_facing_axis: Optional[List[float]] = None, + world: Optional[BulletWorld] = None) -> List[Object]: + """ + Lists all objects which are occluding the given object. This works similar to 'visible'. + First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. + After that the complete scene will be rendered and the previous saved pixel positions will be compared to the + actual pixels, if in one pixel another object is visible ot will be saved as occluding. + + :param object: The object for which occlusion should be checked + :param camera_pose: The pose of the camera in world coordinate frame + :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :param world: The BulletWorld if more than one BulletWorld is active + :return: A list of occluding objects + """ + front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis + world, world_id = _world_and_id(world) + # occ_world = world.copy() + # state = p.saveState(physicsClientId=occ_world.client_id) + with UseProspectionWorld(): + state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) + for obj in BulletWorld.current_world.objects: + if obj.name == BulletWorld.robot.name: + continue + elif object.get_pose() == obj.get_pose(): + object = obj + else: + obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) + + world_to_cam = camera_pose.to_transform("camera") + cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", "point") + target_point = (world_to_cam * cam_to_point).to_pose() + + seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] + + # All indices where the object that could be occluded is in the image + # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension + pix = np.dstack((seg_mask == object.id).nonzero())[0] + + p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) + + occluding = [] + seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] + for c in pix: + if not seg_mask[c[0]][c[1]] == object.id: + occluding.append(seg_mask[c[0]][c[1]]) + + occ_objects = list(set(map(BulletWorld.current_world.get_object_by_id, occluding))) + occ_objects = list(map(world.get_object_from_prospection_object, occ_objects)) + + return occ_objects + + +def reachable(pose: Union[Object, Pose], + robot: Object, + gripper_name: str, + threshold: float = 0.01) -> bool: + """ + Checks if the robot can reach a given position. To determine this the inverse kinematics are + calculated and applied. Afterward the distance between the position and the given end effector is calculated, if + it is smaller than the threshold the reasoning query returns True, if not it returns False. + + :param pose: The position and rotation or Object for which reachability should be checked or an Object + :param robot: The robot that should reach for the position + :param gripper_name: The name of the end effector + :param threshold: The threshold between the end effector and the position. + :return: True if the end effector is closer than the threshold to the target position, False in every other case + """ + if type(pose) == Object: + pose = pose.get_pose() + + shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(robot) + with UseProspectionWorld(): + arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" + joints = robot_description.chains[arm].joints + try: + inv = request_ik(pose, shadow_robot, joints, gripper_name) + except IKError as e: + return False + + _apply_ik(shadow_robot, inv, joints) + + diff = pose.dist(shadow_robot.links[gripper_name].pose) + + return diff < threshold + + +def blocking(pose_or_object: Union[Object, Pose], + robot: Object, + gripper_name: str, + grasp: str = None) -> Union[List[Object], None]: + """ + Checks if any objects are blocking another object when a robot tries to pick it. This works + similar to the reachable predicate. First the inverse kinematics between the robot and the object will be calculated + and applied. Then it will be checked if the robot is in contact with any object except the given one. If the given + pose or Object is not reachable None will be returned + + :param pose_or_object: The object or pose for which blocking objects should be found + :param robot: The robot Object who reaches for the object + :param gripper_name: The name of the end effector of the robot + :param grasp: The grasp type with which the object should be grasped + :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose + or object is not reachable. + """ + if type(pose_or_object) == Object: + input_pose = pose_or_object.get_pose() + else: + input_pose = pose_or_object + + shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(robot) + with UseProspectionWorld(): + arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" + joints = robot_description.chains[arm].joints + local_transformer = LocalTransformer() + + target_map = local_transformer.transform_pose_to_target_frame(input_pose, "map") + if grasp: + grasp_orientation = robot_description.grasps.get_orientation_for_grasp(grasp) + target_map.orientation.x = grasp_orientation[0] + target_map.orientation.y = grasp_orientation[1] + target_map.orientation.z = grasp_orientation[2] + target_map.orientation.w = grasp_orientation[3] + + try: + inv = request_ik(target_map, shadow_robot, joints, gripper_name) + except IKError as e: + rospy.logerr(f"Pose is not reachable: {e}") + return None + _apply_ik(shadow_robot, inv, joints) + + block = [] + for obj in BulletWorld.current_world.objects: + if contact(shadow_robot, obj): + block.append(BulletWorld.current_world.get_object_from_prospection_object(obj)) + return block + + +def supporting(object1: Object, + object2: Object) -> bool: + """ + Checks if one object is supporting another object. An object supports another object if they are in + contact and the second object is above the first one. (e.g. a Bottle will be supported by a table) + + :param object1: Object that is supported + :param object2: Object that supports the first object + :return: True if the second object is in contact with the first one and the second one ist above the first False else + """ + return contact(object1, object2) and object2.get_position().z > object1.get_position().z + + +def link_pose_for_joint_config(object: Object, joint_config: Dict[str, float], link_name: str) -> Pose: + """ + Returns the pose a link would be in if the given joint configuration would be applied to the object. This is done + by using the respective object in the shadow world and applying the joint configuration to this one. After applying + the joint configuration the link position is taken from there. + + :param object: Object of which the link is a part + :param joint_config: Dict with the goal joint configuration + :param link_name: Name of the link for which the pose should be returned + :return: The pose of the link after applying the joint configuration + """ + shadow_object = BulletWorld.current_world.get_prospection_object_from_object(object) + with UseProspectionWorld(): + for joint, pose in joint_config.items(): + shadow_object.set_joint_position(joint, pose) + return shadow_object.links[link_name].pose \ No newline at end of file diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index ad2932322..27e4d08c4 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -1,17 +1,27 @@ import time from threading import Lock +from typing import Tuple, List import pybullet as p import pycram.world_reasoning as btr import pycram.helper as helper -from ..bullet_world import BulletWorld +from ..bullet_world import BulletWorld, Object as BulletWorldObject from ..external_interfaces.ik import request_ik from ..local_transformer import LocalTransformer as local_tf from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description +def _transform_to_torso(pose_and_rotation: Tuple[List[float], List[float]], robot: BulletWorldObject) -> Tuple[ + List[float], List[float]]: + map_T_torso = robot.links[robot_description.torso_link].pose_as_list + torso_T_map = p.invertTransform(map_T_torso[0], map_T_torso[1]) + map_T_target = pose_and_rotation + torso_T_target = p.multiplyTransforms(torso_T_map[0], torso_T_map[1], map_T_target[0], map_T_target[1]) + return torso_T_target + + def _park_arms(arm): """ Defines the joint poses for the parking positions of the arms of Boxy and applies them to the @@ -60,7 +70,7 @@ def _execute(self, desig): robot = BulletWorld.robot grasp = robot_description.grasps.get_orientation_for_grasp(solution['grasp']) target = [object.get_position(), grasp] - target = helper._transform_to_torso(target, robot) + target = _transform_to_torso(target, robot) arm = "left" if solution['gripper'] == robot_description.get_tool_frame("left") else "right" joints = robot_description._safely_access_chains(arm).joints #tip = "r_wrist_roll_link" if solution['gripper'] == "r_gripper_tool_frame" else "l_wrist_roll_link" @@ -83,7 +93,7 @@ def _execute(self, desig): object = solution['object'] robot = BulletWorld.robot target = object.get_position_and_orientation() - target = helper._transform_to_torso(target, robot) + target = _transform_to_torso(target, robot) arm = "left" if solution['gripper'] == robot_description.get_tool_frame("left") else "right" joints = robot_description._safely_access_chains(arm).joints #tip = "r_wrist_roll_link" if solution['gripper'] == "r_gripper_tool_frame" else "l_wrist_roll_link" @@ -113,7 +123,7 @@ def _execute(self, desig): robot.set_joint_position(robot_description.torso_joint, -0.1) arm = "left" if solution['gripper'] == robot_description.get_tool_frame("left") else "right" joints = robot_description._safely_access_chains(arm).joints - target = helper._transform_to_torso(kitchen.links[drawer_handle].pose, robot) + target = _transform_to_torso(kitchen.links[drawer_handle].pose, robot) inv = request_ik(robot_description.base_frame, gripper, target , robot, joints ) helper._apply_ik(robot, inv, gripper) time.sleep(0.2) @@ -121,7 +131,7 @@ def _execute(self, desig): robot.set_position([cur_pose[0]-dis, cur_pose[1], cur_pose[2]]) han_pose = kitchen.links[drawer_handle].position new_p = [[han_pose[0] - dis, han_pose[1], han_pose[2]], kitchen.links[drawer_handle].orientation] - new_p = helper._transform_to_torso(new_p, robot) + new_p = _transform_to_torso(new_p, robot) inv = request_ik(robot_description.base_frame, gripper, new_p, robot, joints) helper._apply_ik(robot, inv, gripper) kitchen.set_joint_position(drawer_joint, 0.3) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 8c96af6e7..93254ba0e 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -1,4 +1,3 @@ -from abc import ABC from threading import Lock from typing import Any @@ -6,17 +5,12 @@ import pycram.world_reasoning as btr import numpy as np -import time import rospy -import pybullet as p - -from ..plan_failures import EnvironmentManipulationImpossible -from ..robot_descriptions import robot_description -from ..process_module import ProcessModule, ProcessModuleManager -from ..bullet_world import BulletWorld, Object -from ..helper import transform -from ..external_interfaces.ik import request_ik, IKError -from ..helper import _transform_to_torso, _apply_ik, calculate_wrist_tool_offset, inverseTimes + + +from ..process_module import ProcessModule +from ..external_interfaces.ik import request_ik +from ..helper import _apply_ik from ..local_transformer import LocalTransformer from ..designators.motion_designator import * from ..enums import JointType, ObjectType diff --git a/src/pycram/world.py b/src/pycram/world.py index f4ff2c2e9..fa5b50d3d 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -13,7 +13,6 @@ from tf.transformations import quaternion_from_euler from typing import List, Optional, Dict, Tuple, Callable from typing import Union -from copy import deepcopy import numpy as np import rospkg @@ -298,6 +297,13 @@ def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: # Simulation runs at 240 Hz time.sleep(self.simulation_time_step) + @abstractmethod + def perform_collision_detection(self) -> None: + """ + Checks for collisions between all objects in the World and updates the contact points. + """ + pass + @abstractmethod def get_object_contact_points(self, obj: Object) -> List: """ @@ -626,6 +632,24 @@ def restore_objects_states(self, state_id: int): for obj in self.objects: obj.restore_state(state_id) + def get_images_for_target(self, + target_pose: Pose, + cam_pose: Pose, + size: Optional[int] = 256) -> List[np.ndarray]: + """ + Calculates the view and projection Matrix and returns 3 images: + + 1. An RGB image + 2. A depth image + 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object. + + :param cam_pose: The pose of the camera + :param target_pose: The pose to which the camera should point to + :param size: The height and width of the images in pixel + :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. + """ + pass + def _copy(self) -> World: """ Copies this World into another and returns it. The other World diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 1e5f48c3e..d992fd0a0 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -4,12 +4,12 @@ import rospy from .world import _world_and_id, Object, UseProspectionWorld -from .bullet_world import BulletWorld +from .world import World from .external_interfaces.ik import request_ik from .local_transformer import LocalTransformer from .plan_failures import IKError from .robot_descriptions import robot_description -from .helper import _transform_to_torso, _apply_ik, calculate_wrist_tool_offset, inverseTimes +from .helper import _apply_ik from .pose import Pose, Transform from typing import List, Tuple, Optional, Union, Dict @@ -24,46 +24,6 @@ def __init__(self, *args, **kwargs): Exception.__init__(self, *args, **kwargs) -def _get_joint_names(robot: Object, tip_link: str) -> List[str]: - res = [] - for i in range(p.getNumJoints(robot.id)): - info = p.getJointInfo(robot.id, i) - if info[2] != p.JOINT_FIXED: - res.append(info[1]) - return res - - -def _get_images_for_target(target_pose: Pose, - cam_pose: Pose, - world: Optional[BulletWorld] = None, - size: Optional[int] = 256) -> List[np.ndarray]: - """ - Calculates the view and projection Matrix and returns 3 images: - - 1. An RGB image - 2. A depth image - 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object. - - From the given target_pose and cam_pose only the position is used. - - :param cam_pose: The pose of the camera - :param target_pose: The pose to which the camera should point to - :param size: The height and width of the images in pixel - :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. - """ - world, world_id = _world_and_id(world) - # TODO: Might depend on robot cameras, if so please add these camera parameters to RobotDescription object - # TODO: of your robot with a CameraDescription object. - fov = 90 - aspect = size / size - near = 0.2 - far = 100 - - view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1]) - projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) - return list(p.getCameraImage(size, size, view_matrix, projection_matrix, physicsClientId=world_id))[2:5] - - def _get_joint_ranges(robot: Object) -> Tuple[List, List, List, List, List]: """ Calculates the lower and upper limits, the joint ranges and the joint damping. For a given robot Object. @@ -86,280 +46,287 @@ def _get_joint_ranges(robot: Object) -> Tuple[List, List, List, List, List]: return ll, ul, jr, rp, jd -def stable(object: Object, - world: Optional[BulletWorld] = None) -> bool: - """ - Checks if an object is stable in the world. Stable meaning that it's position will not change after simulating physics - in the BulletWorld. This will be done by simulating the world for 10 seconds and compare the previous coordinates - with the coordinates after the simulation. - - :param object: The object which should be checked - :param world: The BulletWorld if more than one BulletWorld is active - :return: True if the given object is stable in the world False else +def _try_to_reach_or_grasp(pose_or_object: Union[Pose, Object], prospection_robot: Object, gripper_name: str, + grasp: Optional[str] = None) -> Union[Pose, None]: """ - world, world_id = _world_and_id(world) - shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) - with UseProspectionWorld(): - coords_prev = shadow_obj.get_position_as_list() - state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) - p.setGravity(0, 0, -9.8, BulletWorld.current_world.client_id) - - # one Step is approximately 1/240 seconds - BulletWorld.current_world.simulate(2) - # coords_past = p.getBasePositionAndOrientation(object.id, physicsClientId=world_id)[0] - coords_past = shadow_obj.get_position_as_list() - - # p.restoreState(state, physicsClientId=BulletWorld.current_bullet_world.client_id) - coords_prev = list(map(lambda n: round(n, 3), coords_prev)) - coords_past = list(map(lambda n: round(n, 3), coords_past)) - return coords_past == coords_prev - - -def contact(object1: Object, - object2: Object, - return_links: bool = False) -> Union[bool, Tuple[bool, List]]: - """ - Checks if two objects are in contact or not. If the links should be returned then the output will also contain a - list of tuples where the first element is the link name of 'object1' and the second element is the link name of - 'object2'. - - :param object1: The first object - :param object2: The second object - :param return_links: If the respective links on the objects that are in contact should be returned. - :return: True if the two objects are in contact False else. If links should be returned a list of links in contact - """ - - with UseProspectionWorld(): - shadow_obj1 = BulletWorld.current_world.get_prospection_object_from_object(object1) - shadow_obj2 = BulletWorld.current_world.get_prospection_object_from_object(object2) - p.performCollisionDetection(BulletWorld.current_world.client_id) - con_points = p.getContactPoints(shadow_obj1.id, shadow_obj2.id, - physicsClientId=BulletWorld.current_world.client_id) - - if return_links: - contact_links = [] - for point in con_points: - contact_links.append((shadow_obj1.get_link_by_id(point[3]), shadow_obj2.get_link_by_id(point[4]))) - return con_points != (), contact_links + Checks if the robot can reach a given position and optionally also grasp an object. + To determine this the inverse kinematics are calculated and applied. - else: - return con_points != () - - -def visible(object: Object, - camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None, - threshold: float = 0.8, - world: Optional[BulletWorld] = None) -> bool: - """ - Checks if an object is visible from a given position. This will be achieved by rendering the object - alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the - absolut count of pixels. - - :param object: The object for which the visibility should be checked - :param camera_pose: The pose of the camera in map frame - :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz - :param threshold: The minimum percentage of the object that needs to be visible for this method to return true - :param world: The BulletWorld if more than one BulletWorld is active - :return: True if the object is visible from the camera_position False if not - """ - front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis - with UseProspectionWorld(): - shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) - if BulletWorld.robot: - shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(BulletWorld.robot) - state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) - for obj in BulletWorld.current_world.objects: - if obj == shadow_obj or BulletWorld.robot and obj == shadow_robot: - continue - else: - obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - - world_to_cam = camera_pose.to_transform("camera") - cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", "point") - target_point = (world_to_cam * cam_to_point).to_pose() - # target_point = p.multiplyTransforms(world_to_cam.translation_as_list(), world_to_cam.rotation_as_list(), cam_to_point.translation_as_list(), [0, 0, 0, 1]) - # print(target_point) - - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] - flat_list = list(itertools.chain.from_iterable(seg_mask)) - max_pixel = sum(list(map(lambda x: 1 if x == shadow_obj.id else 0, flat_list))) - p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) - if max_pixel == 0: - # Object is not visible - return False - - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] - flat_list = list(itertools.chain.from_iterable(seg_mask)) - real_pixel = sum(list(map(lambda x: 1 if x == shadow_obj.id else 0, flat_list))) - - return real_pixel / max_pixel > threshold > 0 - - -def occluding(object: Object, - camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None, - world: Optional[BulletWorld] = None) -> List[Object]: - """ - Lists all objects which are occluding the given object. This works similar to 'visible'. - First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. - After that the complete scene will be rendered and the previous saved pixel positions will be compared to the - actual pixels, if in one pixel another object is visible ot will be saved as occluding. - - :param object: The object for which occlusion should be checked - :param camera_pose: The pose of the camera in world coordinate frame - :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz - :param world: The BulletWorld if more than one BulletWorld is active - :return: A list of occluding objects - """ - front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis - world, world_id = _world_and_id(world) - # occ_world = world.copy() - # state = p.saveState(physicsClientId=occ_world.client_id) - with UseProspectionWorld(): - state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) - for obj in BulletWorld.current_world.objects: - if obj.name == BulletWorld.robot.name: - continue - elif object.get_pose() == obj.get_pose(): - object = obj - else: - obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - - world_to_cam = camera_pose.to_transform("camera") - cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", "point") - target_point = (world_to_cam * cam_to_point).to_pose() - - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] - - # All indices where the object that could be occluded is in the image - # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension - pix = np.dstack((seg_mask == object.id).nonzero())[0] - - p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) - - occluding = [] - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] - for c in pix: - if not seg_mask[c[0]][c[1]] == object.id: - occluding.append(seg_mask[c[0]][c[1]]) - - occ_objects = list(set(map(BulletWorld.current_world.get_object_by_id, occluding))) - occ_objects = list(map(world.get_object_from_prospection_object, occ_objects)) - - return occ_objects - - -def reachable(pose: Union[Object, Pose], - robot: Object, - gripper_name: str, - threshold: float = 0.01) -> bool: - """ - Checks if the robot can reach a given position. To determine this the inverse kinematics are - calculated and applied. Afterward the distance between the position and the given end effector is calculated, if - it is smaller than the threshold the reasoning query returns True, if not it returns False. - - :param pose: The position and rotation or Object for which reachability should be checked or an Object - :param robot: The robot that should reach for the position + :param pose_or_object: The position and rotation or Object for which reachability should be checked or an Object + :param prospection_robot: The robot that should reach for the position :param gripper_name: The name of the end effector - :param threshold: The threshold between the end effector and the position. - :return: True if the end effector is closer than the threshold to the target position, False in every other case - """ - if type(pose) == Object: - pose = pose.get_pose() - - shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(robot) - with UseProspectionWorld(): - arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" - joints = robot_description.chains[arm].joints - try: - inv = request_ik(pose, shadow_robot, joints, gripper_name) - except IKError as e: - return False - - _apply_ik(shadow_robot, inv, joints) - - diff = pose.dist(shadow_robot.links[gripper_name].pose) - - return diff < threshold - - -def blocking(pose_or_object: Union[Object, Pose], - robot: Object, - gripper_name: str, - grasp: str = None) -> Union[List[Object], None]: - """ - Checks if any objects are blocking another object when a robot tries to pick it. This works - similar to the reachable predicate. First the inverse kinematics between the robot and the object will be calculated - and applied. Then it will be checked if the robot is in contact with any object except the given one. If the given - pose or Object is not reachable None will be returned - - :param pose_or_object: The object or pose for which blocking objects should be found - :param robot: The robot Object who reaches for the object - :param gripper_name: The name of the end effector of the robot :param grasp: The grasp type with which the object should be grasped - :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose - or object is not reachable. """ - if type(pose_or_object) == Object: + if isinstance(pose_or_object, Object): input_pose = pose_or_object.get_pose() else: input_pose = pose_or_object - shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(robot) - with UseProspectionWorld(): - arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" - joints = robot_description.chains[arm].joints + arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" + joints = robot_description.chains[arm].joints + target_pose = input_pose + if grasp: local_transformer = LocalTransformer() - target_map = local_transformer.transform_pose_to_target_frame(input_pose, "map") - if grasp: - grasp_orientation = robot_description.grasps.get_orientation_for_grasp(grasp) - target_map.orientation.x = grasp_orientation[0] - target_map.orientation.y = grasp_orientation[1] - target_map.orientation.z = grasp_orientation[2] - target_map.orientation.w = grasp_orientation[3] - - try: - inv = request_ik(target_map, robot, joints, gripper_name) - except IKError as e: - rospy.logerr(f"Pose is not reachable: {e}") - return None - _apply_ik(shadow_robot, inv, joints) - - block = [] - for obj in BulletWorld.current_world.objects: - if contact(shadow_robot, obj): - block.append(BulletWorld.current_world.get_object_from_prospection_object(obj)) - return block - - -def supporting(object1: Object, - object2: Object) -> bool: - """ - Checks if one object is supporting another object. An object supports another object if they are in - contact and the second object is above the first one. (e.g. a Bottle will be supported by a table) + grasp_orientation = robot_description.grasps.get_orientation_for_grasp(grasp) + target_map.orientation.x = grasp_orientation[0] + target_map.orientation.y = grasp_orientation[1] + target_map.orientation.z = grasp_orientation[2] + target_map.orientation.w = grasp_orientation[3] + target_pose = target_map + + try: + inv = request_ik(target_pose, prospection_robot, joints, gripper_name) + except IKError as e: + rospy.logerr(f"Pose is not reachable: {e}") + return None + _apply_ik(prospection_robot, inv, joints) + + return target_pose + + +class WorldReasoning: + + def __init__(self, world: World): + self.world = world + + def stable(self, obj: Object) -> bool: + """ + Checks if an object is stable in the world. Stable meaning that it's position will not change after simulating physics + in the BulletWorld. This will be done by simulating the world for 10 seconds and compare the previous coordinates + with the coordinates after the simulation. + + :param obj: The object which should be checked + :return: True if the given object is stable in the world False else + """ + prospection_obj = self.world.get_prospection_object_from_object(obj) + with UseProspectionWorld(): + coords_prev = prospection_obj.get_position_as_list() + self.world.set_gravity([0, 0, -9.8]) + + self.world.simulate(2) + coords_past = prospection_obj.get_position_as_list() + + coords_prev = list(map(lambda n: round(n, 3), coords_prev)) + coords_past = list(map(lambda n: round(n, 3), coords_past)) + return coords_past == coords_prev + + def contact(self, + object1: Object, + object2: Object, + return_links: bool = False) -> Union[bool, Tuple[bool, List]]: + """ + Checks if two objects are in contact or not. If the links should be returned then the output will also contain a + list of tuples where the first element is the link name of 'object1' and the second element is the link name of + 'object2'. + + :param object1: The first object + :param object2: The second object + :param return_links: If the respective links on the objects that are in contact should be returned. + :return: True if the two objects are in contact False else. If links should be returned a list of links in contact + """ + + with UseProspectionWorld(): + prospection_obj1 = self.world.get_prospection_object_from_object(object1) + prospection_obj2 = self.world.get_prospection_object_from_object(object2) + + self.world.perform_collision_detection() + con_points = self.world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) + + if return_links: + contact_links = [] + for point in con_points: + contact_links.append((prospection_obj1.get_link_by_id(point[3]), prospection_obj2.get_link_by_id(point[4]))) + return con_points != (), contact_links - :param object1: Object that is supported - :param object2: Object that supports the first object - :return: True if the second object is in contact with the first one and the second one ist above the first False else - """ - return contact(object1, object2) and object2.get_position().z > object1.get_position().z + else: + return con_points != () + def get_visible_objects(self, camera_pose: Pose, + front_facing_axis: Optional[List[float]] = None) -> Tuple[np.ndarray, Pose]: -def link_pose_for_joint_config(object: Object, joint_config: Dict[str, float], link_name: str) -> Pose: - """ - Returns the pose a link would be in if the given joint configuration would be applied to the object. This is done - by using the respective object in the shadow world and applying the joint configuration to this one. After applying - the joint configuration the link position is taken from there. - - :param object: Object of which the link is a part - :param joint_config: Dict with the goal joint configuration - :param link_name: Name of the link for which the pose should be returned - :return: The pose of the link after applying the joint configuration - """ - shadow_object = BulletWorld.current_world.get_prospection_object_from_object(object) - with UseProspectionWorld(): - for joint, pose in joint_config.items(): - shadow_object.set_joint_position(joint, pose) - return shadow_object.links[link_name].pose + front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis + + world_to_cam = camera_pose.to_transform("camera") + + cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", + "point") + target_point = (world_to_cam * cam_to_point).to_pose() + + seg_mask = self.world.get_images_for_target(target_point, world_to_cam.to_pose())[2] + + return seg_mask, target_point + + def visible(self, + obj: Object, + camera_pose: Pose, + front_facing_axis: Optional[List[float]] = None, + threshold: float = 0.8) -> bool: + """ + Checks if an object is visible from a given position. This will be achieved by rendering the object + alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the + absolut count of pixels. + + :param obj: The object for which the visibility should be checked + :param camera_pose: The pose of the camera in map frame + :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :param threshold: The minimum percentage of the object that needs to be visible for this method to return true. + :return: True if the object is visible from the camera_position False if not + """ + with UseProspectionWorld(): + prospection_obj = self.world.get_prospection_object_from_object(obj) + if World.robot: + prospection_robot = self.world.get_prospection_object_from_object(World.robot) + + state_id = self.world.save_state() + for obj in self.world.objects: + if obj == prospection_obj or World.robot and obj == prospection_robot: + continue + else: + obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) + + seg_mask, target_point = self.get_visible_objects(camera_pose, front_facing_axis) + flat_list = list(itertools.chain.from_iterable(seg_mask)) + max_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) + self.world.restore_state(state_id) + + if max_pixel == 0: + # Object is not visible + return False + + seg_mask = self.world.get_images_for_target(target_point, camera_pose)[2] + flat_list = list(itertools.chain.from_iterable(seg_mask)) + real_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) + + return real_pixel / max_pixel > threshold > 0 + + def occluding(self, + obj: Object, + camera_pose: Pose, + front_facing_axis: Optional[List[float]] = None) -> List[Object]: + """ + Lists all objects which are occluding the given object. This works similar to 'visible'. + First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. + After that the complete scene will be rendered and the previous saved pixel positions will be compared to the + actual pixels, if in one pixel another object is visible ot will be saved as occluding. + + :param obj: The object for which occlusion should be checked + :param camera_pose: The pose of the camera in world coordinate frame + :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :return: A list of occluding objects + """ + + with UseProspectionWorld(): + state_id = self.world.save_state() + for other_obj in self.world.objects: + if other_obj.name == self.world.robot.name: + continue + elif obj.get_pose() == other_obj.get_pose(): + obj = other_obj + else: + obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) + + seg_mask, target_point = self.get_visible_objects(camera_pose, front_facing_axis) + + # All indices where the object that could be occluded is in the image + # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension + pix = np.dstack((seg_mask == obj.id).nonzero())[0] + + self.world.restore_state(state_id) + + occluding = [] + seg_mask = self.world.get_images_for_target(target_point, camera_pose)[2] + for c in pix: + if not seg_mask[c[0]][c[1]] == obj.id: + occluding.append(seg_mask[c[0]][c[1]]) + + occ_objects = list(set(map(self.world.get_object_by_id, occluding))) + occ_objects = list(map(self.world.get_object_from_prospection_object, occ_objects)) + + return occ_objects + + def reachable(self, + pose_or_object: Union[Object, Pose], + robot: Object, + gripper_name: str, + threshold: float = 0.01) -> bool: + """ + Checks if the robot can reach a given position. To determine this the inverse kinematics are + calculated and applied. Afterward the distance between the position and the given end effector is calculated, if + it is smaller than the threshold the reasoning query returns True, if not it returns False. + + :param pose_or_object: The position and rotation or Object for which reachability should be checked or an Object + :param robot: The robot that should reach for the position + :param gripper_name: The name of the end effector + :param threshold: The threshold between the end effector and the position. + :return: True if the end effector is closer than the threshold to the target position, False in every other case + """ + + prospection_robot = self.world.get_prospection_object_from_object(robot) + with UseProspectionWorld(): + target_pose = _try_to_reach_or_grasp(pose_or_object, robot, gripper_name) + + if not target_pose: + return False + + diff = target_pose.dist(prospection_robot.links[gripper_name].pose) + + return diff < threshold + + def blocking(self, + pose_or_object: Union[Object, Pose], + robot: Object, + gripper_name: str, + grasp: str = None) -> Union[List[Object], None]: + """ + Checks if any objects are blocking another object when a robot tries to pick it. This works + similar to the reachable predicate. First the inverse kinematics between the robot and the object will be + calculated and applied. Then it will be checked if the robot is in contact with any object except the given one. + If the given pose or Object is not reachable None will be returned + + :param pose_or_object: The object or pose for which blocking objects should be found + :param robot: The robot Object who reaches for the object + :param gripper_name: The name of the end effector of the robot + :param grasp: The grasp type with which the object should be grasped + :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose + or object is not reachable. + """ + prospection_robot = self.world.get_prospection_object_from_object(robot) + with UseProspectionWorld(): + _try_to_reach_or_grasp(pose_or_object, robot, gripper_name, grasp) + + block = [] + for obj in self.world.objects: + if self.contact(prospection_robot, obj): + block.append(self.world.get_object_from_prospection_object(obj)) + return block + + def supporting(self, + object1: Object, + object2: Object) -> bool: + """ + Checks if one object is supporting another object. An object supports another object if they are in + contact and the second object is above the first one. (e.g. a Bottle will be supported by a table) + + :param object1: Object that is supported + :param object2: Object that supports the first object + :return: True if the second object is in contact with the first one and the second one ist above the first False else + """ + return self.contact(object1, object2) and object2.get_position().z > object1.get_position().z + + def link_pose_for_joint_config(self, obj: Object, joint_config: Dict[str, float], link_name: str) -> Pose: + """ + Returns the pose a link would be in if the given joint configuration would be applied to the object. + This is done by using the respective object in the prospection world and applying the joint configuration + to this one. After applying the joint configuration the link position is taken from there. + + :param obj: Object of which the link is a part + :param joint_config: Dict with the goal joint configuration + :param link_name: Name of the link for which the pose should be returned + :return: The pose of the link after applying the joint configuration + """ + prospection_object = self.world.get_prospection_object_from_object(obj) + with UseProspectionWorld(): + for joint, pose in joint_config.items(): + prospection_object.set_joint_position(joint, pose) + return prospection_object.links[link_name].pose diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index d847957bc..52ab7c386 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -1,6 +1,5 @@ import unittest -import numpy as np from pycram.bullet_world import BulletWorld from pycram.world import Object, fix_missing_inertial from pycram.pose import Pose @@ -9,7 +8,6 @@ from pycram.enums import ObjectType import os import xml.etree.ElementTree as ET -import tf class BulletWorldTest(unittest.TestCase): diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index 2fc1055cd..b7463873f 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -1,7 +1,8 @@ import time from test_bullet_world import BulletWorldTest -import pycram.world_reasoning as btr +from pycram.world_reasoning import WorldReasoning +import pycram.old_world_reasoning as btr from pycram.pose import Pose from pycram.robot_descriptions import robot_description @@ -9,12 +10,14 @@ class TestBulletWorldReasoning(BulletWorldTest): def test_contact(self): + # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([1, 1, 1])) self.cereal.set_pose(Pose([1, 1, 1])) time.sleep(1) self.assertTrue(btr.contact(self.milk, self.cereal)) def test_visible(self): + # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([1.5, 0, 1.2])) self.robot.set_pose(Pose()) time.sleep(1) @@ -22,22 +25,26 @@ def test_visible(self): robot_description.front_facing_axis)) def test_occluding(self): + # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([3, 0, 1.2])) self.robot.set_pose(Pose()) self.assertTrue(btr.occluding(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, robot_description.front_facing_axis) != []) def test_reachable(self): + # btr = WorldReasoning(self.world) self.robot.set_pose(Pose()) self.assertTrue(btr.reachable(Pose([0.5, -0.7, 1]), self.robot, robot_description.get_tool_frame("right"))) self.assertFalse(btr.reachable(Pose([2, 2, 1]), self.robot, robot_description.get_tool_frame("right"))) def test_blocking(self): + # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([0.5, -0.7, 1])) self.robot.set_pose(Pose()) time.sleep(1) self.assertTrue(btr.blocking(Pose([0.5, -0.7, 1]), self.robot, robot_description.get_tool_frame("right")) != []) def test_supporting(self): + # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([1.3, 0, 0.9])) self.assertTrue(btr.supporting(self.kitchen, self.milk)) From 371d72c01fa4bbef5fb5bf6b920b4a686483e28c Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jan 2024 00:24:39 +0100 Subject: [PATCH 30/69] [AbstractWorld] WorldReasoning is now abstracted from pybullet tests are passing created two methods _try_to_reach, try_to_reach_with_grasp in the ik.py --- src/pycram/bullet_world.py | 90 ++++--- src/pycram/external_interfaces/ik.py | 60 ++++- src/pycram/helper.py | 6 +- src/pycram/old_world_reasoning.py | 365 --------------------------- src/pycram/world.py | 40 ++- src/pycram/world_reasoning.py | 104 ++------ test/test_bullet_world_reasoning.py | 23 +- 7 files changed, 194 insertions(+), 494 deletions(-) delete mode 100644 src/pycram/old_world_reasoning.py diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 137764fc0..e5d341959 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -96,6 +96,24 @@ def remove_constraint(self, constraint_id): print("Removing constraint with id: ", constraint_id) p.removeConstraint(constraint_id, physicsClientId=self.client_id) + def get_object_joint_rest_pose(self, obj: Object, joint_name: str) -> float: + """ + Get the joint rest pose of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + """ + return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[0] + + def get_object_joint_damping(self, obj: Object, joint_name: str) -> float: + """ + Get the joint damping of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + """ + return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[6] + def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: """ Get the joint upper limit of an articulated object @@ -124,7 +142,7 @@ def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: :param joint_name: Name of the joint for which the axis should be returned. :return: The axis a vector of xyz """ - return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], self.client_id)[13] + return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[13] def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: """ @@ -134,7 +152,7 @@ def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: :param joint_name: Joint name for which the type should be returned :return: The type of the joint """ - joint_type = p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], self.client_id)[2] + joint_type = p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[2] return JointType(joint_type) def get_object_joint_position(self, obj: Object, joint_name: str) -> float: @@ -183,14 +201,14 @@ def get_object_joint_names(self, obj_id: int) -> List[str]: Get the names of all joints of an articulated object. """ num_joints = self.get_object_number_of_joints(obj_id) - return [p.getJointInfo(obj_id, i, self.client_id)[1].decode('utf-8') for i in range(num_joints)] + return [p.getJointInfo(obj_id, i, physicsClientId=self.client_id)[1].decode('utf-8') for i in range(num_joints)] def get_object_link_names(self, obj_id: int) -> List[str]: """ Get the names of all joints of an articulated object. """ num_links = self.get_object_number_of_links(obj_id) - return [p.getJointInfo(obj_id, i, self.client_id)[12].decode('utf-8') for i in range(num_links)] + return [p.getJointInfo(obj_id, i, physicsClientId=self.client_id)[12].decode('utf-8') for i in range(num_links)] def get_object_number_of_links(self, obj_id: int) -> int: """ @@ -227,13 +245,13 @@ def reset_object_base_pose(self, obj: Object, position: List[float], orientation :param position: The new position of the object as a vector of x,y,z :param orientation: The new orientation of the object as a quaternion of x,y,z,w """ - p.resetBasePositionAndOrientation(obj.id, position, orientation, self.client_id) + p.resetBasePositionAndOrientation(obj.id, position, orientation, physicsClientId=self.client_id) def step(self): """ Step the world simulation using forward dynamics """ - p.stepSimulation(self.client_id) + p.stepSimulation(physicsClientId=self.client_id) def set_object_link_color(self, obj: Object, link_id: int, rgba_color: Color): """ @@ -278,7 +296,7 @@ def get_object_link_aabb(self, obj_id: int, link_id: int) -> AxisAlignedBounding Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. """ - return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj_id, link_id, self.client_id)) + return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj_id, link_id, physicsClientId=self.client_id)) def set_realtime(self, real_time: bool) -> None: """ @@ -287,7 +305,7 @@ def set_realtime(self, real_time: bool) -> None: :param real_time: Whether the World should simulate Physics in real time. """ - p.setRealTimeSimulation(1 if real_time else 0, self.client_id) + p.setRealTimeSimulation(1 if real_time else 0, physicsClientId=self.client_id) def set_gravity(self, gravity_vector: List[float]) -> None: """ @@ -309,7 +327,7 @@ def disconnect_from_physics_server(self): """ Disconnects the world from the physics server. """ - p.disconnect(self.client_id) + p.disconnect(physicsClientId=self.client_id) def join_threads(self): """ @@ -325,7 +343,7 @@ def save_physics_simulator_state(self) -> int: """ Saves the state of the physics simulator and returns the unique id of the state. """ - return p.saveState(self.client_id) + return p.saveState(physicsClientId=self.client_id) def restore_physics_simulator_state(self, state_id): """ @@ -353,11 +371,14 @@ def add_vis_axis(self, pose: Pose, position, orientation = pose.to_list() vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], - rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01]) + rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01], + physicsClientId=self.client_id) vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], - rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01]) + rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01], + physicsClientId=self.client_id) vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], - rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length]) + rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length], + physicsClientId=self.client_id) obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], basePosition=position, baseOrientation=orientation, @@ -368,7 +389,8 @@ def add_vis_axis(self, pose: Pose, linkParentIndices=[0, 0, 0], linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], - linkCollisionShapeIndices=[-1, -1, -1]) + linkCollisionShapeIndices=[-1, -1, -1], + physicsClientId=self.client_id) self.vis_axis.append(obj) @@ -377,7 +399,7 @@ def remove_vis_axis(self) -> None: Removes all spawned vis axis objects that are currently in this BulletWorld. """ for id in self.vis_axis: - p.removeBody(id) + p.removeBody(id, physicsClientId=self.client_id) self.vis_axis = [] def get_images_for_target(self, @@ -405,8 +427,9 @@ def get_images_for_target(self, near = 0.2 far = 100 - view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1]) - projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) + view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1], + physicsClientId=self.client_id) + projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far, physicsClientId=self.client_id) return list(p.getCameraImage(size, size, view_matrix, projection_matrix, physicsClientId=self.client_id))[2:5] @@ -434,21 +457,23 @@ def run(self): self.world.client_id = p.connect(p.GUI) # Disable the side windows of the GUI - p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0, physicsClientId=self.world.client_id) # Change the init camera pose p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, - cameraTargetPosition=[-2, 0, 1]) + cameraTargetPosition=[-2, 0, 1], physicsClientId=self.world.client_id) # Get the initial camera target location - cameraTargetPosition = p.getDebugVisualizerCamera()[11] + cameraTargetPosition = p.getDebugVisualizerCamera(physicsClientId=self.world.client_id)[11] - sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1]) + sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1], + physicsClientId=self.world.client_id) # Create a sphere with a radius of 0.05 and a mass of 0 sphereUid = p.createMultiBody(baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseVisualShapeIndex=sphereVisualId, - basePosition=cameraTargetPosition) + basePosition=cameraTargetPosition, + physicsClientId=self.world.client_id) # Define the maxSpeed, used in calculations maxSpeed = 16 @@ -467,17 +492,18 @@ def run(self): # Loop to update the camera position based on keyboard events while p.isConnected(self.world.client_id): # Monitor user input - keys = p.getKeyboardEvents() - mouse = p.getMouseEvents() + keys = p.getKeyboardEvents(physicalClientId=self.world.client_id) + mouse = p.getMouseEvents(physicalClientId=self.world.client_id) # Get infos about the camera - width, height, dist = p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], \ - p.getDebugVisualizerCamera()[10] - cameraTargetPosition = p.getDebugVisualizerCamera()[11] + width, height, dist = (p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[0], + p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[1], + p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[10]) + cameraTargetPosition = p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[11] # Get vectors used for movement on x,y,z Vector - xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] - yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] + xVec = [p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[2][i] for i in [0, 4, 8]] + yVec = [p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[2][i] for i in [2, 6, 10]] zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] # Check the mouse state @@ -617,8 +643,10 @@ def run(self): dist += dist * 0.02 * speedMult p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, - cameraTargetPosition=cameraTargetPosition) + cameraTargetPosition=cameraTargetPosition, + physicsClientId=self.world.client_id) if visible == 0: cameraTargetPosition = (0.0, -50, 50) - p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) + p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1], + physicsClientId=self.world.client_id) time.sleep(1. / 80.) \ No newline at end of file diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 664d5133d..262a3c063 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -1,4 +1,4 @@ -from typing import List +from typing import List, Union import pybullet as p import rospy @@ -8,7 +8,7 @@ from sensor_msgs.msg import JointState from ..world import Object -from ..helper import calculate_wrist_tool_offset +from ..helper import calculate_wrist_tool_offset, _apply_ik from ..local_transformer import LocalTransformer from ..pose import Pose, Transform from ..robot_descriptions import robot_description @@ -106,6 +106,62 @@ def call_ik(root_link: str, tip_link: str, target_pose: Pose, robot_object: Obje return resp.solution.joint_state.position +def try_to_reach_with_grasp(pose_or_object: Union[Pose, Object], + prospection_robot: Object, gripper_name: str, + grasp: str) -> Union[Pose, None]: + """ + Checks if the robot can reach a given position with a specific grasp orientation. + To determine this the inverse kinematics are calculated and applied. + + :param pose_or_object: The position and rotation or Object for which reachability should be checked or an Object + :param prospection_robot: The robot that should reach for the position + :param gripper_name: The name of the end effector + :param grasp: The grasp type with which the object should be grasped + """ + + input_pose = pose_or_object.get_pose() if isinstance(pose_or_object, Object) else pose_or_object + + target_pose = apply_grasp_orientation_to_pose(grasp, input_pose) + + return try_to_reach(target_pose, prospection_robot, gripper_name) + + +def apply_grasp_orientation_to_pose(grasp: str, pose: Pose) -> Pose: + """ + Applies the orientation of a grasp to a given pose. This is done by using the grasp orientation + of the given grasp and applying it to the given pose. + + :param grasp: The name of the grasp + :param pose: The pose to which the grasp orientation should be applied + """ + local_transformer = LocalTransformer() + target_map = local_transformer.transform_pose_to_target_frame(pose, "map") + grasp_orientation = robot_description.grasps.get_orientation_for_grasp(grasp) + target_map.orientation.x = grasp_orientation[0] + target_map.orientation.y = grasp_orientation[1] + target_map.orientation.z = grasp_orientation[2] + target_map.orientation.w = grasp_orientation[3] + return target_map + + +def try_to_reach(pose_or_object: Union[Pose, Object], prospection_robot: Object, + gripper_name: str) -> Union[Pose, None]: + + input_pose = pose_or_object.get_pose() if isinstance(pose_or_object, Object) else pose_or_object + + arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" + joints = robot_description.chains[arm].joints + + try: + inv = request_ik(input_pose, prospection_robot, joints, gripper_name) + except IKError as e: + rospy.logerr(f"Pose is not reachable: {e}") + return None + _apply_ik(prospection_robot, inv, joints) + + return input_pose + + def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str) -> List[float]: """ Top-level method to request ik solution for a given pose. Before calling the ik service the links directly before diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 090ac17ff..f2ca1ace5 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -15,7 +15,7 @@ from pytransform3d.transformations import transform_from_pq, transform_from, pq_from_transform from macropy.core.quotes import ast_literal, q -from .world import Object as BulletWorldObject +from .world import Object as WorldObject from .pose import Transform, Pose import math @@ -39,7 +39,7 @@ class bcolors: UNDERLINE = '\033[4m' -def _apply_ik(robot: BulletWorldObject, joint_poses: List[float], joints: List[str]) -> None: +def _apply_ik(robot: WorldObject, joint_poses: List[float], joints: List[str]) -> None: """ Apllies a list of joint poses calculated by an inverse kinematics solver to a robot @@ -56,7 +56,7 @@ def _apply_ik(robot: BulletWorldObject, joint_poses: List[float], joints: List[s # robot.set_joint_state(joints[i], joint_poses[i]) -def calculate_wrist_tool_offset(wrist_frame: str, tool_frame: str, robot: BulletWorldObject) -> Transform: +def calculate_wrist_tool_offset(wrist_frame: str, tool_frame: str, robot: WorldObject) -> Transform: return robot.links[wrist_frame].get_transform_to_link(robot.links[tool_frame]) diff --git a/src/pycram/old_world_reasoning.py b/src/pycram/old_world_reasoning.py deleted file mode 100644 index 452cce72c..000000000 --- a/src/pycram/old_world_reasoning.py +++ /dev/null @@ -1,365 +0,0 @@ -import pybullet as p -import itertools -import numpy as np -import rospy - -from .world import _world_and_id, Object, UseProspectionWorld -from .bullet_world import BulletWorld -from .external_interfaces.ik import request_ik -from .local_transformer import LocalTransformer -from .plan_failures import IKError -from .robot_descriptions import robot_description -from .helper import _apply_ik -from .pose import Pose, Transform -from typing import List, Tuple, Optional, Union, Dict - - -class ReasoningError(Exception): - def __init__(self, *args, **kwargs): - Exception.__init__(self, *args, **kwargs) - - -class CollisionError(Exception): - def __init__(self, *args, **kwargs): - Exception.__init__(self, *args, **kwargs) - - -def _get_joint_names(robot: Object, tip_link: str) -> List[str]: - res = [] - for i in range(p.getNumJoints(robot.id)): - info = p.getJointInfo(robot.id, i) - if info[2] != p.JOINT_FIXED: - res.append(info[1]) - return res - - -def _get_images_for_target(target_pose: Pose, - cam_pose: Pose, - world: Optional[BulletWorld] = None, - size: Optional[int] = 256) -> List[np.ndarray]: - """ - Calculates the view and projection Matrix and returns 3 images: - - 1. An RGB image - 2. A depth image - 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object. - - From the given target_pose and cam_pose only the position is used. - - :param cam_pose: The pose of the camera - :param target_pose: The pose to which the camera should point to - :param size: The height and width of the images in pixel - :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. - """ - world, world_id = _world_and_id(world) - # TODO: Might depend on robot cameras, if so please add these camera parameters to RobotDescription object - # TODO: of your robot with a CameraDescription object. - fov = 90 - aspect = size / size - near = 0.2 - far = 100 - - view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1]) - projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) - return list(p.getCameraImage(size, size, view_matrix, projection_matrix, physicsClientId=world_id))[2:5] - - -def _get_joint_ranges(robot: Object) -> Tuple[List, List, List, List, List]: - """ - Calculates the lower and upper limits, the joint ranges and the joint damping. For a given robot Object. - Fixed joints will be skipped because they don't have limits or ranges. - - :param robot: The robot for whom the values should be calculated - :return: The lists for the upper and lower limits, joint ranges, rest poses and joint damping - """ - ll, ul, jr, rp, jd = [], [], [], [], [] - - for i in range(0, p.getNumJoints(robot.id)): - info = p.getJointInfo(robot.id, i) - if info[3] > -1: - ll.append(info[8]) - ul.append(info[9]) - jr.append(info[9] - info[8]) - rp.append(p.getJointState(robot.id, i)[0]) - jd.append(info[6]) - - return ll, ul, jr, rp, jd - - -def stable(object: Object, - world: Optional[BulletWorld] = None) -> bool: - """ - Checks if an object is stable in the world. Stable meaning that it's position will not change after simulating physics - in the BulletWorld. This will be done by simulating the world for 10 seconds and compare the previous coordinates - with the coordinates after the simulation. - - :param object: The object which should be checked - :param world: The BulletWorld if more than one BulletWorld is active - :return: True if the given object is stable in the world False else - """ - world, world_id = _world_and_id(world) - shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) - with UseProspectionWorld(): - coords_prev = shadow_obj.get_position_as_list() - state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) - p.setGravity(0, 0, -9.8, BulletWorld.current_world.client_id) - - # one Step is approximately 1/240 seconds - BulletWorld.current_world.simulate(2) - # coords_past = p.getBasePositionAndOrientation(object.id, physicsClientId=world_id)[0] - coords_past = shadow_obj.get_position_as_list() - - # p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) - coords_prev = list(map(lambda n: round(n, 3), coords_prev)) - coords_past = list(map(lambda n: round(n, 3), coords_past)) - return coords_past == coords_prev - - -def contact(object1: Object, - object2: Object, - return_links: bool = False) -> Union[bool, Tuple[bool, List]]: - """ - Checks if two objects are in contact or not. If the links should be returned then the output will also contain a - list of tuples where the first element is the link name of 'object1' and the second element is the link name of - 'object2'. - - :param object1: The first object - :param object2: The second object - :param return_links: If the respective links on the objects that are in contact should be returned. - :return: True if the two objects are in contact False else. If links should be returned a list of links in contact - """ - - with UseProspectionWorld(): - shadow_obj1 = BulletWorld.current_world.get_prospection_object_from_object(object1) - shadow_obj2 = BulletWorld.current_world.get_prospection_object_from_object(object2) - p.performCollisionDetection(BulletWorld.current_world.client_id) - con_points = p.getContactPoints(shadow_obj1.id, shadow_obj2.id, - physicsClientId=BulletWorld.current_world.client_id) - - if return_links: - contact_links = [] - for point in con_points: - contact_links.append((shadow_obj1.get_link_by_id(point[3]), shadow_obj2.get_link_by_id(point[4]))) - return con_points != (), contact_links - - else: - return con_points != () - - -def visible(object: Object, - camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None, - threshold: float = 0.8, - world: Optional[BulletWorld] = None) -> bool: - """ - Checks if an object is visible from a given position. This will be achieved by rendering the object - alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the - absolut count of pixels. - - :param object: The object for which the visibility should be checked - :param camera_pose: The pose of the camera in map frame - :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz - :param threshold: The minimum percentage of the object that needs to be visible for this method to return true - :param world: The BulletWorld if more than one BulletWorld is active - :return: True if the object is visible from the camera_position False if not - """ - front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis - with UseProspectionWorld(): - shadow_obj = BulletWorld.current_world.get_prospection_object_from_object(object) - if BulletWorld.robot: - shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(BulletWorld.robot) - state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) - for obj in BulletWorld.current_world.objects: - if obj == shadow_obj or BulletWorld.robot and obj == shadow_robot: - continue - else: - obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - - world_to_cam = camera_pose.to_transform("camera") - cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", "point") - target_point = (world_to_cam * cam_to_point).to_pose() - # target_point = p.multiplyTransforms(world_to_cam.translation_as_list(), world_to_cam.rotation_as_list(), cam_to_point.translation_as_list(), [0, 0, 0, 1]) - # print(target_point) - - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] - flat_list = list(itertools.chain.from_iterable(seg_mask)) - max_pixel = sum(list(map(lambda x: 1 if x == shadow_obj.id else 0, flat_list))) - p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) - if max_pixel == 0: - # Object is not visible - return False - - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] - flat_list = list(itertools.chain.from_iterable(seg_mask)) - real_pixel = sum(list(map(lambda x: 1 if x == shadow_obj.id else 0, flat_list))) - - return real_pixel / max_pixel > threshold > 0 - - -def occluding(object: Object, - camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None, - world: Optional[BulletWorld] = None) -> List[Object]: - """ - Lists all objects which are occluding the given object. This works similar to 'visible'. - First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. - After that the complete scene will be rendered and the previous saved pixel positions will be compared to the - actual pixels, if in one pixel another object is visible ot will be saved as occluding. - - :param object: The object for which occlusion should be checked - :param camera_pose: The pose of the camera in world coordinate frame - :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz - :param world: The BulletWorld if more than one BulletWorld is active - :return: A list of occluding objects - """ - front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis - world, world_id = _world_and_id(world) - # occ_world = world.copy() - # state = p.saveState(physicsClientId=occ_world.client_id) - with UseProspectionWorld(): - state = p.saveState(physicsClientId=BulletWorld.current_world.client_id) - for obj in BulletWorld.current_world.objects: - if obj.name == BulletWorld.robot.name: - continue - elif object.get_pose() == obj.get_pose(): - object = obj - else: - obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - - world_to_cam = camera_pose.to_transform("camera") - cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", "point") - target_point = (world_to_cam * cam_to_point).to_pose() - - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] - - # All indices where the object that could be occluded is in the image - # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension - pix = np.dstack((seg_mask == object.id).nonzero())[0] - - p.restoreState(state, physicsClientId=BulletWorld.current_world.client_id) - - occluding = [] - seg_mask = _get_images_for_target(target_point, world_to_cam.to_pose(), BulletWorld.current_world)[2] - for c in pix: - if not seg_mask[c[0]][c[1]] == object.id: - occluding.append(seg_mask[c[0]][c[1]]) - - occ_objects = list(set(map(BulletWorld.current_world.get_object_by_id, occluding))) - occ_objects = list(map(world.get_object_from_prospection_object, occ_objects)) - - return occ_objects - - -def reachable(pose: Union[Object, Pose], - robot: Object, - gripper_name: str, - threshold: float = 0.01) -> bool: - """ - Checks if the robot can reach a given position. To determine this the inverse kinematics are - calculated and applied. Afterward the distance between the position and the given end effector is calculated, if - it is smaller than the threshold the reasoning query returns True, if not it returns False. - - :param pose: The position and rotation or Object for which reachability should be checked or an Object - :param robot: The robot that should reach for the position - :param gripper_name: The name of the end effector - :param threshold: The threshold between the end effector and the position. - :return: True if the end effector is closer than the threshold to the target position, False in every other case - """ - if type(pose) == Object: - pose = pose.get_pose() - - shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(robot) - with UseProspectionWorld(): - arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" - joints = robot_description.chains[arm].joints - try: - inv = request_ik(pose, shadow_robot, joints, gripper_name) - except IKError as e: - return False - - _apply_ik(shadow_robot, inv, joints) - - diff = pose.dist(shadow_robot.links[gripper_name].pose) - - return diff < threshold - - -def blocking(pose_or_object: Union[Object, Pose], - robot: Object, - gripper_name: str, - grasp: str = None) -> Union[List[Object], None]: - """ - Checks if any objects are blocking another object when a robot tries to pick it. This works - similar to the reachable predicate. First the inverse kinematics between the robot and the object will be calculated - and applied. Then it will be checked if the robot is in contact with any object except the given one. If the given - pose or Object is not reachable None will be returned - - :param pose_or_object: The object or pose for which blocking objects should be found - :param robot: The robot Object who reaches for the object - :param gripper_name: The name of the end effector of the robot - :param grasp: The grasp type with which the object should be grasped - :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose - or object is not reachable. - """ - if type(pose_or_object) == Object: - input_pose = pose_or_object.get_pose() - else: - input_pose = pose_or_object - - shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(robot) - with UseProspectionWorld(): - arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" - joints = robot_description.chains[arm].joints - local_transformer = LocalTransformer() - - target_map = local_transformer.transform_pose_to_target_frame(input_pose, "map") - if grasp: - grasp_orientation = robot_description.grasps.get_orientation_for_grasp(grasp) - target_map.orientation.x = grasp_orientation[0] - target_map.orientation.y = grasp_orientation[1] - target_map.orientation.z = grasp_orientation[2] - target_map.orientation.w = grasp_orientation[3] - - try: - inv = request_ik(target_map, shadow_robot, joints, gripper_name) - except IKError as e: - rospy.logerr(f"Pose is not reachable: {e}") - return None - _apply_ik(shadow_robot, inv, joints) - - block = [] - for obj in BulletWorld.current_world.objects: - if contact(shadow_robot, obj): - block.append(BulletWorld.current_world.get_object_from_prospection_object(obj)) - return block - - -def supporting(object1: Object, - object2: Object) -> bool: - """ - Checks if one object is supporting another object. An object supports another object if they are in - contact and the second object is above the first one. (e.g. a Bottle will be supported by a table) - - :param object1: Object that is supported - :param object2: Object that supports the first object - :return: True if the second object is in contact with the first one and the second one ist above the first False else - """ - return contact(object1, object2) and object2.get_position().z > object1.get_position().z - - -def link_pose_for_joint_config(object: Object, joint_config: Dict[str, float], link_name: str) -> Pose: - """ - Returns the pose a link would be in if the given joint configuration would be applied to the object. This is done - by using the respective object in the shadow world and applying the joint configuration to this one. After applying - the joint configuration the link position is taken from there. - - :param object: Object of which the link is a part - :param joint_config: Dict with the goal joint configuration - :param link_name: Name of the link for which the pose should be returned - :return: The pose of the link after applying the joint configuration - """ - shadow_object = BulletWorld.current_world.get_prospection_object_from_object(object) - with UseProspectionWorld(): - for joint, pose in joint_config.items(): - shadow_object.set_joint_position(joint, pose) - return shadow_object.links[link_name].pose \ No newline at end of file diff --git a/src/pycram/world.py b/src/pycram/world.py index fa5b50d3d..5a89c4b63 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -325,6 +325,44 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ pass + def get_joint_ranges(self) -> Tuple[List, List, List, List, List]: + """ + Calculates the lower and upper limits, the joint ranges and the joint damping. For a given robot Object. + Fixed joints will be skipped because they don't have limits or ranges. + + :return: The lists for the upper and lower limits, joint ranges, rest poses and joint damping + """ + ll, ul, jr, rp, jd = [], [], [], [], [] + joint_names = self.get_object_joint_names(self.robot.id) + for name in joint_names: + joint_type = self.get_object_joint_type(self.robot, name) + if joint_type != JointType.FIXED: + ll.append(self.get_object_joint_lower_limit(self.robot, name)) + ul.append(self.get_object_joint_upper_limit(self.robot, name)) + jr.append(ul[-1] - ll[-1]) + rp.append(self.get_object_joint_rest_pose(self.robot, name)) + jd.append(self.get_object_joint_damping(self.robot, name)) + + return ll, ul, jr, rp, jd + + def get_object_joint_rest_pose(self, obj: Object, joint_name: str) -> float: + """ + Get the rest pose of a joint of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + """ + pass + + def get_object_joint_damping(self, obj: Object, joint_name: str) -> float: + """ + Get the damping of a joint of an articulated object + + :param obj: The object + :param joint_name: The name of the joint + """ + pass + @abstractmethod def get_object_joint_names(self, obj_id: int) -> List[str]: """ @@ -716,7 +754,7 @@ def get_prospection_object_from_object(self, obj: Object) -> Object: def get_object_from_prospection_object(self, prospection_object: Object) -> Object: """ Returns the corresponding object from the main World for a given - object in the shadow world. If the given object is not in the shadow + object in the prospection world. If the given object is not in the prospection world an error will be raised. :param prospection_object: The object for which the corresponding object in the main World should be found. diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index d992fd0a0..32f294fa3 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -1,16 +1,11 @@ -import pybullet as p import itertools import numpy as np -import rospy -from .world import _world_and_id, Object, UseProspectionWorld +from .world import Object, UseProspectionWorld from .world import World -from .external_interfaces.ik import request_ik -from .local_transformer import LocalTransformer -from .plan_failures import IKError from .robot_descriptions import robot_description -from .helper import _apply_ik from .pose import Pose, Transform +from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from typing import List, Tuple, Optional, Union, Dict @@ -24,67 +19,6 @@ def __init__(self, *args, **kwargs): Exception.__init__(self, *args, **kwargs) -def _get_joint_ranges(robot: Object) -> Tuple[List, List, List, List, List]: - """ - Calculates the lower and upper limits, the joint ranges and the joint damping. For a given robot Object. - Fixed joints will be skipped because they don't have limits or ranges. - - :param robot: The robot for whom the values should be calculated - :return: The lists for the upper and lower limits, joint ranges, rest poses and joint damping - """ - ll, ul, jr, rp, jd = [], [], [], [], [] - - for i in range(0, p.getNumJoints(robot.id)): - info = p.getJointInfo(robot.id, i) - if info[3] > -1: - ll.append(info[8]) - ul.append(info[9]) - jr.append(info[9] - info[8]) - rp.append(p.getJointState(robot.id, i)[0]) - jd.append(info[6]) - - return ll, ul, jr, rp, jd - - -def _try_to_reach_or_grasp(pose_or_object: Union[Pose, Object], prospection_robot: Object, gripper_name: str, - grasp: Optional[str] = None) -> Union[Pose, None]: - """ - Checks if the robot can reach a given position and optionally also grasp an object. - To determine this the inverse kinematics are calculated and applied. - - :param pose_or_object: The position and rotation or Object for which reachability should be checked or an Object - :param prospection_robot: The robot that should reach for the position - :param gripper_name: The name of the end effector - :param grasp: The grasp type with which the object should be grasped - """ - if isinstance(pose_or_object, Object): - input_pose = pose_or_object.get_pose() - else: - input_pose = pose_or_object - - arm = "left" if gripper_name == robot_description.get_tool_frame("left") else "right" - joints = robot_description.chains[arm].joints - target_pose = input_pose - if grasp: - local_transformer = LocalTransformer() - target_map = local_transformer.transform_pose_to_target_frame(input_pose, "map") - grasp_orientation = robot_description.grasps.get_orientation_for_grasp(grasp) - target_map.orientation.x = grasp_orientation[0] - target_map.orientation.y = grasp_orientation[1] - target_map.orientation.z = grasp_orientation[2] - target_map.orientation.w = grasp_orientation[3] - target_pose = target_map - - try: - inv = request_ik(target_pose, prospection_robot, joints, gripper_name) - except IKError as e: - rospy.logerr(f"Pose is not reachable: {e}") - return None - _apply_ik(prospection_robot, inv, joints) - - return target_pose - - class WorldReasoning: def __init__(self, world: World): @@ -130,13 +64,14 @@ def contact(self, prospection_obj1 = self.world.get_prospection_object_from_object(object1) prospection_obj2 = self.world.get_prospection_object_from_object(object2) - self.world.perform_collision_detection() - con_points = self.world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) + World.current_world.perform_collision_detection() + con_points = World.current_world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) if return_links: contact_links = [] for point in con_points: - contact_links.append((prospection_obj1.get_link_by_id(point[3]), prospection_obj2.get_link_by_id(point[4]))) + contact_links.append((prospection_obj1.get_link_by_id(point[3]), + prospection_obj2.get_link_by_id(point[4]))) return con_points != (), contact_links else: @@ -153,7 +88,7 @@ def get_visible_objects(self, camera_pose: Pose, "point") target_point = (world_to_cam * cam_to_point).to_pose() - seg_mask = self.world.get_images_for_target(target_point, world_to_cam.to_pose())[2] + seg_mask = self.world.get_images_for_target(target_point, camera_pose)[2] return seg_mask, target_point @@ -178,8 +113,8 @@ def visible(self, if World.robot: prospection_robot = self.world.get_prospection_object_from_object(World.robot) - state_id = self.world.save_state() - for obj in self.world.objects: + state_id = World.current_world.save_state() + for obj in World.current_world.objects: if obj == prospection_obj or World.robot and obj == prospection_robot: continue else: @@ -188,13 +123,13 @@ def visible(self, seg_mask, target_point = self.get_visible_objects(camera_pose, front_facing_axis) flat_list = list(itertools.chain.from_iterable(seg_mask)) max_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) - self.world.restore_state(state_id) + World.current_world.restore_state(state_id) if max_pixel == 0: # Object is not visible return False - seg_mask = self.world.get_images_for_target(target_point, camera_pose)[2] + seg_mask = World.current_world.get_images_for_target(target_point, camera_pose)[2] flat_list = list(itertools.chain.from_iterable(seg_mask)) real_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) @@ -224,7 +159,7 @@ def occluding(self, elif obj.get_pose() == other_obj.get_pose(): obj = other_obj else: - obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) + other_obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) seg_mask, target_point = self.get_visible_objects(camera_pose, front_facing_axis) @@ -240,7 +175,7 @@ def occluding(self, if not seg_mask[c[0]][c[1]] == obj.id: occluding.append(seg_mask[c[0]][c[1]]) - occ_objects = list(set(map(self.world.get_object_by_id, occluding))) + occ_objects = list(set(map(World.current_world.get_object_by_id, occluding))) occ_objects = list(map(self.world.get_object_from_prospection_object, occ_objects)) return occ_objects @@ -264,12 +199,13 @@ def reachable(self, prospection_robot = self.world.get_prospection_object_from_object(robot) with UseProspectionWorld(): - target_pose = _try_to_reach_or_grasp(pose_or_object, robot, gripper_name) + target_pose = try_to_reach(pose_or_object, prospection_robot, gripper_name) if not target_pose: return False - diff = target_pose.dist(prospection_robot.links[gripper_name].pose) + gripper_pose = prospection_robot.links[gripper_name].pose + diff = target_pose.dist(gripper_pose) return diff < threshold @@ -291,12 +227,16 @@ def blocking(self, :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose or object is not reachable. """ + prospection_robot = self.world.get_prospection_object_from_object(robot) with UseProspectionWorld(): - _try_to_reach_or_grasp(pose_or_object, robot, gripper_name, grasp) + if grasp: + try_to_reach_with_grasp(pose_or_object, prospection_robot, gripper_name, grasp) + else: + try_to_reach(pose_or_object, prospection_robot, gripper_name) block = [] - for obj in self.world.objects: + for obj in World.current_world.objects: if self.contact(prospection_robot, obj): block.append(self.world.get_object_from_prospection_object(obj)) return block diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index b7463873f..4375ca52e 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -2,49 +2,52 @@ from test_bullet_world import BulletWorldTest from pycram.world_reasoning import WorldReasoning -import pycram.old_world_reasoning as btr from pycram.pose import Pose from pycram.robot_descriptions import robot_description +use_new = True + class TestBulletWorldReasoning(BulletWorldTest): def test_contact(self): - # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([1, 1, 1])) self.cereal.set_pose(Pose([1, 1, 1])) time.sleep(1) + btr = WorldReasoning(self.world) self.assertTrue(btr.contact(self.milk, self.cereal)) def test_visible(self): - # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([1.5, 0, 1.2])) self.robot.set_pose(Pose()) time.sleep(1) + btr = WorldReasoning(self.world) self.assertTrue(btr.visible(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, - robot_description.front_facing_axis)) + robot_description.front_facing_axis)) def test_occluding(self): - # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([3, 0, 1.2])) self.robot.set_pose(Pose()) + btr = WorldReasoning(self.world) self.assertTrue(btr.occluding(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, - robot_description.front_facing_axis) != []) + robot_description.front_facing_axis) != []) def test_reachable(self): - # btr = WorldReasoning(self.world) self.robot.set_pose(Pose()) + time.sleep(1) + btr = WorldReasoning(self.world) self.assertTrue(btr.reachable(Pose([0.5, -0.7, 1]), self.robot, robot_description.get_tool_frame("right"))) self.assertFalse(btr.reachable(Pose([2, 2, 1]), self.robot, robot_description.get_tool_frame("right"))) def test_blocking(self): - # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([0.5, -0.7, 1])) self.robot.set_pose(Pose()) time.sleep(1) - self.assertTrue(btr.blocking(Pose([0.5, -0.7, 1]), self.robot, robot_description.get_tool_frame("right")) != []) + btr = WorldReasoning(self.world) + self.assertTrue( + btr.blocking(Pose([0.5, -0.7, 1]), self.robot, robot_description.get_tool_frame("right")) != []) def test_supporting(self): - # btr = WorldReasoning(self.world) self.milk.set_pose(Pose([1.3, 0, 0.9])) + btr = WorldReasoning(self.world) self.assertTrue(btr.supporting(self.kitchen, self.milk)) From 624bb8c8ca9c520d84e660ed7378d55b9ab42fd6 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jan 2024 14:13:11 +0100 Subject: [PATCH 31/69] [WorldAbstractClass] world_reasoning.py is not a class anymore. --- src/pycram/world_reasoning.py | 525 ++++++++++++++-------------- test/test_bullet_world_reasoning.py | 10 +- 2 files changed, 268 insertions(+), 267 deletions(-) diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 32f294fa3..35fca3d44 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -1,272 +1,279 @@ import itertools +from typing import List, Tuple, Optional, Union, Dict + import numpy as np +from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp +from .pose import Pose, Transform +from .robot_descriptions import robot_description from .world import Object, UseProspectionWorld from .world import World -from .robot_descriptions import robot_description -from .pose import Pose, Transform -from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp -from typing import List, Tuple, Optional, Union, Dict class ReasoningError(Exception): - def __init__(self, *args, **kwargs): - Exception.__init__(self, *args, **kwargs) + def __init__(*args, **kwargs): + Exception.__init__(*args, **kwargs) class CollisionError(Exception): - def __init__(self, *args, **kwargs): - Exception.__init__(self, *args, **kwargs) - - -class WorldReasoning: - - def __init__(self, world: World): - self.world = world - - def stable(self, obj: Object) -> bool: - """ - Checks if an object is stable in the world. Stable meaning that it's position will not change after simulating physics - in the BulletWorld. This will be done by simulating the world for 10 seconds and compare the previous coordinates - with the coordinates after the simulation. - - :param obj: The object which should be checked - :return: True if the given object is stable in the world False else - """ - prospection_obj = self.world.get_prospection_object_from_object(obj) - with UseProspectionWorld(): - coords_prev = prospection_obj.get_position_as_list() - self.world.set_gravity([0, 0, -9.8]) - - self.world.simulate(2) - coords_past = prospection_obj.get_position_as_list() - - coords_prev = list(map(lambda n: round(n, 3), coords_prev)) - coords_past = list(map(lambda n: round(n, 3), coords_past)) - return coords_past == coords_prev - - def contact(self, - object1: Object, - object2: Object, - return_links: bool = False) -> Union[bool, Tuple[bool, List]]: - """ - Checks if two objects are in contact or not. If the links should be returned then the output will also contain a - list of tuples where the first element is the link name of 'object1' and the second element is the link name of - 'object2'. - - :param object1: The first object - :param object2: The second object - :param return_links: If the respective links on the objects that are in contact should be returned. - :return: True if the two objects are in contact False else. If links should be returned a list of links in contact - """ - - with UseProspectionWorld(): - prospection_obj1 = self.world.get_prospection_object_from_object(object1) - prospection_obj2 = self.world.get_prospection_object_from_object(object2) - - World.current_world.perform_collision_detection() - con_points = World.current_world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) - - if return_links: - contact_links = [] - for point in con_points: - contact_links.append((prospection_obj1.get_link_by_id(point[3]), - prospection_obj2.get_link_by_id(point[4]))) - return con_points != (), contact_links - + def __init__(*args, **kwargs): + Exception.__init__(*args, **kwargs) + + +def stable(obj: Object) -> bool: + """ + Checks if an object is stable in the world. Stable meaning that it's position will not change after simulating + physics in the BulletWorld. This will be done by simulating the world for 10 seconds and compare + the previous coordinates with the coordinates after the simulation. + + :param obj: The object which should be checked + :return: True if the given object is stable in the world False else + """ + prospection_obj = World.current_world.get_prospection_object_from_object(obj) + with UseProspectionWorld(): + coords_prev = prospection_obj.get_position_as_list() + World.current_world.set_gravity([0, 0, -9.8]) + + World.current_world.simulate(2) + coords_past = prospection_obj.get_position_as_list() + + coords_prev = list(map(lambda n: round(n, 3), coords_prev)) + coords_past = list(map(lambda n: round(n, 3), coords_past)) + return coords_past == coords_prev + + +def contact( + object1: Object, + object2: Object, + return_links: bool = False) -> Union[bool, Tuple[bool, List]]: + """ + Checks if two objects are in contact or not. If the links should be returned then the output will also contain a + list of tuples where the first element is the link name of 'object1' and the second element is the link name of + 'object2'. + + :param object1: The first object + :param object2: The second object + :param return_links: If the respective links on the objects that are in contact should be returned. + :return: True if the two objects are in contact False else. If links should be returned a list of links in contact + """ + + with UseProspectionWorld(): + prospection_obj1 = World.current_world.get_prospection_object_from_object(object1) + prospection_obj2 = World.current_world.get_prospection_object_from_object(object2) + + World.current_world.perform_collision_detection() + con_points = World.current_world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) + + if return_links: + contact_links = [] + for point in con_points: + contact_links.append((prospection_obj1.get_link_by_id(point[3]), + prospection_obj2.get_link_by_id(point[4]))) + return con_points != (), contact_links + + else: + return con_points != () + + +def get_visible_objects( + camera_pose: Pose, + front_facing_axis: Optional[List[float]] = None) -> Tuple[np.ndarray, Pose]: + front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis + + world_to_cam = camera_pose.to_transform("camera") + + cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", + "point") + target_point = (world_to_cam * cam_to_point).to_pose() + + seg_mask = World.current_world.get_images_for_target(target_point, camera_pose)[2] + + return seg_mask, target_point + + +def visible( + obj: Object, + camera_pose: Pose, + front_facing_axis: Optional[List[float]] = None, + threshold: float = 0.8) -> bool: + """ + Checks if an object is visible from a given position. This will be achieved by rendering the object + alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the + absolut count of pixels. + + :param obj: The object for which the visibility should be checked + :param camera_pose: The pose of the camera in map frame + :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :param threshold: The minimum percentage of the object that needs to be visible for this method to return true. + :return: True if the object is visible from the camera_position False if not + """ + with UseProspectionWorld(): + prospection_obj = World.current_world.get_prospection_object_from_object(obj) + if World.robot: + prospection_robot = World.current_world.get_prospection_object_from_object(World.robot) + + state_id = World.current_world.save_state() + for obj in World.current_world.objects: + if obj == prospection_obj or World.robot and obj == prospection_robot: + continue else: - return con_points != () - - def get_visible_objects(self, camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None) -> Tuple[np.ndarray, Pose]: - - front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis - - world_to_cam = camera_pose.to_transform("camera") - - cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", - "point") - target_point = (world_to_cam * cam_to_point).to_pose() - - seg_mask = self.world.get_images_for_target(target_point, camera_pose)[2] - - return seg_mask, target_point - - def visible(self, - obj: Object, - camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None, - threshold: float = 0.8) -> bool: - """ - Checks if an object is visible from a given position. This will be achieved by rendering the object - alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the - absolut count of pixels. - - :param obj: The object for which the visibility should be checked - :param camera_pose: The pose of the camera in map frame - :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz - :param threshold: The minimum percentage of the object that needs to be visible for this method to return true. - :return: True if the object is visible from the camera_position False if not - """ - with UseProspectionWorld(): - prospection_obj = self.world.get_prospection_object_from_object(obj) - if World.robot: - prospection_robot = self.world.get_prospection_object_from_object(World.robot) - - state_id = World.current_world.save_state() - for obj in World.current_world.objects: - if obj == prospection_obj or World.robot and obj == prospection_robot: - continue - else: - obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - - seg_mask, target_point = self.get_visible_objects(camera_pose, front_facing_axis) - flat_list = list(itertools.chain.from_iterable(seg_mask)) - max_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) - World.current_world.restore_state(state_id) - - if max_pixel == 0: - # Object is not visible - return False - - seg_mask = World.current_world.get_images_for_target(target_point, camera_pose)[2] - flat_list = list(itertools.chain.from_iterable(seg_mask)) - real_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) - - return real_pixel / max_pixel > threshold > 0 - - def occluding(self, - obj: Object, - camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None) -> List[Object]: - """ - Lists all objects which are occluding the given object. This works similar to 'visible'. - First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. - After that the complete scene will be rendered and the previous saved pixel positions will be compared to the - actual pixels, if in one pixel another object is visible ot will be saved as occluding. - - :param obj: The object for which occlusion should be checked - :param camera_pose: The pose of the camera in world coordinate frame - :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz - :return: A list of occluding objects - """ - - with UseProspectionWorld(): - state_id = self.world.save_state() - for other_obj in self.world.objects: - if other_obj.name == self.world.robot.name: - continue - elif obj.get_pose() == other_obj.get_pose(): - obj = other_obj - else: - other_obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - - seg_mask, target_point = self.get_visible_objects(camera_pose, front_facing_axis) - - # All indices where the object that could be occluded is in the image - # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension - pix = np.dstack((seg_mask == obj.id).nonzero())[0] - - self.world.restore_state(state_id) - - occluding = [] - seg_mask = self.world.get_images_for_target(target_point, camera_pose)[2] - for c in pix: - if not seg_mask[c[0]][c[1]] == obj.id: - occluding.append(seg_mask[c[0]][c[1]]) - - occ_objects = list(set(map(World.current_world.get_object_by_id, occluding))) - occ_objects = list(map(self.world.get_object_from_prospection_object, occ_objects)) - - return occ_objects - - def reachable(self, - pose_or_object: Union[Object, Pose], - robot: Object, - gripper_name: str, - threshold: float = 0.01) -> bool: - """ - Checks if the robot can reach a given position. To determine this the inverse kinematics are - calculated and applied. Afterward the distance between the position and the given end effector is calculated, if - it is smaller than the threshold the reasoning query returns True, if not it returns False. - - :param pose_or_object: The position and rotation or Object for which reachability should be checked or an Object - :param robot: The robot that should reach for the position - :param gripper_name: The name of the end effector - :param threshold: The threshold between the end effector and the position. - :return: True if the end effector is closer than the threshold to the target position, False in every other case - """ - - prospection_robot = self.world.get_prospection_object_from_object(robot) - with UseProspectionWorld(): - target_pose = try_to_reach(pose_or_object, prospection_robot, gripper_name) - - if not target_pose: - return False - - gripper_pose = prospection_robot.links[gripper_name].pose - diff = target_pose.dist(gripper_pose) - - return diff < threshold - - def blocking(self, - pose_or_object: Union[Object, Pose], - robot: Object, - gripper_name: str, - grasp: str = None) -> Union[List[Object], None]: - """ - Checks if any objects are blocking another object when a robot tries to pick it. This works - similar to the reachable predicate. First the inverse kinematics between the robot and the object will be - calculated and applied. Then it will be checked if the robot is in contact with any object except the given one. - If the given pose or Object is not reachable None will be returned - - :param pose_or_object: The object or pose for which blocking objects should be found - :param robot: The robot Object who reaches for the object - :param gripper_name: The name of the end effector of the robot - :param grasp: The grasp type with which the object should be grasped - :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose - or object is not reachable. - """ - - prospection_robot = self.world.get_prospection_object_from_object(robot) - with UseProspectionWorld(): - if grasp: - try_to_reach_with_grasp(pose_or_object, prospection_robot, gripper_name, grasp) + obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) + + seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis) + flat_list = list(itertools.chain.from_iterable(seg_mask)) + max_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) + World.current_world.restore_state(state_id) + + if max_pixel == 0: + # Object is not visible + return False + + seg_mask = World.current_world.get_images_for_target(target_point, camera_pose)[2] + flat_list = list(itertools.chain.from_iterable(seg_mask)) + real_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) + + return real_pixel / max_pixel > threshold > 0 + + +def occluding( + obj: Object, + camera_pose: Pose, + front_facing_axis: Optional[List[float]] = None) -> List[Object]: + """ + Lists all objects which are occluding the given object. This works similar to 'visible'. + First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. + After that the complete scene will be rendered and the previous saved pixel positions will be compared to the + actual pixels, if in one pixel another object is visible ot will be saved as occluding. + + :param obj: The object for which occlusion should be checked + :param camera_pose: The pose of the camera in world coordinate frame + :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :return: A list of occluding objects + """ + + with UseProspectionWorld(): + state_id = World.current_world.save_state() + for other_obj in World.current_world.objects: + if other_obj.name == World.current_world.robot.name: + continue + elif obj.get_pose() == other_obj.get_pose(): + obj = other_obj else: - try_to_reach(pose_or_object, prospection_robot, gripper_name) - - block = [] - for obj in World.current_world.objects: - if self.contact(prospection_robot, obj): - block.append(self.world.get_object_from_prospection_object(obj)) - return block - - def supporting(self, - object1: Object, - object2: Object) -> bool: - """ - Checks if one object is supporting another object. An object supports another object if they are in - contact and the second object is above the first one. (e.g. a Bottle will be supported by a table) - - :param object1: Object that is supported - :param object2: Object that supports the first object - :return: True if the second object is in contact with the first one and the second one ist above the first False else - """ - return self.contact(object1, object2) and object2.get_position().z > object1.get_position().z - - def link_pose_for_joint_config(self, obj: Object, joint_config: Dict[str, float], link_name: str) -> Pose: - """ - Returns the pose a link would be in if the given joint configuration would be applied to the object. - This is done by using the respective object in the prospection world and applying the joint configuration - to this one. After applying the joint configuration the link position is taken from there. - - :param obj: Object of which the link is a part - :param joint_config: Dict with the goal joint configuration - :param link_name: Name of the link for which the pose should be returned - :return: The pose of the link after applying the joint configuration - """ - prospection_object = self.world.get_prospection_object_from_object(obj) - with UseProspectionWorld(): - for joint, pose in joint_config.items(): - prospection_object.set_joint_position(joint, pose) - return prospection_object.links[link_name].pose + other_obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) + + seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis) + + # All indices where the object that could be occluded is in the image + # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension + pix = np.dstack(np.nonzero(seg_mask == obj.id))[0] + + World.current_world.restore_state(state_id) + + occluding_obj_ids = [] + seg_mask = World.current_world.get_images_for_target(target_point, camera_pose)[2] + for c in pix: + if not seg_mask[c[0]][c[1]] == obj.id: + occluding_obj_ids.append(seg_mask[c[0]][c[1]]) + + occ_objects = list(set(map(World.current_world.get_object_by_id, occluding_obj_ids))) + occ_objects = list(map(World.current_world.get_object_from_prospection_object, occ_objects)) + + return occ_objects + + +def reachable( + pose_or_object: Union[Object, Pose], + robot: Object, + gripper_name: str, + threshold: float = 0.01) -> bool: + """ + Checks if the robot can reach a given position. To determine this the inverse kinematics are + calculated and applied. Afterward the distance between the position and the given end effector is calculated, if + it is smaller than the threshold the reasoning query returns True, if not it returns False. + + :param pose_or_object: The position and rotation or Object for which reachability should be checked or an Object + :param robot: The robot that should reach for the position + :param gripper_name: The name of the end effector + :param threshold: The threshold between the end effector and the position. + :return: True if the end effector is closer than the threshold to the target position, False in every other case + """ + + prospection_robot = World.current_world.get_prospection_object_from_object(robot) + with UseProspectionWorld(): + target_pose = try_to_reach(pose_or_object, prospection_robot, gripper_name) + + if not target_pose: + return False + + gripper_pose = prospection_robot.links[gripper_name].pose + diff = target_pose.dist(gripper_pose) + + return diff < threshold + + +def blocking( + pose_or_object: Union[Object, Pose], + robot: Object, + gripper_name: str, + grasp: str = None) -> Union[List[Object], None]: + """ + Checks if any objects are blocking another object when a robot tries to pick it. This works + similar to the reachable predicate. First the inverse kinematics between the robot and the object will be + calculated and applied. Then it will be checked if the robot is in contact with any object except the given one. + If the given pose or Object is not reachable None will be returned + + :param pose_or_object: The object or pose for which blocking objects should be found + :param robot: The robot Object who reaches for the object + :param gripper_name: The name of the end effector of the robot + :param grasp: The grasp type with which the object should be grasped + :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose + or object is not reachable. + """ + + prospection_robot = World.current_world.get_prospection_object_from_object(robot) + with UseProspectionWorld(): + if grasp: + try_to_reach_with_grasp(pose_or_object, prospection_robot, gripper_name, grasp) + else: + try_to_reach(pose_or_object, prospection_robot, gripper_name) + + block = [] + for obj in World.current_world.objects: + if contact(prospection_robot, obj): + block.append(World.current_world.get_object_from_prospection_object(obj)) + return block + + +def supporting( + object1: Object, + object2: Object) -> bool: + """ + Checks if one object is supporting another object. An object supports another object if they are in + contact and the second object is above the first one. (e.g. a Bottle will be supported by a table) + + :param object1: Object that is supported + :param object2: Object that supports the first object + :return: True if the second object is in contact with the first one and the second is above the first else False + """ + return contact(object1, object2) and object2.get_position().z > object1.get_position().z + + +def link_pose_for_joint_config( + obj: Object, + joint_config: Dict[str, float], + link_name: str) -> Pose: + """ + Returns the pose a link would be in if the given joint configuration would be applied to the object. + This is done by using the respective object in the prospection world and applying the joint configuration + to this one. After applying the joint configuration the link position is taken from there. + + :param obj: Object of which the link is a part + :param joint_config: Dict with the goal joint configuration + :param link_name: Name of the link for which the pose should be returned + :return: The pose of the link after applying the joint configuration + """ + prospection_object = World.current_world.get_prospection_object_from_object(obj) + with UseProspectionWorld(): + for joint, pose in joint_config.items(): + prospection_object.set_joint_position(joint, pose) + return prospection_object.links[link_name].pose diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index 4375ca52e..2c6f2338d 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -1,7 +1,7 @@ import time from test_bullet_world import BulletWorldTest -from pycram.world_reasoning import WorldReasoning +import pycram.world_reasoning as btr from pycram.pose import Pose from pycram.robot_descriptions import robot_description @@ -14,28 +14,24 @@ def test_contact(self): self.milk.set_pose(Pose([1, 1, 1])) self.cereal.set_pose(Pose([1, 1, 1])) time.sleep(1) - btr = WorldReasoning(self.world) self.assertTrue(btr.contact(self.milk, self.cereal)) def test_visible(self): self.milk.set_pose(Pose([1.5, 0, 1.2])) self.robot.set_pose(Pose()) time.sleep(1) - btr = WorldReasoning(self.world) self.assertTrue(btr.visible(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, - robot_description.front_facing_axis)) + robot_description.front_facing_axis)) def test_occluding(self): self.milk.set_pose(Pose([3, 0, 1.2])) self.robot.set_pose(Pose()) - btr = WorldReasoning(self.world) self.assertTrue(btr.occluding(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, robot_description.front_facing_axis) != []) def test_reachable(self): self.robot.set_pose(Pose()) time.sleep(1) - btr = WorldReasoning(self.world) self.assertTrue(btr.reachable(Pose([0.5, -0.7, 1]), self.robot, robot_description.get_tool_frame("right"))) self.assertFalse(btr.reachable(Pose([2, 2, 1]), self.robot, robot_description.get_tool_frame("right"))) @@ -43,11 +39,9 @@ def test_blocking(self): self.milk.set_pose(Pose([0.5, -0.7, 1])) self.robot.set_pose(Pose()) time.sleep(1) - btr = WorldReasoning(self.world) self.assertTrue( btr.blocking(Pose([0.5, -0.7, 1]), self.robot, robot_description.get_tool_frame("right")) != []) def test_supporting(self): self.milk.set_pose(Pose([1.3, 0, 0.9])) - btr = WorldReasoning(self.world) self.assertTrue(btr.supporting(self.kitchen, self.milk)) From 8583fd1994f52203b24391fccb921d21960308da Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jan 2024 22:41:35 +0100 Subject: [PATCH 32/69] [WorldAbstractClass] (in progress) implementing abstract shape creation and visualization for usage in costmap for example. --- src/pycram/bullet_world.py | 19 +++++- src/pycram/costmaps.py | 109 ++++++++++++++++---------------- src/pycram/enums.py | 13 ++++ src/pycram/world.py | 36 ++++++++++- src/pycram/world_dataclasses.py | 102 +++++++++++++++++++++++++++++- 5 files changed, 216 insertions(+), 63 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index e5d341959..0a9d24a62 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -7,13 +7,13 @@ import numpy as np import pybullet as p -import rospy import rosgraph +import rospy from .enums import JointType, ObjectType from .pose import Pose from .world import World, Object -from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox +from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox, MultiBody class BulletWorld(World): @@ -402,6 +402,19 @@ def remove_vis_axis(self) -> None: p.removeBody(id, physicsClientId=self.client_id) self.vis_axis = [] + def create_multi_body(self, multi_body: MultiBody) -> int: + return p.createMultiBody(baseVisualShapeIndex=-MultiBody.base_visual_shape_index, + linkVisualShapeIndices=MultiBody.link_visual_shape_indices, + basePosition=MultiBody.base_position, baseOrientation=MultiBody.base_orientation, + linkPositions=MultiBody.link_positions, linkMasses=MultiBody.link_masses, + linkOrientations=MultiBody.link_orientations, + linkInertialFramePositions=MultiBody.link_inertial_frame_positions, + linkInertialFrameOrientations=MultiBody.link_inertial_frame_orientations, + linkParentIndices=MultiBody.link_parent_indices, + linkJointTypes=MultiBody.link_joint_types, + linkJointAxis=MultiBody.link_joint_axis, + linkCollisionShapeIndices=MultiBody.link_collision_shape_indices) + def get_images_for_target(self, target_pose: Pose, cam_pose: Pose, @@ -649,4 +662,4 @@ def run(self): cameraTargetPosition = (0.0, -50, 50) p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1], physicsClientId=self.world.client_id) - time.sleep(1. / 80.) \ No newline at end of file + time.sleep(1. / 80.) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 100ecbdbf..0fe5af279 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -1,43 +1,48 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations +from typing import Tuple, List, Optional + +import matplotlib.pyplot as plt import numpy as np +import psutil import pybullet as p import rospy -import matplotlib.pyplot as plt from matplotlib import colors -import psutil -import time -from .bullet_world import BulletWorld -from pycram.world import UseProspectionWorld, Object, Link -from .world_dataclasses import AxisAlignedBoundingBox from nav_msgs.msg import OccupancyGrid, MapMetaData -from typing import Tuple, List, Union, Optional +from pycram.world import UseProspectionWorld, Object, Link from .local_transformer import LocalTransformer from .pose import Pose +from .world import World +from .world_dataclasses import AxisAlignedBoundingBox, BoxVisualShape, BoxShapeData, MultiBody, Color class Costmap: """ The base class of all Costmaps which implements the visualization of costmaps - in the BulletWorld. + in the World. """ def __init__(self, resolution: float, height: int, width: int, origin: Pose, - map: np.ndarray): + map: np.ndarray, + world: Optional[World] = None): """ The constructor of the base class of all Costmaps. - :param resolution: The distance in metre in the real world which is represented by a single entry in the costmap. + :param resolution: The distance in metre in the real-world which is + represented by a single entry in the costmap. :param height: The height of the costmap. :param width: The width of the costmap. - :param origin: The origin of the costmap, in world coordinate frame. The origin of the costmap is located in the centre of the costmap + :param origin: The origin of the costmap, in world coordinate frame. The origin of the costmap is located in the + centre of the costmap. :param map: The costmap represents as a 2D numpy array. + :param world: The World for which the costmap should be created. """ + self.world = world if world else World.current_world self.resolution: float = resolution self.size: int = height self.height: int = height @@ -49,18 +54,15 @@ def __init__(self, resolution: float, def visualize(self) -> None: """ - Visualizes a costmap in the BulletWorld, the visualisation works by + Visualizes a costmap in the World, the visualisation works by subdividing the costmap in rectangles which are then visualized as pybullet visual shapes. """ - if self.vis_ids != []: + if self.vis_ids: return # working on a copy of the costmap, since found rectangles are deleted map = np.copy(self.map) - curr_width = 0 - curr_height = 0 - curr_pose = [] boxes = [] # Finding all rectangles in the costmap for i in range(0, map.shape[0]): @@ -76,13 +78,13 @@ def visualize(self) -> None: # Creation of the visual shapes, for documentation of the visual shapes # please look here: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.q1gn7v6o58bf for box in boxes: - visual = p.createVisualShape(p.GEOM_BOX, - halfExtents=[(box[1] * self.resolution) / 2, (box[2] * self.resolution) / 2, - 0.001], - rgbaColor=[1, 0, 0, 0.6], - visualFramePosition=[(box[0][0] + box[1] / 2) * self.resolution, - (box[0][1] + box[2] / 2) * self.resolution, 0.]) + box_shape_data = BoxShapeData([(box[1] * self.resolution) / 2, (box[2] * self.resolution) / 2, 0.001]) + visual_frame_position = [(box[0][0] + box[1] / 2) * self.resolution, + (box[0][1] + box[2] / 2) * self.resolution, 0.] + visual_shape = BoxVisualShape(Color(1, 0, 0, 0.6), visual_frame_position, box_shape_data) + visual = self.world.create_box_visual_shape(visual_shape) cells.append(visual) + # Set to 127 for since this is the maximal amount of links in a multibody for cell_parts in self._chunks(cells, 127): # Dummy paramater since these are needed to spawn visual shapes as a @@ -94,24 +96,21 @@ def visualize(self) -> None: link_joints = [p.JOINT_FIXED for c in cell_parts] link_collision = [-1 for c in cell_parts] link_joint_axis = [[1, 0, 0] for c in cell_parts] - # The position at which the multibody will be spawned. Offset such that - # the origin referes to the centre of the costmap. - # origin_pose = self.origin.position_as_list() - # base_pose = [origin_pose[0] - self.height / 2 * self.resolution, - # origin_pose[1] - self.width / 2 * self.resolution, origin_pose[2]] offset = [[-self.height / 2 * self.resolution, -self.width / 2 * self.resolution, 0.05], [0, 0, 0, 1]] new_pose = p.multiplyTransforms(self.origin.position_as_list(), self.origin.orientation_as_list(), offset[0], offset[1]) - map_obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=cell_parts, - basePosition=new_pose[0], baseOrientation=new_pose[1], linkPositions=link_poses, - # [0, 0, 1, 0] - linkMasses=link_masses, linkOrientations=link_orientations, - linkInertialFramePositions=link_poses, - linkInertialFrameOrientations=link_orientations, linkParentIndices=link_parent, - linkJointTypes=link_joints, linkJointAxis=link_joint_axis, - linkCollisionShapeIndices=link_collision) + multi_body = MultiBody(base_visual_shape_index=-1, base_position=new_pose[0], base_orientation=new_pose[1], + link_visual_shape_indices=cell_parts, link_positions=link_poses, + link_orientations=link_orientations, link_masses=link_masses, + link_inertial_frame_positions=link_poses, + link_inertial_frame_orientations=link_orientations, + link_parent_indices=link_parent, link_joint_types=link_joints, + link_joint_axis=link_joint_axis, + link_collision_shape_indices=link_collision) + + map_obj = self.world.create_multi_body(multi_body) self.vis_ids.append(map_obj) def _chunks(self, lst: List, n: int) -> List: @@ -127,7 +126,7 @@ def _chunks(self, lst: List, n: int) -> List: def close_visualization(self) -> None: """ - Removes the visualization from the BulletWorld. + Removes the visualization from the World. """ for id in self.vis_ids: p.removeBody(id) @@ -151,7 +150,7 @@ def _find_consectuive_line(self, start: Tuple[int, int], map: np.ndarray) -> int return lenght return lenght - def _find_max_box_height(self, start: Tuple[int, int], lenght: int, map: np.ndarray) -> int: + def _find_max_box_height(self, start: Tuple[int, int], length: int, map: np.ndarray) -> int: """ Finds the maximal height for a rectangle with a given width in a costmap. The method traverses one row at a time and checks if all entries for the @@ -166,7 +165,7 @@ def _find_max_box_height(self, start: Tuple[int, int], lenght: int, map: np.ndar height, width = map.shape curr_height = 1 for i in range(start[0], height): - for j in range(start[1], start[1] + lenght): + for j in range(start[1], start[1] + length): if map[i][j] <= 0: return curr_height curr_height += 1 @@ -235,9 +234,9 @@ def __init__(self, distance_to_obstacle: float, inflated. Meaning that obstacles in the costmap are growing bigger by this distance. :param from_ros: This determines if the Occupancy map should be created - from the map provided by the ROS map_server or from the BulletWorld. + from the map provided by the ROS map_server or from the World. If True then the map from the ROS map_server will be used otherwise - the Occupancy map will be created from the BulletWorld. + the Occupancy map will be created from the World. :param size: The length of the side of the costmap. The costmap will be created as a square. This will only be used if from_ros is False. :param resolution: The resolution of this costmap. This determines how much @@ -281,7 +280,7 @@ def _calculate_diff_origin(self, height: int, width: int) -> Pose: """ actual_origin = [int(height / 2) * self.resolution, int(width / 2) * self.resolution, 0] origin = np.array(self.meta_origin) + np.array(actual_origin) - return Pose(origin) + return Pose(origin.tolist()) @staticmethod def _get_map() -> np.ndarray: @@ -361,7 +360,7 @@ def create_sub_map(self, sub_origin: Pose, size: int) -> Costmap: def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: """ - Creates an Occupancy Costmap for the specified BulletWorld. + Creates an Occupancy Costmap for the specified World. This map marks every position as valid that has no object above it. After creating the costmap the distance to obstacle parameter is applied. @@ -388,15 +387,16 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: for n in self._chunks(np.array(rays), 16380): with UseProspectionWorld(): r_t = p.rayTestBatch(n[:, 0], n[:, 1], numThreads=0, - physicsClientId=BulletWorld.current_world.client_id) + physicsClientId=World.current_world.client_id) while r_t is None: r_t = p.rayTestBatch(n[:, 0], n[:, 1], numThreads=0, - physicsClientId=BulletWorld.current_world.client_id) + physicsClientId=World.current_world.client_id) j += len(n) - if BulletWorld.robot: - shadow_robot = BulletWorld.current_world.get_prospection_object_from_object(BulletWorld.robot) - attached_objs = BulletWorld.robot.attachments.keys() - attached_objs_shadow_id = [BulletWorld.current_world.get_prospection_object_from_object(x).id for x in + if World.robot: + shadow_robot = World.current_world.get_prospection_object_from_object(World.robot) + attached_objs = World.robot.attachments.keys() + attached_objs_shadow_id = [World.current_world.get_prospection_object_from_object(x).id for x + in attached_objs] res[i:j] = [ 1 if ray[0] == -1 or ray[0] == shadow_robot.id or ray[0] in attached_objs_shadow_id else 0 for @@ -450,7 +450,7 @@ def __init__(self, min_height: float, size: Optional[int] = 100, resolution: Optional[float] = 0.02, origin: Optional[Pose] = None, - world: Optional[BulletWorld] = None): + world: Optional[World] = None): """ Visibility Costmaps show for every position around the origin pose if the origin can be seen from this pose. The costmap is able to deal with height differences of the camera while in a single position, for example, if @@ -466,12 +466,11 @@ def __init__(self, min_height: float, costmap represents. :param origin: The pose in world coordinate frame around which the costmap should be created. - :param world: The BulletWorld for which the costmap should be created. + :param world: The World for which the costmap should be created. """ if (11 * size ** 2 + size ** 3) * 2 > psutil.virtual_memory().available: raise OSError("Not enough free RAM to calculate a costmap of this size") - self.world = world if world else BulletWorld.current_world self.map = np.zeros((size, size)) self.size = size self.resolution = resolution @@ -584,7 +583,7 @@ def _generate_map(self): depth_indices[int(self.size / 2), int(self.size / 2), 1] = 1 # Calculate columns for the respective position in the costmap - columns = np.around(((depth_indices[:, :, :1] / depth_indices[:, :, 1:2]) \ + columns = np.around(((depth_indices[:, :, :1] / depth_indices[:, :, 1:2]) * (self.size / 2)) + self.size / 2).reshape((self.size, self.size)).astype('int16') # An array with size * size that contains the euclidean distance to the @@ -691,9 +690,9 @@ def __init__(self, object, urdf_link_name, size=100, resolution=0.02, world=None :param object: The object of which the link is a part :param urdf_link_name: The link name, as stated in the URDF :param resolution: Resolution of the final costmap - :param world: The BulletWorld from which the costmap should be created + :param world: The World from which the costmap should be created """ - self.world: BulletWorld = world if world else BulletWorld.current_world + self.world: World = world if world else World.current_world self.object: Object = object self.link: Link = object.links[urdf_link_name] self.resolution: float = resolution @@ -723,7 +722,7 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox: :return: Two points in world coordinate space, which span a rectangle """ - prospection_object = BulletWorld.current_world.get_prospection_object_from_object(self.object) + prospection_object = World.current_world.get_prospection_object_from_object(self.object) with UseProspectionWorld(): prospection_object.set_orientation(Pose(orientation=[0, 0, 0, 1])) link_pose_trans = self.link.transform diff --git a/src/pycram/enums.py b/src/pycram/enums.py index 40257a3c2..79238403d 100644 --- a/src/pycram/enums.py +++ b/src/pycram/enums.py @@ -2,6 +2,8 @@ from enum import Enum, auto +import pybullet + class Arms(Enum): """Enum for Arms.""" @@ -57,3 +59,14 @@ class ObjectType(Enum): ENVIRONMENT = auto() GENERIC_OBJECT = auto() + +class Shape(Enum): + """ + Enum for visual shapes of objects + """ + SPHERE = 2 + BOX = 3 + CYLINDER = 4 + MESH = 5 + PLANE = 6 + CAPSULE = 7 diff --git a/src/pycram/world.py b/src/pycram/world.py index 5a89c4b63..5e9c1f46d 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -32,7 +32,9 @@ from abc import ABC, abstractmethod from dataclasses import dataclass -from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox, CollisionCallbacks +from .world_dataclasses import (Color, Constraint, AxisAlignedBoundingBox, CollisionCallbacks, + MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape) class World(ABC): @@ -698,7 +700,8 @@ def _copy(self) -> World: """ world = World("DIRECT", False, World.simulation_time_step) for obj in self.objects: - o = Object(obj.name, obj.obj_type, obj.path, Pose(obj.get_position(), obj.get_orientation()), world, + obj_pose = Pose(obj.get_position_as_list(), obj.get_orientation_as_list()) + o = Object(obj.name, obj.obj_type, obj.path, obj_pose, world, obj.color) for joint in obj.joint_name_to_id: o.set_joint_position(joint, obj.get_joint_position(joint)) @@ -793,6 +796,30 @@ def update_transforms_for_objects_in_current_world(self) -> None: for obj in list(self.current_world.objects): obj.update_link_transforms(curr_time) + def create_visual_shape(self, visual_shape: VisualShape) -> int: + raise NotImplementedError + + def create_multi_body(self, multi_body: MultiBody) -> int: + raise NotImplementedError + + def create_box_visual_shape(self, shape_data: BoxVisualShape) -> int: + raise NotImplementedError + + def create_cylinder_visual_shape(self, shape_data: CylinderVisualShape) -> int: + raise NotImplementedError + + def create_sphere_visual_shape(self, shape_data: SphereVisualShape) -> int: + raise NotImplementedError + + def create_capsule_visual_shape(self, shape_data: CapsuleVisualShape) -> int: + raise NotImplementedError + + def create_plane_visual_shape(self, shape_data: PlaneVisualShape) -> int: + raise NotImplementedError + + def create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: + raise NotImplementedError + class UseProspectionWorld: """ @@ -1602,7 +1629,10 @@ def set_color(self, color: Color, link: Optional[str] = "") -> None: self.world.set_object_color(self, color, link) def get_color(self, link: Optional[str] = None) -> Union[Color, Dict[str, Color], None]: - return self.world.get_color_of_object(self, link) + if link is None: + return self.world.get_color_of_object(self) + else: + return self.world.get_color_of_object_link(self, link) def get_aabb(self) -> AxisAlignedBoundingBox: return self.world.get_object_aabb(self) diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index fb057aa3a..8c22c7fa9 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -1,6 +1,6 @@ from dataclasses import dataclass from typing import List, Optional, Tuple, Callable -from .enums import JointType +from .enums import JointType, Shape @dataclass @@ -138,4 +138,102 @@ def get_max(self) -> List[float]: @dataclass class CollisionCallbacks: on_collision_cb: Callable - no_collision_cb: Optional[Callable] = None \ No newline at end of file + no_collision_cb: Optional[Callable] = None + + +@dataclass +class MultiBody: + base_visual_shape_index: int + base_position: List[float] + base_orientation: List[float] + link_visual_shape_indices: List[int] + link_positions: List[List[float]] + link_orientations: List[List[float]] + link_masses: List[float] + link_inertial_frame_positions: List[List[float]] + link_inertial_frame_orientations: List[List[float]] + link_parent_indices: List[int] + link_joint_types: List[JointType] + link_joint_axis: List[List[float]] + link_collision_shape_indices: List[int] + + +@dataclass +class ShapeData: + pass + + +@dataclass +class BoxShapeData(ShapeData): + half_extents: List[float] + + +@dataclass +class SphereShapeData(ShapeData): + radius: float + + +@dataclass +class CapsuleShapeData(ShapeData): + radius: float + length: float + + +@dataclass +class CylinderShapeData(ShapeData): + radius: float + length: float + + +@dataclass +class MeshShapeData(ShapeData): + mesh_scale: List[float] + mesh_file_name: str + + +@dataclass +class PlaneShapeData(ShapeData): + normal: List[float] + + +@dataclass +class VisualShape: + rgba_color: Color + visual_frame_position: List[float] + shape_data: ShapeData + + +@dataclass +class BoxVisualShape(VisualShape): + shape_data: BoxShapeData + visual_geometry_type = Shape.BOX + + +@dataclass +class SphereVisualShape(VisualShape): + shape_data: SphereShapeData + visual_geometry_type = Shape.SPHERE + + +@dataclass +class CapsuleVisualShape(VisualShape): + shape_data: CapsuleShapeData + visual_geometry_type = Shape.CAPSULE + + +@dataclass +class CylinderVisualShape(VisualShape): + shape_data: CylinderShapeData + visual_geometry_type = Shape.CYLINDER + + +@dataclass +class MeshVisualShape(VisualShape): + visual_geometry_type = Shape.MESH + shape_data: MeshShapeData + + +@dataclass +class PlaneVisualShape(VisualShape): + shape_data: PlaneShapeData + visual_geometry_type = Shape.PLANE From ed33735dcf5c4fab1039dd947f73d0cbfc7a36e7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jan 2024 12:09:31 +0100 Subject: [PATCH 33/69] Merge branch 'dev' of github.com:cram2/pycram into abstract_world --- src/neem_interface_python | 2 +- src/pycram/costmaps.py | 3 ++- src/pycram/designator.py | 2 +- src/pycram/designators/action_designator.py | 21 ++++++++++--------- src/pycram/external_interfaces/ik.py | 6 +++--- src/pycram/external_interfaces/robokudo.py | 2 +- src/pycram/local_transformer.py | 4 ++-- src/pycram/pose_generator_and_validator.py | 6 +++--- .../process_modules/pr2_process_modules.py | 14 ++++++------- src/pycram/world.py | 6 +++--- test/bullet_world_testcase.py | 11 +++------- test/test_failure_handling.py | 4 ++-- test/test_language.py | 6 +++--- test/test_local_transformer.py | 4 ++-- 14 files changed, 44 insertions(+), 47 deletions(-) diff --git a/src/neem_interface_python b/src/neem_interface_python index 51aa0f9a3..05ad42c41 160000 --- a/src/neem_interface_python +++ b/src/neem_interface_python @@ -1 +1 @@ -Subproject commit 51aa0f9a3b38fde3156bdf29be7aad7bb9547926 +Subproject commit 05ad42c41042335dd2287d865f6a37c115ea8ffe diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 0fe5af279..e0acabd03 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -48,7 +48,7 @@ def __init__(self, resolution: float, self.height: int = height self.width: int = width local_transformer = LocalTransformer() - self.origin: Pose = local_transformer.transform_pose_to_target_frame(origin, 'map') + self.origin: Pose = local_transformer.transform_pose(origin, 'map') self.map: np.ndarray = map self.vis_ids: List[int] = [] @@ -471,6 +471,7 @@ def __init__(self, min_height: float, if (11 * size ** 2 + size ** 3) * 2 > psutil.virtual_memory().available: raise OSError("Not enough free RAM to calculate a costmap of this size") + self.world = world if world else World.current_world self.map = np.zeros((size, size)) self.size = size self.resolution = resolution diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 209373022..65aedce37 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -701,7 +701,7 @@ def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: :return: The adjusted grasp pose """ lt = LocalTransformer() - pose_in_object = lt.transform_pose_to_target_frame(pose, self.bullet_world_object.tf_frame) + pose_in_object = lt.transform_pose(pose, self.bullet_world_object.tf_frame) special_knowledge = [] # Initialize as an empty list if self.obj_type in SPECIAL_KNOWLEDGE: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 852569b45..91d5fff85 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -310,11 +310,11 @@ def perform(self) -> None: # oTm = Object Pose in Frame map oTm = object.get_pose() # Transform the object pose to the object frame, basically the origin of the object frame - mTo = object.local_transformer.transform_pose_to_target_frame(oTm, object.tf_frame) + mTo = object.local_transformer.transform_pose(oTm, object.tf_frame) # Adjust the pose according to the special knowledge of the object designator adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) # Transform the adjusted pose to the map frame - adjusted_oTm = object.local_transformer.transform_pose_to_target_frame(adjusted_pose, "map") + adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper ori = multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y, adjusted_oTm.orientation.z, adjusted_oTm.orientation.w], @@ -330,11 +330,11 @@ def perform(self) -> None: # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" gripper_frame = robot.links[robot_description.get_tool_frame(self.arm)].tf_frame # First rotate the gripper, so the further calculations makes sense - tmp_for_rotate_pose = object.local_transformer.transform_pose_to_target_frame(adjusted_oTm, gripper_frame) + tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) tmp_for_rotate_pose.pose.position.x = 0 tmp_for_rotate_pose.pose.position.y = 0 tmp_for_rotate_pose.pose.position.z = -0.1 - gripper_rotate_pose = object.local_transformer.transform_pose_to_target_frame(tmp_for_rotate_pose, "map") + gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") #Perform Gripper Rotate # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) @@ -437,16 +437,17 @@ def perform(self) -> None: # Transformations such that the target position is the position of the object and not the tcp tool_name = robot_description.get_tool_frame(self.arm) - tcp_to_object = local_tf.transform_pose_to_target_frame(object_pose, - BulletWorld.robot.links[tool_name].tf_frame) + tcp_to_object = local_tf.transform_pose(object_pose, + BulletWorld.robot.links[tool_name].tf_frame) target_diff = self.target_location.to_transform("target").inverse_times( tcp_to_object.to_transform("object")).to_pose() MoveTCPMotion(target_diff, self.arm).resolve().perform() MoveGripperMotion("open", self.arm).resolve().perform() BulletWorld.robot.detach(self.object_designator.bullet_world_object) - retract_pose = local_tf.transform_pose(target_diff,BulletWorld.robot.get_link_tf_frame( - robot_description.get_tool_frame(self.arm))) + retract_pose = local_tf.transform_pose( + target_diff, + BulletWorld.robot.links[robot_description.get_tool_frame(self.arm)].tf_frame) retract_pose.position.x -= 0.07 MoveTCPMotion(retract_pose, self.arm).resolve().perform() @@ -887,8 +888,8 @@ def perform(self) -> Any: lt = LocalTransformer() gripper_name = robot_description.get_tool_frame(self.arm) - object_pose_in_gripper = lt.transform_pose_to_target_frame(object_pose, - BulletWorld.robot.links[gripper_name].tf_frame) + object_pose_in_gripper = lt.transform_pose(object_pose, + BulletWorld.robot.links[gripper_name].tf_frame) pre_grasp = object_pose_in_gripper.copy() pre_grasp.pose.position.x -= 0.1 diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 262a3c063..b7d1fb50b 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -45,7 +45,7 @@ def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_ob :return: A moveit_msgs/PositionIKRequest message containing all the information from the parameter """ local_transformer = LocalTransformer() - target_pose = local_transformer.transform_pose_to_target_frame(target_pose, robot_object.links[root_link].tf_frame) + target_pose = local_transformer.transform_pose(target_pose, robot_object.links[root_link].tf_frame) robot_state = RobotState() joint_state = JointState() @@ -135,7 +135,7 @@ def apply_grasp_orientation_to_pose(grasp: str, pose: Pose) -> Pose: :param pose: The pose to which the grasp orientation should be applied """ local_transformer = LocalTransformer() - target_map = local_transformer.transform_pose_to_target_frame(pose, "map") + target_map = local_transformer.transform_pose(pose, "map") grasp_orientation = robot_description.grasps.get_orientation_for_grasp(grasp) target_map.orientation.x = grasp_orientation[0] target_map.orientation.y = grasp_orientation[1] @@ -180,7 +180,7 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str # Get link after last joint in chain end_effector = robot_description.get_child(joints[-1]) - target_torso = local_transformer.transform_pose_to_target_frame(target_pose, robot.links[base_link].tf_frame) + target_torso = local_transformer.transform_pose(target_pose, robot.links[base_link].tf_frame) diff = calculate_wrist_tool_offset(end_effector, gripper, robot) target_diff = target_torso.to_transform("target").inverse_times(diff).to_pose() diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index b2574b55b..fbfad9b5a 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -129,7 +129,7 @@ def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescripti source = query_result.res[0].poseSource[i] lt = LocalTransformer() - pose = lt.transform_pose_to_target_frame(pose, "map") + pose = lt.transform_pose(pose, "map") pose_candidates[source] = pose diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 05d149277..ebbe4e64d 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -55,7 +55,7 @@ def __init__(self): # If the singelton was already initialized self._initialized = True - def transform_pose_to_target_frame(self, pose: Pose, target_frame: str) -> Union[Pose, None]: + def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: """ Transforms a given pose to the target frame after updating the transforms for all objects in the current world. @@ -115,5 +115,5 @@ def transformPose(self, target_frame, ps) -> Pose: Alias for :func:`~LocalTransformer.transform_pose_to_target_frame` to avoid confusion since a similar method exists in the super class. """ - return self.transform_pose_to_target_frame(ps, target_frame) + return self.transform_pose(ps, target_frame) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index ff761aa6e..2aa5797af 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -152,7 +152,7 @@ def reachability_validator(pose: Pose, if robot in allowed_collision.keys(): allowed_robot_links = allowed_collision[robot] - joint_state_before_ik=robot._current_joint_states + joint_state_before_ik=robot._current_joints_positions try: # resp = request_ik(base_link, end_effector, target_diff, robot, left_joints) resp = request_ik(target, robot, left_joints, left_gripper) @@ -175,7 +175,7 @@ def reachability_validator(pose: Pose, except IKError: pass finally: - robot.set_joint_states(joint_state_before_ik) + robot.set_positions_of_all_joints(joint_state_before_ik) try: # resp = request_ik(base_link, end_effector, target_diff, robot, right_joints) @@ -199,6 +199,6 @@ def reachability_validator(pose: Pose, except IKError: pass finally: - robot.set_joint_states(joint_state_before_ik) + robot.set_positions_of_all_joints(joint_state_before_ik) return res, arms diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 87b699d3d..5f432560b 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -91,7 +91,7 @@ def _execute(self, desig: PlaceMotion.Motion): object_pose = object.get_pose() local_tf = LocalTransformer() tool_name = robot_description.get_tool_frame(arm) - tcp_to_object = local_tf.transform_pose_to_target_frame(object_pose, robot.links[tool_name].tf_frame) + tcp_to_object = local_tf.transform_pose(object_pose, robot.links[tool_name].tf_frame) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, arm) @@ -109,8 +109,8 @@ def _execute(self, desig: LookingMotion.Motion): robot = BulletWorld.robot local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose_to_target_frame(target, robot.links["head_pan_link"].tf_frame) - pose_in_tilt = local_transformer.transform_pose_to_target_frame(target, robot.links["head_tilt_link"].tf_frame) + pose_in_pan = local_transformer.transform_pose(target, robot.links["head_pan_link"].tf_frame) + pose_in_tilt = local_transformer.transform_pose(target, robot.links["head_tilt_link"].tf_frame) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 @@ -289,8 +289,8 @@ def _execute(self, desig: LookingMotion.Motion): robot = BulletWorld.robot local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose_to_target_frame(target, robot.links["head_pan_link"].tf_frame) - pose_in_tilt = local_transformer.transform_pose_to_target_frame(target, robot.links["head_tilt_link"].tf_frame) + pose_in_pan = local_transformer.transform_pose(target, robot.links["head_pan_link"].tf_frame) + pose_in_tilt = local_transformer.transform_pose(target, robot.links["head_tilt_link"].tf_frame) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 @@ -315,7 +315,7 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: obj_pose = query_result["ClusterPoseBBAnnotator"] lt = LocalTransformer() - obj_pose = lt.transform_pose_to_target_frame(obj_pose, BulletWorld.robot.links["torso_lift_link"].tf_frame) + obj_pose = lt.transform_pose(obj_pose, BulletWorld.robot.links["torso_lift_link"].tf_frame) obj_pose.orientation = [0, 0, 0, 1] obj_pose.position.x += 0.05 @@ -344,7 +344,7 @@ class Pr2MoveTCPReal(ProcessModule): def _execute(self, designator: MoveTCPMotion.Motion) -> Any: lt = LocalTransformer() - pose_in_map = lt.transform_pose_to_target_frame(designator.target, "map") + pose_in_map = lt.transform_pose(designator.target, "map") if designator.allow_gripper_collision: giskard.allow_gripper_collision(designator.arm) diff --git a/src/pycram/world.py b/src/pycram/world.py index 5e9c1f46d..4113dd1cb 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -1015,7 +1015,7 @@ def get_transform_from_link(self, link: Link) -> Transform: return self.get_pose_wrt_link(link).to_transform(self.tf_frame) def get_pose_wrt_link(self, link: Link) -> Pose: - return self.local_transformer.transform_pose_to_target_frame(self.pose, link.tf_frame) + return self.local_transformer.transform_pose(self.pose, link.tf_frame) def get_aabb(self) -> AxisAlignedBoundingBox: return self.world.get_object_link_aabb(self.object.id, self.id) @@ -1131,7 +1131,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.color: Color = color self.local_transformer = LocalTransformer() - self.original_pose = self.local_transformer.transform_pose_to_target_frame(pose, "map") + self.original_pose = self.local_transformer.transform_pose(pose, "map") position, orientation = self.original_pose.to_list() self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignore_cached_files) self._current_pose = self.original_pose @@ -1332,7 +1332,7 @@ def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Op :param base: If True places the object base instead of origin at the specified position and orientation :param set_attachments: Whether to set the poses of the attached objects to this object or not. """ - pose_in_map = self.local_transformer.transform_pose_to_target_frame(pose, "map") + pose_in_map = self.local_transformer.transform_pose(pose, "map") position, orientation = pose_in_map.to_list() if base: diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index b34b434c4..b3fb307eb 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -1,15 +1,10 @@ import unittest -import numpy as np -import rospkg - -from pycram.bullet_world import BulletWorld, Object, fix_missing_inertial +from pycram.bullet_world import BulletWorld, Object from pycram.pose import Pose from pycram.robot_descriptions import robot_description from pycram.process_module import ProcessModule from pycram.enums import ObjectType -import os -import tf class BulletWorldTestCase(unittest.TestCase): @@ -26,14 +21,14 @@ def setUpClass(cls): ProcessModule.execution_delay = False def setUp(self): - self.world.reset_bullet_world() + self.world.reset_world() # DO NOT WRITE TESTS HERE!!! # Test related to the BulletWorld should be written in test_bullet_world.py # Tests in here would not be properly executed in the CI def tearDown(self): - self.world.reset_bullet_world() + self.world.reset_world() @classmethod def tearDownClass(cls): diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index ab0a8491b..d23a9d93b 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -34,7 +34,7 @@ def setUpClass(cls): ProcessModule.execution_delay = True def setUp(self): - self.world.reset_bullet_world() + self.world.reset_world() def test_retry_with_success(self): with simulated_robot: @@ -46,7 +46,7 @@ def test_retry_with_failure(self): Retry(DummyActionDesignator(), max_tries=5).perform() def tearDown(self): - self.world.reset_bullet_world() + self.world.reset_world() @classmethod def tearDownClass(cls): diff --git a/test/test_language.py b/test/test_language.py index 951db67c4..7470ed751 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -144,11 +144,11 @@ def test_perform_desig(self): with simulated_robot: plan.perform() self.assertEqual(self.robot.get_pose(), Pose([1, 1, 0])) - self.assertEqual(self.robot.get_joint_state("torso_lift_joint"), 0.3) + self.assertEqual(self.robot.get_joint_position("torso_lift_joint"), 0.3) for joint, pose in robot_description.get_static_joint_chain("right", "park").items(): - self.assertEqual(self.world.robot.get_joint_state(joint), pose) + self.assertEqual(self.world.robot.get_joint_position(joint), pose) for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): - self.assertEqual(self.world.robot.get_joint_state(joint), pose) + self.assertEqual(self.world.robot.get_joint_position(joint), pose) def test_perform_code(self): def test_set(param): diff --git a/test/test_local_transformer.py b/test/test_local_transformer.py index 4b4705eda..4cad086e7 100644 --- a/test/test_local_transformer.py +++ b/test/test_local_transformer.py @@ -22,7 +22,7 @@ def test_transform_pose(self): l.setTransform(Transform([1, 1, 1], [0, 0, 0, 1], "map", "test_frame")) p = Pose() - transformed_pose = l.transform_pose_to_target_frame(p, "test_frame") + transformed_pose = l.transform_pose(p, "test_frame") self.assertTrue(transformed_pose.position_as_list() == [-1, -1, -1]) self.assertTrue(transformed_pose.orientation_as_list() == [0, 0, 0, 1]) @@ -33,7 +33,7 @@ def test_transform_pose_position(self): l.setTransform(Transform([1, 1, 1], [0, 0, 0, 1], "map", "test_frame")) p = Pose() - transformed_pose = l.transform_pose_to_target_frame(p, "test_frame") + transformed_pose = l.transform_pose(p, "test_frame") self.assertTrue(transformed_pose.position == transformed_pose.pose.position) From 49a70264ab1c3df776cda17b9f944c5acd46b060 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jan 2024 12:25:25 +0100 Subject: [PATCH 34/69] Merge branch 'dev' of github.com:cram2/pycram into abstract_world (fixed missing import and use of World instead of BulletWorld in test_database_resolver.py) --- test/test_database_resolver.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index ea22b3c3c..18d7c047b 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -5,6 +5,7 @@ import pycram.plan_failures from pycram.world import Object from pycram import task +from pycram.world import World from pycram.designators import action_designator, object_designator from pycram.orm.base import Base from pycram.process_module import ProcessModule @@ -32,7 +33,7 @@ @unittest.skipIf(not jpt_installed, "jpt is not installed but needed for the definition of DatabaseCostmapLocations. " "Install via 'pip install pyjpt'") class DatabaseResolverTestCase(unittest.TestCase,): - world: BulletWorld + world: World milk: Object robot: Object engine: sqlalchemy.engine.Engine @@ -41,7 +42,7 @@ class DatabaseResolverTestCase(unittest.TestCase,): @classmethod def setUpClass(cls) -> None: global pycrorm_uri - cls.world = BulletWorld("DIRECT") + cls.world = World("DIRECT") cls.milk = Object("milk", "milk", "milk.stl", pose=Pose([1.3, 1, 0.9])) cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf") ProcessModule.execution_delay = False @@ -49,12 +50,12 @@ def setUpClass(cls) -> None: cls.session = sqlalchemy.orm.Session(bind=cls.engine) def setUp(self) -> None: - self.world.reset_bullet_world() + self.world.reset_world() pycram.orm.base.Base.metadata.create_all(self.engine) self.session.commit() def tearDown(self) -> None: - self.world.reset_bullet_world() + self.world.reset_world() pycram.task.reset_tree() @classmethod From 3916ded30093fe135f70531d4f7abacc60f258f8 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jan 2024 21:58:06 +0100 Subject: [PATCH 35/69] [Abstract World] (check TODOs) Costmaps now is independent of pybullet, HSR module is improved but still dependent on pybullet, designators are independent of pybullet. --- src/pycram/bullet_world.py | 19 ++++- src/pycram/costmaps.py | 31 ++++---- src/pycram/designator.py | 30 ++++---- src/pycram/designators/action_designator.py | 38 +++++----- src/pycram/designators/location_designator.py | 39 +++++----- src/pycram/designators/motion_designator.py | 2 +- src/pycram/designators/object_designator.py | 31 ++++---- src/pycram/pose_generator_and_validator.py | 30 ++++---- .../process_modules/boxy_process_modules.py | 16 ++-- .../default_process_modules.py | 16 ++-- .../process_modules/donbot_process_modules.py | 4 +- .../process_modules/hsr_process_modules.py | 76 ++++++++++--------- .../process_modules/pr2_process_modules.py | 16 ++-- src/pycram/world.py | 59 +++++++++----- src/pycram/world_dataclasses.py | 14 ++-- test/test_action_designator.py | 6 +- 16 files changed, 231 insertions(+), 196 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 173420bfa..74766046c 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -13,7 +13,10 @@ from .enums import JointType, ObjectType from .pose import Pose from .world import World, Object -from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox, MultiBody +from .world_dataclasses import (Color, Constraint, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, + CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, + MeshVisualShape) +from dataclasses import asdict class BulletWorld(World): @@ -404,6 +407,20 @@ def remove_vis_axis(self) -> None: p.removeBody(id, physicsClientId=self.client_id) self.vis_axis = [] + def ray_test(self, from_position: List[float], to_position: List[float]) -> int: + res = p.rayTest(from_position, to_position, physicsClientId=self.client_id) + return res[0][0] + + def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], + num_threads: int = 1) -> List[int]: + return p.rayTestBatch(from_positions, to_positions, numThreads=num_threads, + physicsClientId=self.client_id) + + def create_visual_shape(self, visual_shape: VisualShape) -> int: + return p.createVisualShape(visual_shape.visual_geometry_type, rgbaColor=visual_shape.rgba_color, + visualFramePosition=visual_shape.visual_frame_position, + physicsClientId=self.client_id, **asdict(visual_shape.shape_data)) + def create_multi_body(self, multi_body: MultiBody) -> int: return p.createMultiBody(baseVisualShapeIndex=-MultiBody.base_visual_shape_index, linkVisualShapeIndices=MultiBody.link_visual_shape_indices, diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index e0acabd03..6e0154e2e 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -6,16 +6,15 @@ import matplotlib.pyplot as plt import numpy as np import psutil -import pybullet as p import rospy from matplotlib import colors from nav_msgs.msg import OccupancyGrid, MapMetaData from pycram.world import UseProspectionWorld, Object, Link from .local_transformer import LocalTransformer -from .pose import Pose +from .pose import Pose, Transform from .world import World -from .world_dataclasses import AxisAlignedBoundingBox, BoxVisualShape, BoxShapeData, MultiBody, Color +from .world_dataclasses import AxisAlignedBoundingBox, BoxVisualShape, BoxShapeData, MultiBody, Color, JointType class Costmap: @@ -82,7 +81,7 @@ def visualize(self) -> None: visual_frame_position = [(box[0][0] + box[1] / 2) * self.resolution, (box[0][1] + box[2] / 2) * self.resolution, 0.] visual_shape = BoxVisualShape(Color(1, 0, 0, 0.6), visual_frame_position, box_shape_data) - visual = self.world.create_box_visual_shape(visual_shape) + visual = self.world.create_visual_shape(visual_shape) cells.append(visual) # Set to 127 for since this is the maximal amount of links in a multibody @@ -93,13 +92,15 @@ def visualize(self) -> None: link_orientations = [[0, 0, 0, 1] for c in cell_parts] link_masses = [1.0 for c in cell_parts] link_parent = [0 for c in cell_parts] - link_joints = [p.JOINT_FIXED for c in cell_parts] + link_joints = [JointType.FIXED for c in cell_parts] link_collision = [-1 for c in cell_parts] link_joint_axis = [[1, 0, 0] for c in cell_parts] - offset = [[-self.height / 2 * self.resolution, -self.width / 2 * self.resolution, 0.05], [0, 0, 0, 1]] - new_pose = p.multiplyTransforms(self.origin.position_as_list(), self.origin.orientation_as_list(), - offset[0], offset[1]) + offset = Transform([-self.height / 2 * self.resolution, -self.width / 2 * self.resolution, 0.05], + [0, 0, 0, 1]) + origin = Transform(self.origin.position_as_list(), self.origin.orientation_as_list()) + new_transform = origin * offset + new_pose = new_transform.to_pose().to_list() multi_body = MultiBody(base_visual_shape_index=-1, base_position=new_pose[0], base_orientation=new_pose[1], link_visual_shape_indices=cell_parts, link_positions=link_poses, @@ -128,8 +129,8 @@ def close_visualization(self) -> None: """ Removes the visualization from the World. """ - for id in self.vis_ids: - p.removeBody(id) + for v_id in self.vis_ids: + self.world.remove_object(v_id) self.vis_ids = [] def _find_consectuive_line(self, start: Tuple[int, int], map: np.ndarray) -> int: @@ -224,7 +225,8 @@ def __init__(self, distance_to_obstacle: float, from_ros: Optional[bool] = False, size: Optional[int] = 100, resolution: Optional[float] = 0.02, - origin: Optional[Pose] = None): + origin: Optional[Pose] = None, + world: Optional[World] = None): """ Constructor for the Occupancy costmap, the actual costmap is received from the ROS map_server and wrapped by this class. Meta-data about the @@ -246,6 +248,7 @@ def __init__(self, distance_to_obstacle: float, be in the middle of the costmap. This parameter is only used if from_ros is False. """ + self.world = world if world else World.current_world if from_ros: meta = self._get_map_metadata() self.original_map = np.reshape(self._get_map(), (meta.height, meta.width)) @@ -386,11 +389,9 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: j = 0 for n in self._chunks(np.array(rays), 16380): with UseProspectionWorld(): - r_t = p.rayTestBatch(n[:, 0], n[:, 1], numThreads=0, - physicsClientId=World.current_world.client_id) + r_t = self.world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) while r_t is None: - r_t = p.rayTestBatch(n[:, 0], n[:, 1], numThreads=0, - physicsClientId=World.current_world.client_id) + r_t = self.world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) j += len(n) if World.robot: shadow_robot = World.current_world.get_prospection_object_from_object(World.robot) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 65aedce37..8f5c70a8a 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -3,14 +3,12 @@ import dataclasses from abc import ABC, abstractmethod -from copy import copy from inspect import isgenerator, isgeneratorfunction from sqlalchemy.orm.session import Session import rospy -from .world import Object as BulletWorldObject -from pycram.bullet_world import BulletWorld +from .world import World, Object as WorldObject from .helper import GeneratorList, bcolors from threading import Lock from time import time @@ -487,9 +485,9 @@ class Action: """ def __post_init__(self): - self.robot_position = BulletWorld.robot.get_pose() - self.robot_torso_height = BulletWorld.robot.get_joint_position(robot_description.torso_joint) - self.robot_type = BulletWorld.robot.obj_type + self.robot_position = World.robot.get_pose() + self.robot_torso_height = World.robot.get_joint_position(robot_description.torso_joint) + self.robot_type = World.robot.obj_type @with_tree def perform(self) -> Any: @@ -612,20 +610,20 @@ class Object: Type of the object """ - bullet_world_object: Optional[BulletWorldObject] + world_object: Optional[WorldObject] """ - Reference to the BulletWorld object + Reference to the World object """ _pose: Optional[Callable] = dataclasses.field(init=False) """ A callable returning the pose of this object. The _pose member is used overwritten for data copies - which will not update when the original bullet_world_object is moved. + which will not update when the original world_object is moved. """ def __post_init__(self): - if self.bullet_world_object: - self._pose = self.bullet_world_object.get_pose + if self.world_object: + self._pose = self.world_object.get_pose def to_sql(self) -> ORMObjectDesignator: """ @@ -658,7 +656,7 @@ def insert(self, session: Session) -> ORMObjectDesignator: def data_copy(self) -> 'ObjectDesignatorDescription.Object': """ - :return: A copy containing only the fields of this class. The BulletWorldObject attached to this pycram + :return: A copy containing only the fields of this class. The WorldObject attached to this pycram object is not copied. The _pose gets set to a method that statically returns the pose of the object when this method was called. """ @@ -701,7 +699,7 @@ def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: :return: The adjusted grasp pose """ lt = LocalTransformer() - pose_in_object = lt.transform_pose(pose, self.bullet_world_object.tf_frame) + pose_in_object = lt.transform_pose(pose, self.world_object.tf_frame) special_knowledge = [] # Initialize as an empty list if self.obj_type in SPECIAL_KNOWLEDGE: @@ -751,7 +749,7 @@ def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] def ground(self) -> Union[Object, bool]: """ - Return the first object from the bullet world that fits the description. + Return the first object from the world that fits the description. :return: A resolved object designator """ @@ -763,8 +761,8 @@ def __iter__(self) -> Iterable[Object]: :yield: A resolved object designator """ - # for every bullet world object - for obj in BulletWorld.current_world.objects: + # for every world object + for obj in World.current_world.objects: # skip if name does not match specification if self.names and obj.name not in self.names: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 91d5fff85..142b21bdf 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1,7 +1,5 @@ -import dataclasses import itertools -import time -from typing import List, Optional, Any, Tuple, Union +from typing import Any, Union import sqlalchemy.orm @@ -12,18 +10,18 @@ from ..orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction, MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction, - Action as ORMAction, LookAtAction as ORMLookAtAction, + LookAtAction as ORMLookAtAction, DetectAction as ORMDetectAction, TransportAction as ORMTransportAction, OpenAction as ORMOpenAction, CloseAction as ORMCloseAction, GraspingAction as ORMGraspingAction) -from ..orm.base import Quaternion, Position, Base, RobotState, ProcessMetaData +from ..orm.base import Base from ..plan_failures import ObjectUnfetchable, ReachabilityFailure from ..robot_descriptions import robot_description from ..task import with_tree from ..enums import Arms from ..designator import ActionDesignatorDescription -from ..bullet_world import BulletWorld +from ..world import World from ..pose import Pose from ..helper import multiply_quaternions @@ -295,16 +293,16 @@ class Action(ActionDesignatorDescription.Action): object_at_execution: Optional[ObjectDesignatorDescription.Object] = dataclasses.field(init=False) """ The object at the time this Action got created. It is used to be a static, information holding entity. It is - not updated when the BulletWorld object is changed. + not updated when the World object is changed. """ @with_tree def perform(self) -> None: # Store the object's data copy at execution self.object_at_execution = self.object_designator.data_copy() - robot = BulletWorld.robot + robot = World.robot # Retrieve object and robot from designators - object = self.object_designator.bullet_world_object + object = self.object_designator.world_object # Get grasp orientation and target pose grasp = robot_description.grasps.get_orientation_for_grasp(self.grasp) # oTm = Object Pose in Frame map @@ -337,7 +335,7 @@ def perform(self) -> None: gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") #Perform Gripper Rotate - # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) + # World.current_world.add_vis_axis(gripper_rotate_pose) # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) @@ -345,12 +343,12 @@ def perform(self) -> None: prepose = object.local_transformer.transform_pose(oTg, "map") # Perform the motion with the prepose and open gripper - BulletWorld.current_world.add_vis_axis(prepose) + World.current_world.add_vis_axis(prepose) MoveTCPMotion(prepose, self.arm).resolve().perform() MoveGripperMotion(motion="open", gripper=self.arm).resolve().perform() # Perform the motion with the adjusted pose -> actual grasp and close gripper - BulletWorld.current_world.add_vis_axis(adjusted_oTm) + World.current_world.add_vis_axis(adjusted_oTm) MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() adjusted_oTm.pose.position.z += 0.03 MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform() @@ -358,11 +356,11 @@ def perform(self) -> None: robot.attach(object, tool_frame) # Lift object - BulletWorld.current_world.add_vis_axis(adjusted_oTm) + World.current_world.add_vis_axis(adjusted_oTm) MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() # Remove the vis axis from the world - BulletWorld.current_world.remove_vis_axis() + World.current_world.remove_vis_axis() def to_sql(self) -> ORMPickUpAction: return ORMPickUpAction(self.arm, self.grasp) @@ -432,22 +430,22 @@ class Action(ActionDesignatorDescription.Action): @with_tree def perform(self) -> None: - object_pose = self.object_designator.bullet_world_object.get_pose() + object_pose = self.object_designator.world_object.get_pose() local_tf = LocalTransformer() # Transformations such that the target position is the position of the object and not the tcp tool_name = robot_description.get_tool_frame(self.arm) tcp_to_object = local_tf.transform_pose(object_pose, - BulletWorld.robot.links[tool_name].tf_frame) + World.robot.links[tool_name].tf_frame) target_diff = self.target_location.to_transform("target").inverse_times( tcp_to_object.to_transform("object")).to_pose() MoveTCPMotion(target_diff, self.arm).resolve().perform() MoveGripperMotion("open", self.arm).resolve().perform() - BulletWorld.robot.detach(self.object_designator.bullet_world_object) + World.robot.detach(self.object_designator.world_object) retract_pose = local_tf.transform_pose( target_diff, - BulletWorld.robot.links[robot_description.get_tool_frame(self.arm)].tf_frame) + World.robot.links[robot_description.get_tool_frame(self.arm)].tf_frame) retract_pose.position.x -= 0.07 MoveTCPMotion(retract_pose, self.arm).resolve().perform() @@ -884,12 +882,12 @@ def perform(self) -> Any: if isinstance(self.object_desig, ObjectPart.Object): object_pose = self.object_desig.part_pose else: - object_pose = self.object_desig.bullet_world_object.get_pose() + object_pose = self.object_desig.world_object.get_pose() lt = LocalTransformer() gripper_name = robot_description.get_tool_frame(self.arm) object_pose_in_gripper = lt.transform_pose(object_pose, - BulletWorld.robot.links[gripper_name].tf_frame) + World.robot.links[gripper_name].tf_frame) pre_grasp = object_pose_in_gripper.copy() pre_grasp.pose.position.x -= 0.1 diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index d726f8957..8e55b147f 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -1,17 +1,14 @@ import dataclasses -import time -from typing import List, Tuple, Union, Iterable, Optional, Callable +from typing import List, Union, Iterable, Optional, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart -from ..world import Object, UseProspectionWorld -from ..bullet_world import BulletWorld +from ..world import World, UseProspectionWorld from ..world_reasoning import link_pose_for_joint_config -from ..designator import Designator, DesignatorError, LocationDesignatorDescription +from ..designator import DesignatorError, LocationDesignatorDescription from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap from ..robot_descriptions import robot_description from ..enums import JointType from ..helper import transform -from ..plan_failures import EnvironmentManipulationImpossible from ..pose_generator_and_validator import pose_generator, visibility_validator, reachability_validator, \ generate_orientation from ..robot_description import ManipulatorDescription @@ -161,7 +158,7 @@ def __iter__(self): max_height = list(robot_description.cameras.values())[0].max_height # This ensures that the costmaps always get a position as their origin. if isinstance(self.target, ObjectDesignatorDescription.Object): - target_pose = self.target.bullet_world_object.get_pose() + target_pose = self.target.world_object.get_pose() else: target_pose = self.target.copy() @@ -180,8 +177,8 @@ def __iter__(self): final_map += visible if self.visible_for or self.reachable_for: - robot_object = self.visible_for.bullet_world_object if self.visible_for else self.reachable_for.bullet_world_object - test_robot = BulletWorld.current_world.get_prospection_object_from_object(robot_object) + robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object + test_robot = World.current_world.get_prospection_object_from_object(robot_object) with UseProspectionWorld(): @@ -190,7 +187,7 @@ def __iter__(self): arms = None if self.visible_for: res = res and visibility_validator(maybe_pose, test_robot, target_pose, - BulletWorld.current_world) + World.current_world) if self.reachable_for: hand_links = [] for name, chain in robot_description.chains.items(): @@ -230,7 +227,7 @@ def __init__(self, handle_desig: ObjectPart.Object, robot_desig: ObjectDesignato """ super().__init__(resolver) self.handle: ObjectPart.Object = handle_desig - self.robot: ObjectDesignatorDescription.Object = robot_desig.bullet_world_object + self.robot: ObjectDesignatorDescription.Object = robot_desig.world_object def ground(self) -> Location: """ @@ -257,23 +254,23 @@ def __iter__(self) -> Location: final_map = occupancy + gaussian - test_robot = BulletWorld.current_world.get_prospection_object_from_object(self.robot) + test_robot = World.current_world.get_prospection_object_from_object(self.robot) # Find a Joint of type prismatic which is above the handle in the URDF tree - container_joint = self.handle.bullet_world_object.find_joint_above(self.handle.name, JointType.PRISMATIC) + container_joint = self.handle.world_object.find_joint_above(self.handle.name, JointType.PRISMATIC) - init_pose = link_pose_for_joint_config(self.handle.bullet_world_object, { - container_joint: self.handle.bullet_world_object.get_joint_limits(container_joint)[0]}, + init_pose = link_pose_for_joint_config(self.handle.world_object, { + container_joint: self.handle.world_object.get_joint_limits(container_joint)[0]}, self.handle.name) # Calculate the pose the handle would be in if the drawer was to be fully opened - goal_pose = link_pose_for_joint_config(self.handle.bullet_world_object, { - container_joint: self.handle.bullet_world_object.get_joint_limits(container_joint)[1] - 0.05}, + goal_pose = link_pose_for_joint_config(self.handle.world_object, { + container_joint: self.handle.world_object.get_joint_limits(container_joint)[1] - 0.05}, self.handle.name) # Handle position for calculating rotation of the final pose - half_pose = link_pose_for_joint_config(self.handle.bullet_world_object, { - container_joint: self.handle.bullet_world_object.get_joint_limits(container_joint)[1] / 1.5}, + half_pose = link_pose_for_joint_config(self.handle.world_object, { + container_joint: self.handle.world_object.get_joint_limits(container_joint)[1] / 1.5}, self.handle.name) with UseProspectionWorld(): @@ -336,10 +333,10 @@ def __iter__(self): :yield: An instance of SemanticCostmapLocation.Location with the found valid position of the Costmap. """ - sem_costmap = SemanticCostmap(self.part_of.bullet_world_object, self.urdf_link_name) + sem_costmap = SemanticCostmap(self.part_of.world_object, self.urdf_link_name) height_offset = 0 if self.for_object: - min_p, max_p = self.for_object.bullet_world_object.get_aabb().get_min_max_points() + min_p, max_p = self.for_object.world_object.get_aabb().get_min_max_points() height_offset = (max_p.z - min_p.z) / 2 for maybe_pose in pose_generator(sem_costmap): maybe_pose.position.z += height_offset diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index c602227c2..247f1d508 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -291,7 +291,7 @@ def __init__(self, target: Optional[Pose] = None, object: Optional[ObjectDesigna super().__init__(resolver) self.cmd: str = 'looking' self.target: Optional[Pose] = target - self.object: Object = object.bullet_world_object if object else object + self.object: Object = object.world_object if object else object def ground(self) -> Motion: """ diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 7ba1491b3..d7d28507a 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,9 +1,8 @@ import dataclasses -from typing import List, Union, Optional, Callable, Tuple, Iterable +from typing import List, Optional, Callable import sqlalchemy.orm -from ..bullet_world import BulletWorld -from pycram.world import Object as BulletWorldObject -from ..designator import DesignatorDescription, ObjectDesignatorDescription +from pycram.world import World, Object as WorldObject +from ..designator import ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) from ..pose import Pose @@ -95,9 +94,9 @@ def __iter__(self): :yield: A resolved Object designator """ for name in self.names: - if name in self.part_of.bullet_world_object.link_name_to_id.keys(): - yield self.Object(name, self.type, self.part_of.bullet_world_object, - self.part_of.bullet_world_object.links[name].pose) + if name in self.part_of.world_object.link_name_to_id.keys(): + yield self.Object(name, self.type, self.part_of.world_object, + self.part_of.world_object.links[name].pose) class LocatedObject(ObjectDesignatorDescription): @@ -137,7 +136,7 @@ class RealObject(ObjectDesignatorDescription): """ Object designator representing an object in the real world, when resolving this object designator description ] RoboKudo is queried to perceive an object fitting the given criteria. Afterward the resolver tries to match - the found object to an Object in the BulletWorld. + the found object to an Object in the World. """ @dataclasses.dataclass @@ -148,28 +147,28 @@ class Object(ObjectDesignatorDescription.Object): """ def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] = None, - bullet_world_object: BulletWorldObject = None, resolver: Optional[Callable] = None): + world_object: WorldObject = None, resolver: Optional[Callable] = None): """ :param names: :param types: - :param bullet_world_object: + :param world_object: :param resolver: """ super().__init__(resolver) self.types: Optional[List[str]] = types self.names: Optional[List[str]] = names - self.bullet_world_object: BulletWorldObject = bullet_world_object + self.world_object: WorldObject = world_object def __iter__(self): """ - Queries RoboKudo for objects that fit the description and then iterates over all BulletWorld objects that have - the same type to match a BulletWorld object to the real object. + Queries RoboKudo for objects that fit the description and then iterates over all World objects that have + the same type to match a World object to the real object. - :yield: A resolved object designator with reference bullet world object + :yield: A resolved object designator with reference world object """ object_candidates = query(self) for obj_desig in object_candidates: - for bullet_obj in BulletWorld.get_objects_by_type(obj_desig.obj_type): - obj_desig.bullet_world_object = bullet_obj + for world_obj in World.get_objects_by_type(obj_desig.obj_type): + obj_desig.world_object = world_obj yield obj_desig diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 2aa5797af..131d87124 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -1,10 +1,8 @@ import tf import numpy as np -import rospy -import pybullet as p from .world import Object -from .bullet_world import BulletWorld +from .bullet_world import World from .world_reasoning import contact from .costmaps import Costmap from .pose import Pose, Transform @@ -12,7 +10,7 @@ from .external_interfaces.ik import request_ik from .plan_failures import IKError from .helper import _apply_ik -from typing import Type, Tuple, List, Union, Dict, Iterable +from typing import Tuple, List, Union, Dict, Iterable def pose_generator(costmap: Costmap, number_of_samples=100, orientation_generator=None) -> Iterable: @@ -79,11 +77,11 @@ def generate_orientation(position: List[float], origin: Pose) -> List[float]: def visibility_validator(pose: Pose, robot: Object, object_or_pose: Union[Object, Pose], - world: BulletWorld) -> bool: + world: World) -> bool: """ This method validates if the robot can see the target position from a given pose candidate. The target position can either be a position, in world coordinate - system, or an object in the BulletWorld. The validation is done by shooting a + system, or an object in the World. The validation is done by shooting a ray from the camera to the target position and checking that it does not collide with anything else. @@ -91,23 +89,23 @@ def visibility_validator(pose: Pose, :param robot: The robot object for which this should be validated :param object_or_pose: The target position or object for which the pose candidate should be validated. - :param world: The BulletWorld instance in which this should be validated. + :param world: The World instance in which this should be validated. :return: True if the target is visible for the robot, None in any other case. """ robot_pose = robot.get_pose() - if type(object_or_pose) == Object: + if isinstance(object_or_pose, Object): robot.set_pose(pose) camera_pose = robot.links[robot_description.get_camera_frame()].pose robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - ray = p.rayTest(camera_pose.position_as_list(), object_or_pose.get_pose().position_as_list(), - physicsClientId=world.client_id) - res = ray[0][0] == object_or_pose.id + ray = world.ray_test(camera_pose.position_as_list(), object_or_pose.get_position_as_list()) + res = ray == object_or_pose.id else: robot.set_pose(pose) camera_pose = robot.links[robot_description.get_camera_frame()].pose robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - ray = p.rayTest(camera_pose.position_as_list(), object_or_pose, physicsClientId=world.client_id) - res = ray[0][0] == -1 + # TODO: Check if this is correct + ray = world.ray_test(camera_pose.position_as_list(), object_or_pose) + res = ray == -1 robot.set_pose(robot_pose) return res @@ -123,7 +121,7 @@ def reachability_validator(pose: Pose, the validator returns True and False in any other case. :param pose: The pose candidate for which the reachability should be validated - :param robot: The robot object in the BulletWorld for which the reachability + :param robot: The robot object in the World for which the reachability should be validated. :param target: The target position or object instance which should be the target for reachability. @@ -158,7 +156,7 @@ def reachability_validator(pose: Pose, resp = request_ik(target, robot, left_joints, left_gripper) _apply_ik(robot, resp, left_joints) - for obj in BulletWorld.current_world.objects: + for obj in World.current_world.objects: if obj.name == "floor": continue in_contact, contact_links = contact(robot, obj, return_links=True) @@ -182,7 +180,7 @@ def reachability_validator(pose: Pose, resp = request_ik(target, robot, right_joints, right_gripper) _apply_ik(robot, resp, right_joints) - for obj in BulletWorld.current_world.objects: + for obj in World.current_world.objects: if obj.name == "floor": continue in_contact, contact_links = contact(robot, obj, return_links=True) diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index e0173e0bc..a8185b760 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -59,7 +59,7 @@ class BoxyPickUp(ProcessModule): """ def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.bullet_world_object + object = desig.object_desig.world_object robot = BulletWorld.robot grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) target = object.get_pose() @@ -86,7 +86,7 @@ def _execute(self, desig: PlaceMotion.Motion): :param desig: A PlaceMotion :return: """ - object = desig.object.bullet_world_object + object = desig.object.world_object robot = BulletWorld.robot arm = desig.arm @@ -107,7 +107,7 @@ class BoxyOpen(ProcessModule): """ def _execute(self, desig: OpeningMotion.Motion): - part_of_object = desig.object_part.bullet_world_object + part_of_object = desig.object_part.world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -116,8 +116,8 @@ def _execute(self, desig: OpeningMotion.Motion): _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - desig.object_part.bullet_world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( + desig.object_part.world_object.set_joint_state(container_joint, + part_of_object.get_joint_limits( container_joint)[1]) @@ -126,7 +126,7 @@ class BoxyClose(ProcessModule): Low-level implementation that lets the robot close a grasped container, in simulation """ def _execute(self, desig: ClosingMotion.Motion): - part_of_object = desig.object_part.bullet_world_object + part_of_object = desig.object_part.world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -135,8 +135,8 @@ def _execute(self, desig: ClosingMotion.Motion): _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - desig.object_part.bullet_world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( + desig.object_part.world_object.set_joint_state(container_joint, + part_of_object.get_joint_limits( container_joint)[0]) diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 390558bd9..84567c3b9 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -30,7 +30,7 @@ class DefaultPickUp(ProcessModule): """ def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.bullet_world_object + object = desig.object_desig.world_object robot = BulletWorld.robot grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) target = object.get_pose() @@ -57,7 +57,7 @@ def _execute(self, desig: PlaceMotion.Motion): :param desig: A PlaceMotion :return: """ - object = desig.object.bullet_world_object + object = desig.object.world_object robot = BulletWorld.robot arm = desig.arm @@ -181,7 +181,7 @@ class DefaultOpen(ProcessModule): """ def _execute(self, desig: OpeningMotion.Motion): - part_of_object = desig.object_part.bullet_world_object + part_of_object = desig.object_part.world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -190,8 +190,8 @@ def _execute(self, desig: OpeningMotion.Motion): _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - desig.object_part.bullet_world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( + desig.object_part.world_object.set_joint_state(container_joint, + part_of_object.get_joint_limits( container_joint)[1]) @@ -200,7 +200,7 @@ class DefaultClose(ProcessModule): Low-level implementation that lets the robot close a grasped container, in simulation """ def _execute(self, desig: ClosingMotion.Motion): - part_of_object = desig.object_part.bullet_world_object + part_of_object = desig.object_part.world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -209,8 +209,8 @@ def _execute(self, desig: ClosingMotion.Motion): _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - desig.object_part.bullet_world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( + desig.object_part.world_object.set_joint_state(container_joint, + part_of_object.get_joint_limits( container_joint)[0]) diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 12bad2cf3..45cf31899 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -45,7 +45,7 @@ class DonbotPickUp(ProcessModule): """ def _execute(self, desig): - object = desig.object_desig.bullet_world_object + object = desig.object_desig.world_object robot = BulletWorld.robot grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) target = object.get_pose() @@ -65,7 +65,7 @@ class DonbotPlace(ProcessModule): """ def _execute(self, desig): - object = desig.object.bullet_world_object + object = desig.object.world_object robot = BulletWorld.robot # Transformations such that the target position is the position of the object and not the tcp diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 02ca0edcf..5bd04c89e 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -1,8 +1,10 @@ from threading import Lock +from typing import Optional from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..bullet_world import BulletWorld +from ..world import World +from ..pose import Pose, Point from ..helper import _apply_ik import pycram.world_reasoning as btr import pybullet as p @@ -10,14 +12,26 @@ import time +def calculate_and_apply_ik(robot, gripper: str, target_position: Point, max_iterations: Optional[int] = None): + """ + Calculates the inverse kinematics for the given target pose and applies it to the robot. + """ + inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), target_position, + maxNumIterations=max_iterations) + # TODO: Check if this is correct (getting the arm and using its joints), previously joints was not provided. + arm = "right" if gripper == robot_description.get_tool_frame("right") else "left" + joints = robot_description.chains[arm].joints + _apply_ik(robot, inv, joints) + + def _park_arms(arm): """ Defines the joint poses for the parking positions of the arm of HSR and applies them to the - in the BulletWorld defined robot. + in the World defined robot. :return: None """ - robot = BulletWorld.robot + robot = World.robot if arm == "left": for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): robot.set_joint_position(joint, pose) @@ -31,8 +45,8 @@ class HSRNavigation(ProcessModule): def _execute(self, desig): solution = desig.reference() if solution['cmd'] == 'navigate': - robot = BulletWorld.robot - robot.set_position_and_orientation(solution['target'], solution['orientation']) + robot = World.robot + robot.set_pose(Pose(solution['target'], solution['orientation'])) class HSRPickUp(ProcessModule): @@ -44,13 +58,13 @@ class HSRPickUp(ProcessModule): def _execute(self, desig): solution = desig.reference() if solution['cmd'] == 'pick': - object = solution['object'] - robot = BulletWorld.robot - target = object.get_position() - inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(solution['gripper']), target, - maxNumIterations=100) - _apply_ik(robot, inv) - robot.attach(object, solution['gripper']) + obj = solution['object'] + robot = World.robot + target = obj.get_position() + + calculate_and_apply_ik(robot, solution['gripper'], target, 100) + + robot.attach(obj, solution['gripper']) time.sleep(0.5) @@ -62,12 +76,10 @@ class HSRPlace(ProcessModule): def _execute(self, desig): solution = desig.reference() if solution['cmd'] == 'place': - object = solution['object'] - robot = BulletWorld.robot - inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(solution['gripper']), solution['target'], - maxNumIterations=100) - _apply_ik(robot, inv) - robot.detach(object) + obj = solution['object'] + robot = World.robot + calculate_and_apply_ik(robot, solution['gripper'], solution['target'], 100) + robot.detach(obj) time.sleep(0.5) @@ -83,19 +95,16 @@ def _execute(self, desig): solution = desig.reference() if solution['cmd'] == 'access': kitchen = solution['part_of'] - robot = BulletWorld.robot + robot = World.robot gripper = solution['gripper'] drawer_handle = solution['drawer_handle'] drawer_joint = solution['drawer_joint'] dis = solution['distance'] - inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), - kitchen.links[drawer_handle].position) - _apply_ik(robot, inv) + calculate_and_apply_ik(robot, gripper, kitchen.links[drawer_handle].position) time.sleep(0.2) han_pose = kitchen.links[drawer_handle].position - new_p = [han_pose[0] - dis, han_pose[1], han_pose[2]] - inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), new_p) - _apply_ik(robot, inv) + new_p = Point(han_pose[0] - dis, han_pose[1], han_pose[2]) + calculate_and_apply_ik(robot, gripper, new_p) kitchen.set_joint_position(drawer_joint, 0.3) time.sleep(0.5) @@ -123,7 +132,7 @@ def _execute(self, desig): if solutions['cmd'] == 'looking': target = solutions['target'] if target == 'forward' or target == 'down': - robot = BulletWorld.robot + robot = World.robot for joint, state in robot_description.get_static_joint_chain("neck", target).items(): robot.set_joint_position(joint, state) else: @@ -139,7 +148,7 @@ class HSRMoveGripper(ProcessModule): def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "move-gripper": - robot = BulletWorld.robot + robot = World.robot gripper = solution['gripper'] motion = solution['motion'] for joint, state in robot_description.get_static_gripper_chain(gripper, motion).items(): @@ -156,12 +165,12 @@ class HSRDetecting(ProcessModule): def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "detecting": - robot = BulletWorld.robot + robot = World.robot object_type = solution['object_type'] cam_frame_name = solution['cam_frame'] front_facing_axis = solution['front_facing_axis'] - objects = BulletWorld.current_world.get_objects_by_type(object_type) + objects = World.current_world.get_objects_by_type(object_type) for obj in objects: if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis, 0.5): return obj @@ -177,9 +186,8 @@ def _execute(self, desig): if solution['cmd'] == "move-tcp": target = solution['target'] gripper = solution['gripper'] - robot = BulletWorld.robot - inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), target) - _apply_ik(robot, inv) + robot = World.robot + calculate_and_apply_ik(robot, gripper, target) time.sleep(0.5) @@ -192,7 +200,7 @@ class HSRMoveJoints(ProcessModule): def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "move-arm-joints": - robot = BulletWorld.robot + robot = World.robot left_arm_poses = solution['left_arm_poses'] if type(left_arm_poses) == dict: @@ -213,7 +221,7 @@ def _execute(self, desig): solution = desig.reference() if solution['cmd'] == "world-state-detecting": obj_type = solution['object_type'] - return list(filter(lambda obj: obj.obj_type == obj_type, BulletWorld.current_world.objects))[0] + return list(filter(lambda obj: obj.obj_type == obj_type, World.current_world.objects))[0] class HSRManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 5f432560b..88b5a1b6d 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -56,7 +56,7 @@ class Pr2PickUp(ProcessModule): """ def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.bullet_world_object + object = desig.object_desig.world_object robot = BulletWorld.robot grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) target = object.get_pose() @@ -83,7 +83,7 @@ def _execute(self, desig: PlaceMotion.Motion): :param desig: A PlaceMotion :return: """ - object = desig.object.bullet_world_object + object = desig.object.world_object robot = BulletWorld.robot arm = desig.arm @@ -208,7 +208,7 @@ class Pr2Open(ProcessModule): """ def _execute(self, desig: OpeningMotion.Motion): - part_of_object = desig.object_part.bullet_world_object + part_of_object = desig.object_part.world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -217,8 +217,8 @@ def _execute(self, desig: OpeningMotion.Motion): _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - desig.object_part.bullet_world_object.set_joint_position(container_joint, - part_of_object.get_joint_limits( + desig.object_part.world_object.set_joint_position(container_joint, + part_of_object.get_joint_limits( container_joint)[1]) @@ -228,7 +228,7 @@ class Pr2Close(ProcessModule): """ def _execute(self, desig: ClosingMotion.Motion): - part_of_object = desig.object_part.bullet_world_object + part_of_object = desig.object_part.world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -237,8 +237,8 @@ def _execute(self, desig: ClosingMotion.Motion): _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - desig.object_part.bullet_world_object.set_joint_position(container_joint, - part_of_object.get_joint_limits( + desig.object_part.world_object.set_joint_position(container_joint, + part_of_object.get_joint_limits( container_joint)[0]) diff --git a/src/pycram/world.py b/src/pycram/world.py index 4113dd1cb..4f8222036 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -45,8 +45,8 @@ class World(ABC): current_world: World = None """ Global reference to the currently used World, usually this is the - graphical one. However, if you are inside a Use_shadow_world() environment the current_world points to the - shadow world. In this way you can comfortably use the current_world, which should point towards the World + graphical one. However, if you are inside a UseProspectionWorld() environment the current_world points to the + prospection world. In this way you can comfortably use the current_world, which should point towards the World used at the moment. """ @@ -533,7 +533,7 @@ def get_manipulation_event(self) -> Event: @abstractmethod def set_realtime(self, real_time: bool) -> None: """ - Enables the real time simulation of Physic in the BulletWorld. By default, this is disabled and Physics is only + Enables the real time simulation of Physics in the World. By default, this is disabled and Physics is only simulated to reason about it. :param real_time: Whether the World should simulate Physics in real time. @@ -579,7 +579,7 @@ def robot_is_set() -> bool: def exit(self, wait_time_before_exit_in_secs: Optional[float] = None) -> None: """ - Closes the World as well as the shadow world, also collects any other thread that is running. + Closes the World as well as the prospection world, also collects any other thread that is running. """ if wait_time_before_exit_in_secs is not None: time.sleep(wait_time_before_exit_in_secs) @@ -727,7 +727,7 @@ def register_two_objects_collision_callbacks(self, @classmethod def add_resource_path(cls, path: str) -> None: """ - Adds a resource path in which the BulletWorld will search for files. This resource directory is searched if an + Adds a resource path in which the World will search for files. This resource directory is searched if an Object is spawned only with a filename. :param path: A path in the filesystem in which to search for files. @@ -767,7 +767,7 @@ def get_object_from_prospection_object(self, prospection_object: Object) -> Obje try: return list(object_map.keys())[list(object_map.values()).index(prospection_object)] except ValueError: - raise ValueError("The given object is not in the shadow world.") + raise ValueError("The given object is not in the prospection world.") def reset_world(self, remove_saved_states=True) -> None: """ @@ -796,6 +796,30 @@ def update_transforms_for_objects_in_current_world(self) -> None: for obj in list(self.current_world.objects): obj.update_link_transforms(curr_time) + @abstractmethod + def ray_test(self, from_position: List[float], to_position: List[float]) -> int: + """ Cast a ray and return the first object hit, if any. + + param from_position: The starting position of the ray in Cartesian world coordinates. + param to_position: The ending position of the ray in Cartesian world coordinates. + return: The object id of the first object hit, or -1 if no object was hit. + """ + pass + + @abstractmethod + def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], + num_threads: int = 1) -> List[int]: + """ Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) + Takes optional argument num_threads to specify the number of threads to use + to compute the ray intersections for the batch. Specify 0 to let simulator decide, 1 (default) for single + core execution, 2 or more to select the number of threads to use. + + param from_positions: The starting positions of the rays in Cartesian world coordinates. + param to_positions: The ending positions of the rays in Cartesian world coordinates. + param num_threads: The number of threads to use to compute the ray intersections for the batch. + """ + pass + def create_visual_shape(self, visual_shape: VisualShape) -> int: raise NotImplementedError @@ -871,7 +895,7 @@ def __init__(self, world: World, prospection_world: World): self.add_obj_queue: Queue = Queue() self.remove_obj_queue: Queue = Queue() self.pause_sync: bool = False - # Maps bullet to prospection world objects + # Maps world to prospection world objects self.object_mapping: Dict[Object, Object] = {} self.equal_states = False @@ -1239,7 +1263,7 @@ def attach(self, saving the transformation between the given link, if there is one, and the base pose of the other object. Additionally, the name of the link, to which the object is attached, will be saved. - Furthermore, a constraint of pybullet will be created so the attachment + Furthermore, a simulator constraint will be created so the attachment also works while simulation. Loose attachments means that the attachment will only be one-directional. For example, if this object moves the other, attached, object will also move but not the other way around. @@ -1263,7 +1287,7 @@ def detach(self, child_object: Object) -> None: """ Detaches another object from this object. This is done by deleting the attachment from the attachments dictionary of both objects - and deleting the constraint of pybullet. + and deleting the constraint of the simulator. Afterward the detachment event of the corresponding World will be fired. :param child_object: The object which should be detached @@ -1462,7 +1486,7 @@ def _get_link_name_to_id_map(self) -> Dict[str, int]: def get_joint_id(self, name: str) -> int: """ - Returns the unique id for a joint name. As used by PyBullet. + Returns the unique id for a joint name. As used by the world/simulator. :param name: The joint name :return: The unique id @@ -1502,9 +1526,9 @@ def get_link_by_id(self, link_id: int) -> Link: def get_joint_by_id(self, joint_id: int) -> str: """ - Returns the joint name for a unique PyBullet id + Returns the joint name for a unique world id - :param joint_id: The Pybullet id of for joint + :param joint_id: The world id of for joint :return: The joint name """ return dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys()))[joint_id] @@ -1564,8 +1588,7 @@ def get_joint_position(self, joint_name: str) -> float: def contact_points(self) -> List: """ - Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ + Returns a list of contact points of this Object with other Objects. :return: A list of all contact points with other objects """ @@ -1574,8 +1597,6 @@ def contact_points(self) -> List: def contact_points_simulated(self) -> List: """ Returns a list of all contact points between this Object and other Objects after stepping the simulation once. - For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ :return: A list of contact points between this Object and other Objects """ @@ -1935,8 +1956,8 @@ def _is_cached(path: str, name: str, cach_dir: str) -> bool: def _correct_urdf_string(urdf_string: str) -> str: """ - Changes paths for files in the URDF from ROS paths to paths in the file system. Since PyBullet can't deal with ROS - package paths. + Changes paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legac) + can't deal with ROS package paths. :param urdf_string: The name of the URDf on the parameter server :return: The URDF string with paths in the filesystem instead of ROS packages @@ -1957,7 +1978,7 @@ def _correct_urdf_string(urdf_string: str) -> str: def fix_missing_inertial(urdf_string: str) -> str: """ Insert inertial tags for every URDF link that has no inertia. - This is used to prevent PyBullet from dumping warnings in the terminal + This is used to prevent Legacy(PyBullet) from dumping warnings in the terminal :param urdf_string: The URDF description as string :returns: The new, corrected URDF description as string. diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 8c22c7fa9..5c4e20c9e 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -174,15 +174,13 @@ class SphereShapeData(ShapeData): @dataclass -class CapsuleShapeData(ShapeData): - radius: float +class CapsuleShapeData(SphereShapeData): length: float @dataclass -class CylinderShapeData(ShapeData): - radius: float - length: float +class CylinderShapeData(CapsuleShapeData): + pass @dataclass @@ -201,6 +199,7 @@ class VisualShape: rgba_color: Color visual_frame_position: List[float] shape_data: ShapeData + visual_geometry_type: Shape @dataclass @@ -216,14 +215,13 @@ class SphereVisualShape(VisualShape): @dataclass -class CapsuleVisualShape(VisualShape): +class CapsuleVisualShape(SphereVisualShape): shape_data: CapsuleShapeData visual_geometry_type = Shape.CAPSULE @dataclass -class CylinderVisualShape(VisualShape): - shape_data: CylinderShapeData +class CylinderVisualShape(CapsuleVisualShape): visual_geometry_type = Shape.CYLINDER diff --git a/test/test_action_designator.py b/test/test_action_designator.py index b0bc2f62b..d6867b6f5 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -64,7 +64,7 @@ def test_pick_up(self): action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() action_designator.MoveTorsoAction.Action(0.3).perform() description.resolve().perform() - self.assertTrue(object_description.resolve().bullet_world_object in self.robot.attachments.keys()) + self.assertTrue(object_description.resolve().world_object in self.robot.attachments.keys()) def test_place(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) @@ -75,7 +75,7 @@ def test_place(self): action_designator.MoveTorsoAction.Action(0.3).perform() action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() description.resolve().perform() - self.assertFalse(object_description.resolve().bullet_world_object in self.robot.attachments.keys()) + self.assertFalse(object_description.resolve().world_object in self.robot.attachments.keys()) def test_look_at(self): description = action_designator.LookAtAction([Pose([1, 0, 1])]) @@ -94,7 +94,7 @@ def test_detect(self): detected_object = description.resolve().perform() self.assertEqual(detected_object.name, "milk") self.assertEqual(detected_object.obj_type, ObjectType.MILK) - self.assertEqual(detected_object.bullet_world_object, self.milk) + self.assertEqual(detected_object.world_object, self.milk) # Skipped since open and close work only in the apartment at the moment @unittest.skip From 6c4e42cb6a4817001c22490f4f61de9b29713a6d Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jan 2024 14:41:58 +0100 Subject: [PATCH 36/69] [Abstract World] Corrected setting position of object to allow lists. --- src/pycram/world.py | 8 +++++--- test/test_object.py | 4 ++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 4f8222036..c1c41b206 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -1443,11 +1443,13 @@ def set_position(self, position: Union[Pose, Point], base=False) -> None: pose.frame = position.frame elif isinstance(position, Point): target_position = position + elif isinstance(position, list): + target_position = position else: - raise TypeError("The position has to be either a Pose or a Point") + raise TypeError("The given position has to be a Pose, Point or a list of xyz.") - pose.pose.position = target_position - pose.pose.orientation = self.get_orientation() + pose.position = target_position + pose.orientation = self.get_orientation() self.set_pose(pose, base=base) def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: diff --git a/test/test_object.py b/test/test_object.py index 254495c5f..43100b8ce 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -17,6 +17,10 @@ def test_set_position_as_pose(self): self.milk.set_position(Pose([1, 2, 3])) self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) + def test_set_position_as_list(self): + self.milk.set_position([1, 2, 3]) + self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) + def test_save_state(self): self.robot.attach(self.milk) self.robot.save_state(1) From 11fbdc443a5f08f8fc83c2c08479b921db42de20 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jan 2024 15:59:45 +0100 Subject: [PATCH 37/69] [Abstract World] Corrected setting position of object to allow lists and points. --- src/pycram/pose.py | 3 ++- test/test_bullet_world.py | 1 - test/test_object.py | 6 +----- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/pycram/pose.py b/src/pycram/pose.py index cd30c2d4b..3a6cf54bc 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -100,7 +100,8 @@ def position(self, value) -> None: :param value: List or geometry_msgs/Pose message for the position """ - if not type(value) == list and not type(value) == tuple and not type(value) == GeoPose: + if (not type(value) == list and not type(value) == tuple and not type(value) == GeoPose + and not type(value) == Point): print(type(value)) rospy.logwarn("Position can only be a list or geometry_msgs/Pose") return diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index f60e42a79..881412e1c 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -20,7 +20,6 @@ def test_object_movement(self): def test_robot_orientation(self): self.robot.set_pose(Pose([0, 1, 1])) head_position = self.robot.links['head_pan_link'].position.z - #self.robot.set_orientation(list(2*tf.transformations.quaternion_from_euler(0, 0, np.pi, axes="sxyz"))) self.robot.set_orientation(Pose(orientation=[0, 0, 1, 1])) self.assertEqual(self.robot.links['head_pan_link'].position.z, head_position) diff --git a/test/test_object.py b/test/test_object.py index 43100b8ce..ece06cc46 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -6,11 +6,7 @@ class TestObject(test_bullet_world.BulletWorldTest): def test_set_position_as_point(self): - p = Point() - p.x = 1 - p.y = 2 - p.z = 3 - self.milk.set_position(p) + self.milk.set_position(Point(1, 2, 3)) self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) def test_set_position_as_pose(self): From f139b2a994a8ac2c2225b90effe7f852b6d34f8f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jan 2024 16:17:48 +0100 Subject: [PATCH 38/69] [Abstract World] updated method names in bullet_world.ipynb --- doc/source/notebooks/bullet_world.ipynb | 178 ++++++++++++++++-------- 1 file changed, 119 insertions(+), 59 deletions(-) diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb index 39a9e1d8f..a2ce21a1d 100644 --- a/doc/source/notebooks/bullet_world.ipynb +++ b/doc/source/notebooks/bullet_world.ipynb @@ -17,8 +17,8 @@ "id": "23bbba35", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:27:48.492859720Z", - "start_time": "2023-11-30T09:27:47.729848173Z" + "end_time": "2024-01-25T15:12:30.985683070Z", + "start_time": "2024-01-25T15:12:30.145357542Z" } }, "outputs": [ @@ -31,12 +31,47 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "/home/bassioun/cram_ws/src/pycram/src/pycram/orm/action_designator.py:10: SAWarning: Implicitly combining column Designator.dtype with column Action.dtype under attribute 'dtype'. Please configure one or more attributes for these same-named columns explicitly.\n", - " class Action(MapperArgsMixin, Designator):\n", - "/home/bassioun/cram_ws/src/pycram/src/pycram/orm/motion_designator.py:16: SAWarning: Implicitly combining column Designator.dtype with column Motion.dtype under attribute 'dtype'. Please configure one or more attributes for these same-named columns explicitly.\n", - " class Motion(MapperArgsMixin, Designator):\n", - "[WARN] [1701336468.169695]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1701336468.173591]: Failed to import Giskard messages\n" + "[WARN] [1706195550.581271]: Failed to import Giskard messages\n", + "[WARN] [1706195550.584688]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "Exception in thread Thread-11:\n", + "Traceback (most recent call last):\n", + " File \"/usr/lib/python3.8/threading.py\", line 932, in _bootstrap_inner\n", + " self.run()\n", + " File \"/home/bassioun/cram_ws/src/pycram/src/pycram/bullet_world.py\", line 527, in run\n", + " keys = p.getKeyboardEvents(physicalClientId=self.world.client_id)\n", + "TypeError: 'physicalClientId' is an invalid keyword argument for this function\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "startThreads creating 1 threads.\n", + "starting thread 0\n", + "started thread 0 \n", + "argc=2\n", + "argv[0] = --unused\n", + "argv[1] = --start_demo_name=Physics Server\n", + "ExampleBrowserThreadFunc started\n", + "X11 functions dynamically loaded using dlopen/dlsym OK!\n", + "X11 functions dynamically loaded using dlopen/dlsym OK!\n", + "Creating context\n", + "Created GL 3.3 context\n", + "Direct GLX rendering context obtained\n", + "Making context current\n", + "GL_VENDOR=NVIDIA Corporation\n", + "GL_RENDERER=NVIDIA GeForce RTX 4070/PCIe/SSE2\n", + "GL_VERSION=3.3.0 NVIDIA 535.154.05\n", + "GL_SHADING_LANGUAGE_VERSION=3.30 NVIDIA via Cg compiler\n", + "pthread_getconcurrency()=0\n", + "Version = 3.3.0 NVIDIA 535.154.05\n", + "Vendor = NVIDIA Corporation\n", + "Renderer = NVIDIA GeForce RTX 4070/PCIe/SSE2\n", + "b3Printf: Selected demo: Physics Server\n", + "startThreads creating 1 threads.\n", + "starting thread 0\n", + "started thread 0 \n", + "MotionThreadFunc thread started\n" ] } ], @@ -73,12 +108,12 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 18, "id": "b1e6ed82", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T08:32:14.940359764Z", - "start_time": "2023-11-30T08:32:14.691411659Z" + "end_time": "2024-01-25T15:15:13.021035123Z", + "start_time": "2024-01-25T15:15:12.770510755Z" } }, "outputs": [], @@ -100,8 +135,8 @@ "id": "48a3aed2", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:27:52.209614939Z", - "start_time": "2023-11-30T09:27:52.159974972Z" + "end_time": "2024-01-25T15:12:45.684564926Z", + "start_time": "2024-01-25T15:12:45.625809554Z" } }, "outputs": [], @@ -135,8 +170,8 @@ "id": "4ae2bc42", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:27:57.498773893Z", - "start_time": "2023-11-30T09:27:57.497252820Z" + "end_time": "2024-01-25T15:12:49.219929668Z", + "start_time": "2024-01-25T15:12:49.218144234Z" } }, "outputs": [], @@ -150,8 +185,8 @@ "id": "4adc7b11", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:27:57.867485393Z", - "start_time": "2023-11-30T09:27:57.865626321Z" + "end_time": "2024-01-25T15:12:51.501613968Z", + "start_time": "2024-01-25T15:12:51.498849668Z" } }, "outputs": [], @@ -165,8 +200,8 @@ "id": "f91d1809", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:27:58.203719277Z", - "start_time": "2023-11-30T09:27:58.201735028Z" + "end_time": "2024-01-25T15:12:53.093477139Z", + "start_time": "2024-01-25T15:12:53.091079615Z" } }, "outputs": [], @@ -188,8 +223,8 @@ "id": "1db2aa78", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:27:59.859274393Z", - "start_time": "2023-11-30T09:27:59.856354972Z" + "end_time": "2024-01-25T15:12:55.623481479Z", + "start_time": "2024-01-25T15:12:55.617521322Z" } }, "outputs": [ @@ -210,8 +245,8 @@ "header: \n", " seq: 0\n", " stamp: \n", - " secs: 1701336478\n", - " nsecs: 200602293\n", + " secs: 1706195573\n", + " nsecs: 89663267\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -251,8 +286,8 @@ "id": "10a493b1", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:28:02.809496385Z", - "start_time": "2023-11-30T09:28:02.808590814Z" + "end_time": "2024-01-25T15:13:00.617061159Z", + "start_time": "2024-01-25T15:13:00.545727421Z" } }, "outputs": [], @@ -266,8 +301,8 @@ "id": "275372e3", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:28:03.828259221Z", - "start_time": "2023-11-30T09:28:03.825819607Z" + "end_time": "2024-01-25T15:13:02.886236097Z", + "start_time": "2024-01-25T15:13:02.883330740Z" } }, "outputs": [], @@ -289,8 +324,8 @@ "id": "dff85834", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:28:05.435992616Z", - "start_time": "2023-11-30T09:28:05.433266300Z" + "end_time": "2024-01-25T15:13:05.361953697Z", + "start_time": "2024-01-25T15:13:05.354577792Z" } }, "outputs": [], @@ -312,11 +347,19 @@ "id": "b0bba145", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:28:06.817969292Z", - "start_time": "2023-11-30T09:28:06.815384873Z" + "end_time": "2024-01-25T15:13:08.363391145Z", + "start_time": "2024-01-25T15:13:08.361381235Z" } }, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Removing constraint with id: 1\n" + ] + } + ], "source": [ "cereal.detach(milk)" ] @@ -338,8 +381,8 @@ "id": "edf5cb72", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:28:13.754327852Z", - "start_time": "2023-11-30T09:28:10.551209150Z" + "end_time": "2024-01-25T15:13:17.474926968Z", + "start_time": "2024-01-25T15:13:13.769261909Z" } }, "outputs": [ @@ -347,7 +390,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "{'base_link': 0, 'base_bellow_link': 1, 'base_laser_link': 2, 'fl_caster_rotation_link': 3, 'fl_caster_l_wheel_link': 4, 'fl_caster_r_wheel_link': 5, 'fr_caster_rotation_link': 6, 'fr_caster_l_wheel_link': 7, 'fr_caster_r_wheel_link': 8, 'bl_caster_rotation_link': 9, 'bl_caster_l_wheel_link': 10, 'bl_caster_r_wheel_link': 11, 'br_caster_rotation_link': 12, 'br_caster_l_wheel_link': 13, 'br_caster_r_wheel_link': 14, 'torso_lift_link': 15, 'l_torso_lift_side_plate_link': 16, 'r_torso_lift_side_plate_link': 17, 'imu_link': 18, 'head_pan_link': 19, 'head_tilt_link': 20, 'head_plate_frame': 21, 'sensor_mount_link': 22, 'high_def_frame': 23, 'high_def_optical_frame': 24, 'double_stereo_link': 25, 'wide_stereo_link': 26, 'wide_stereo_optical_frame': 27, 'wide_stereo_l_stereo_camera_frame': 28, 'wide_stereo_l_stereo_camera_optical_frame': 29, 'wide_stereo_r_stereo_camera_frame': 30, 'wide_stereo_r_stereo_camera_optical_frame': 31, 'narrow_stereo_link': 32, 'narrow_stereo_optical_frame': 33, 'narrow_stereo_l_stereo_camera_frame': 34, 'narrow_stereo_l_stereo_camera_optical_frame': 35, 'narrow_stereo_r_stereo_camera_frame': 36, 'narrow_stereo_r_stereo_camera_optical_frame': 37, 'projector_wg6802418_frame': 38, 'projector_wg6802418_child_frame': 39, 'laser_tilt_mount_link': 40, 'laser_tilt_link': 41, 'r_shoulder_pan_link': 42, 'r_shoulder_lift_link': 43, 'r_upper_arm_roll_link': 44, 'r_upper_arm_link': 45, 'r_elbow_flex_link': 46, 'r_forearm_roll_link': 47, 'r_forearm_link': 48, 'r_wrist_flex_link': 49, 'r_wrist_roll_link': 50, 'r_gripper_palm_link': 51, 'r_gripper_led_frame': 52, 'r_gripper_motor_accelerometer_link': 53, 'r_gripper_tool_frame': 54, 'r_gripper_motor_slider_link': 55, 'r_gripper_motor_screw_link': 56, 'r_gripper_l_finger_link': 57, 'r_gripper_l_finger_tip_link': 58, 'r_gripper_r_finger_link': 59, 'r_gripper_r_finger_tip_link': 60, 'r_gripper_l_finger_tip_frame': 61, 'r_forearm_cam_frame': 62, 'r_forearm_cam_optical_frame': 63, 'l_shoulder_pan_link': 64, 'l_shoulder_lift_link': 65, 'l_upper_arm_roll_link': 66, 'l_upper_arm_link': 67, 'l_elbow_flex_link': 68, 'l_forearm_roll_link': 69, 'l_forearm_link': 70, 'l_wrist_flex_link': 71, 'l_wrist_roll_link': 72, 'l_gripper_palm_link': 73, 'l_gripper_led_frame': 74, 'l_gripper_motor_accelerometer_link': 75, 'l_gripper_tool_frame': 76, 'l_gripper_motor_slider_link': 77, 'l_gripper_motor_screw_link': 78, 'l_gripper_l_finger_link': 79, 'l_gripper_l_finger_tip_link': 80, 'l_gripper_r_finger_link': 81, 'l_gripper_r_finger_tip_link': 82, 'l_gripper_l_finger_tip_frame': 83, 'l_forearm_cam_frame': 84, 'l_forearm_cam_optical_frame': 85, 'torso_lift_motor_screw_link': 86, 'base_footprint': -1}\n" + "{'base_link': , 'base_footprint': , 'base_bellow_link': , 'base_laser_link': , 'fl_caster_rotation_link': , 'fl_caster_l_wheel_link': , 'fl_caster_r_wheel_link': , 'fr_caster_rotation_link': , 'fr_caster_l_wheel_link': , 'fr_caster_r_wheel_link': , 'bl_caster_rotation_link': , 'bl_caster_l_wheel_link': , 'bl_caster_r_wheel_link': , 'br_caster_rotation_link': , 'br_caster_l_wheel_link': , 'br_caster_r_wheel_link': , 'torso_lift_link': , 'l_torso_lift_side_plate_link': , 'r_torso_lift_side_plate_link': , 'torso_lift_motor_screw_link': , 'imu_link': , 'head_pan_link': , 'head_tilt_link': , 'head_plate_frame': , 'sensor_mount_link': , 'high_def_frame': , 'high_def_optical_frame': , 'double_stereo_link': , 'wide_stereo_link': , 'wide_stereo_optical_frame': , 'wide_stereo_l_stereo_camera_frame': , 'wide_stereo_l_stereo_camera_optical_frame': , 'wide_stereo_r_stereo_camera_frame': , 'wide_stereo_r_stereo_camera_optical_frame': , 'narrow_stereo_link': , 'narrow_stereo_optical_frame': , 'narrow_stereo_l_stereo_camera_frame': , 'narrow_stereo_l_stereo_camera_optical_frame': , 'narrow_stereo_r_stereo_camera_frame': , 'narrow_stereo_r_stereo_camera_optical_frame': , 'projector_wg6802418_frame': , 'projector_wg6802418_child_frame': , 'laser_tilt_mount_link': , 'laser_tilt_link': , 'r_shoulder_pan_link': , 'r_shoulder_lift_link': , 'r_upper_arm_roll_link': , 'r_upper_arm_link': , 'r_forearm_roll_link': , 'r_elbow_flex_link': , 'r_forearm_link': , 'r_wrist_flex_link': , 'r_wrist_roll_link': , 'r_gripper_palm_link': , 'r_gripper_led_frame': , 'r_gripper_motor_accelerometer_link': , 'r_gripper_tool_frame': , 'r_gripper_motor_slider_link': , 'r_gripper_motor_screw_link': , 'r_gripper_l_finger_link': , 'r_gripper_r_finger_link': , 'r_gripper_l_finger_tip_link': , 'r_gripper_r_finger_tip_link': , 'r_gripper_l_finger_tip_frame': , 'l_shoulder_pan_link': , 'l_shoulder_lift_link': , 'l_upper_arm_roll_link': , 'l_upper_arm_link': , 'l_forearm_roll_link': , 'l_elbow_flex_link': , 'l_forearm_link': , 'l_wrist_flex_link': , 'l_wrist_roll_link': , 'l_gripper_palm_link': , 'l_gripper_led_frame': , 'l_gripper_motor_accelerometer_link': , 'l_gripper_tool_frame': , 'l_gripper_motor_slider_link': , 'l_gripper_motor_screw_link': , 'l_gripper_l_finger_link': , 'l_gripper_r_finger_link': , 'l_gripper_l_finger_tip_link': , 'l_gripper_r_finger_tip_link': , 'l_gripper_l_finger_tip_frame': , 'l_forearm_cam_frame': , 'l_forearm_cam_optical_frame': , 'r_forearm_cam_frame': , 'r_forearm_cam_optical_frame': }\n" ] } ], @@ -370,8 +413,8 @@ "id": "51b59ffd", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:28:17.138722982Z", - "start_time": "2023-11-30T09:28:17.136550048Z" + "end_time": "2024-01-25T15:13:25.916641115Z", + "start_time": "2024-01-25T15:13:25.914531215Z" } }, "outputs": [ @@ -392,8 +435,8 @@ "header: \n", " seq: 0\n", " stamp: \n", - " secs: 1701336493\n", - " nsecs: 695036649\n", + " secs: 1706195605\n", + " nsecs: 914234161\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -432,12 +475,12 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 14, "id": "38a8aef4", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:28:19.963901990Z", - "start_time": "2023-11-30T09:28:19.914673646Z" + "end_time": "2024-01-25T15:13:51.297038606Z", + "start_time": "2024-01-25T15:13:51.284471328Z" } }, "outputs": [ @@ -454,11 +497,11 @@ "source": [ "print(f\"Joint limits: {pr2.get_joint_limits('torso_lift_joint')}\")\n", "\n", - "print(f\"Current Joint state: {pr2.get_joint_state('torso_lift_joint')}\")\n", + "print(f\"Current Joint state: {pr2.get_joint_position('torso_lift_joint')}\")\n", "\n", - "pr2.set_joint_state(\"torso_lift_joint\", 0.2)\n", + "pr2.set_joint_position(\"torso_lift_joint\", 0.2)\n", "\n", - "print(f\"New Joint state: {pr2.get_joint_state('torso_lift_joint')}\")" + "print(f\"New Joint state: {pr2.get_joint_position('torso_lift_joint')}\")" ] }, { @@ -472,12 +515,12 @@ }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 15, "id": "5eee9b8a", "metadata": { "ExecuteTime": { - "end_time": "2023-11-30T09:28:22.289708761Z", - "start_time": "2023-11-30T09:28:22.287988559Z" + "end_time": "2024-01-25T15:14:04.637139529Z", + "start_time": "2024-01-25T15:14:04.594359252Z" } }, "outputs": [ @@ -485,7 +528,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Pr2 forearm color: (0.699999988079071, 0.699999988079071, 0.699999988079071, 1.0)\n" + "Pr2 forearm color: Color(R=0.699999988079071, G=0.699999988079071, B=0.699999988079071, A=1.0)\n" ] } ], @@ -495,9 +538,14 @@ }, { "cell_type": "code", - "execution_count": 18, + "execution_count": 16, "id": "07aa1c3f", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T15:14:13.906293669Z", + "start_time": "2024-01-25T15:14:13.900095646Z" + } + }, "outputs": [], "source": [ "pr2.set_color([1, 0, 0], \"r_forearm_link\")" @@ -513,25 +561,37 @@ }, { "cell_type": "code", - "execution_count": 19, + "execution_count": 17, "id": "2a78715b", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T15:14:50.452351191Z", + "start_time": "2024-01-25T15:14:50.437446512Z" + } + }, "outputs": [ { "data": { - "text/plain": [ - "((-0.0015000000000000005, -0.0015000000000000005, 0.06949999999999999),\n", - " (0.0015000000000000005, 0.0015000000000000005, 0.0725))" - ] + "text/plain": "AxisAlignedBoundingBox(min_x=-0.0015000000000000005, min_y=-0.0015000000000000005, min_z=0.06949999999999999, max_x=0.0015000000000000005, max_y=0.0015000000000000005, max_z=0.0725)" }, - "execution_count": 19, + "execution_count": 17, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "pr2.get_AABB()" + "pr2.get_aabb()" ] + }, + { + "cell_type": "code", + "execution_count": null, + "outputs": [], + "source": [], + "metadata": { + "collapsed": false + }, + "id": "b4093cc83958ab47" } ], "metadata": { From c267c68bea92bf742c6015fcfd601db5d9b328d8 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jan 2024 21:56:20 +0100 Subject: [PATCH 39/69] [Abstract World] applying code review edits, stopped at get_link_color in world.py --- doc/source/notebooks/bullet_world.ipynb | 4 +- examples/bullet_world.ipynb | 235 ++++++++++++++------ src/pycram/bullet_world.py | 99 ++++----- src/pycram/costmaps.py | 2 +- src/pycram/enums.py | 8 + src/pycram/ros/viz_marker_publisher.py | 2 +- src/pycram/world.py | 275 ++++++++++++++---------- src/pycram/world_dataclasses.py | 8 +- test/bullet_world_testcase.py | 4 +- test/test_bullet_world.py | 4 - test/test_database_merger.py | 4 +- test/test_database_resolver.py | 4 +- test/test_failure_handling.py | 4 +- test/test_jpt_resolver.py | 3 +- 14 files changed, 407 insertions(+), 249 deletions(-) diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb index a2ce21a1d..c9a0def28 100644 --- a/doc/source/notebooks/bullet_world.ipynb +++ b/doc/source/notebooks/bullet_world.ipynb @@ -533,7 +533,7 @@ } ], "source": [ - "print(f\"Pr2 forearm color: {pr2.get_color('r_forearm_link')}\")" + "print(f\"Pr2 forearm color: {pr2.links['r_forearm_link'].color}\")" ] }, { @@ -548,7 +548,7 @@ }, "outputs": [], "source": [ - "pr2.set_color([1, 0, 0], \"r_forearm_link\")" + "pr2.links[\"r_forearm_link\"].color = [1, 0, 0, 1]" ] }, { diff --git a/examples/bullet_world.ipynb b/examples/bullet_world.ipynb index b735ca76c..d36ce6737 100644 --- a/examples/bullet_world.ipynb +++ b/examples/bullet_world.ipynb @@ -15,16 +15,63 @@ "cell_type": "code", "execution_count": 1, "id": "23bbba35", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:29:09.773814091Z", + "start_time": "2024-01-25T18:29:08.901441651Z" + } + }, "outputs": [ { "name": "stderr", "output_type": "stream", "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" + "pybullet build time: Nov 28 2023 23:51:11\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "[WARN] [1706207349.376331]: Failed to import Giskard messages\n", + "[WARN] [1706207349.395439]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "Exception in thread Thread-11:\n", + "Traceback (most recent call last):\n", + " File \"/usr/lib/python3.8/threading.py\", line 932, in _bootstrap_inner\n", + " self.run()\n", + " File \"/home/bassioun/cram_ws/src/pycram/src/pycram/bullet_world.py\", line 527, in run\n", + " keys = p.getKeyboardEvents(physicalClientId=self.world.client_id)\n", + "TypeError: 'physicalClientId' is an invalid keyword argument for this function\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "startThreads creating 1 threads.\n", + "starting thread 0\n", + "started thread 0 \n", + "argc=2\n", + "argv[0] = --unused\n", + "argv[1] = --start_demo_name=Physics Server\n", + "ExampleBrowserThreadFunc started\n", + "X11 functions dynamically loaded using dlopen/dlsym OK!\n", + "X11 functions dynamically loaded using dlopen/dlsym OK!\n", + "Creating context\n", + "Created GL 3.3 context\n", + "Direct GLX rendering context obtained\n", + "Making context current\n", + "GL_VENDOR=NVIDIA Corporation\n", + "GL_RENDERER=NVIDIA GeForce RTX 4070/PCIe/SSE2\n", + "GL_VERSION=3.3.0 NVIDIA 535.154.05\n", + "GL_SHADING_LANGUAGE_VERSION=3.30 NVIDIA via Cg compiler\n", + "pthread_getconcurrency()=0\n", + "Version = 3.3.0 NVIDIA 535.154.05\n", + "Vendor = NVIDIA Corporation\n", + "Renderer = NVIDIA GeForce RTX 4070/PCIe/SSE2\n", + "b3Printf: Selected demo: Physics Server\n", + "startThreads creating 1 threads.\n", + "starting thread 0\n", + "started thread 0 \n", + "MotionThreadFunc thread started\n" ] } ], @@ -81,7 +128,12 @@ "cell_type": "code", "execution_count": 2, "id": "48a3aed2", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:29:17.841474190Z", + "start_time": "2024-01-25T18:29:17.782682388Z" + } + }, "outputs": [], "source": [ "from pycram.bullet_world import Object\n", @@ -111,7 +163,12 @@ "cell_type": "code", "execution_count": 3, "id": "4ae2bc42", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:29:23.208599020Z", + "start_time": "2024-01-25T18:29:23.206209707Z" + } + }, "outputs": [], "source": [ "milk.set_position(Pose([1, 1, 1]))" @@ -121,7 +178,12 @@ "cell_type": "code", "execution_count": 4, "id": "4adc7b11", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:29:23.675871190Z", + "start_time": "2024-01-25T18:29:23.673804147Z" + } + }, "outputs": [], "source": [ "milk.set_orientation(Pose(orientation=[1,0, 0, 1]))" @@ -131,7 +193,12 @@ "cell_type": "code", "execution_count": 5, "id": "f91d1809", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:29:24.180713247Z", + "start_time": "2024-01-25T18:29:24.175368603Z" + } + }, "outputs": [], "source": [ "milk.set_pose(Pose([0, 0, 1], [0, 0, 0, 1]))" @@ -147,9 +214,14 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 6, "id": "1db2aa78", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:29:58.869360443Z", + "start_time": "2024-01-25T18:29:58.827502541Z" + } + }, "outputs": [ { "name": "stdout", @@ -168,8 +240,8 @@ "header: \n", " seq: 0\n", " stamp: \n", - " secs: 1690203002\n", - " nsecs: 93374252\n", + " secs: 1706207364\n", + " nsecs: 174507617\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -205,9 +277,14 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 7, "id": "10a493b1", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:02.056106044Z", + "start_time": "2024-01-25T18:30:02.001333744Z" + } + }, "outputs": [], "source": [ "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1, 0, 1]))" @@ -215,9 +292,14 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 8, "id": "275372e3", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:03.288290549Z", + "start_time": "2024-01-25T18:30:03.283992204Z" + } + }, "outputs": [], "source": [ "milk.attach(cereal)" @@ -233,9 +315,14 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 9, "id": "dff85834", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:05.029258354Z", + "start_time": "2024-01-25T18:30:05.026329993Z" + } + }, "outputs": [], "source": [ "milk.set_position(Pose([1,1,1]))" @@ -251,10 +338,23 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 10, "id": "b0bba145", - "metadata": {}, - "outputs": [], + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:06.737363899Z", + "start_time": "2024-01-25T18:30:06.734694119Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Removing constraint with id: 1\n" + ] + } + ], "source": [ "cereal.detach(milk)" ] @@ -272,29 +372,20 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 11, "id": "edf5cb72", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:13.548802172Z", + "start_time": "2024-01-25T18:30:13.540787905Z" + } + }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "{'base_link': 0, 'base_bellow_link': 1, 'base_laser_link': 2, 'fl_caster_rotation_link': 3, 'fl_caster_l_wheel_link': 4, 'fl_caster_r_wheel_link': 5, 'fr_caster_rotation_link': 6, 'fr_caster_l_wheel_link': 7, 'fr_caster_r_wheel_link': 8, 'bl_caster_rotation_link': 9, 'bl_caster_l_wheel_link': 10, 'bl_caster_r_wheel_link': 11, 'br_caster_rotation_link': 12, 'br_caster_l_wheel_link': 13, 'br_caster_r_wheel_link': 14, 'torso_lift_link': 15, 'l_torso_lift_side_plate_link': 16, 'r_torso_lift_side_plate_link': 17, 'imu_link': 18, 'head_pan_link': 19, 'head_tilt_link': 20, 'head_plate_frame': 21, 'sensor_mount_link': 22, 'high_def_frame': 23, 'high_def_optical_frame': 24, 'double_stereo_link': 25, 'wide_stereo_link': 26, 'wide_stereo_optical_frame': 27, 'wide_stereo_l_stereo_camera_frame': 28, 'wide_stereo_l_stereo_camera_optical_frame': 29, 'wide_stereo_r_stereo_camera_frame': 30, 'wide_stereo_r_stereo_camera_optical_frame': 31, 'narrow_stereo_link': 32, 'narrow_stereo_optical_frame': 33, 'narrow_stereo_l_stereo_camera_frame': 34, 'narrow_stereo_l_stereo_camera_optical_frame': 35, 'narrow_stereo_r_stereo_camera_frame': 36, 'narrow_stereo_r_stereo_camera_optical_frame': 37, 'projector_wg6802418_frame': 38, 'projector_wg6802418_child_frame': 39, 'laser_tilt_mount_link': 40, 'laser_tilt_link': 41, 'r_shoulder_pan_link': 42, 'r_shoulder_lift_link': 43, 'r_upper_arm_roll_link': 44, 'r_upper_arm_link': 45, 'r_elbow_flex_link': 46, 'r_forearm_roll_link': 47, 'r_forearm_link': 48, 'r_wrist_flex_link': 49, 'r_wrist_roll_link': 50, 'r_gripper_palm_link': 51, 'r_gripper_led_frame': 52, 'r_gripper_motor_accelerometer_link': 53, 'r_gripper_tool_frame': 54, 'r_gripper_motor_slider_link': 55, 'r_gripper_motor_screw_link': 56, 'r_gripper_l_finger_link': 57, 'r_gripper_l_finger_tip_link': 58, 'r_gripper_r_finger_link': 59, 'r_gripper_r_finger_tip_link': 60, 'r_gripper_l_finger_tip_frame': 61, 'r_forearm_cam_frame': 62, 'r_forearm_cam_optical_frame': 63, 'l_shoulder_pan_link': 64, 'l_shoulder_lift_link': 65, 'l_upper_arm_roll_link': 66, 'l_upper_arm_link': 67, 'l_elbow_flex_link': 68, 'l_forearm_roll_link': 69, 'l_forearm_link': 70, 'l_wrist_flex_link': 71, 'l_wrist_roll_link': 72, 'l_gripper_palm_link': 73, 'l_gripper_led_frame': 74, 'l_gripper_motor_accelerometer_link': 75, 'l_gripper_tool_frame': 76, 'l_gripper_motor_slider_link': 77, 'l_gripper_motor_screw_link': 78, 'l_gripper_l_finger_link': 79, 'l_gripper_l_finger_tip_link': 80, 'l_gripper_r_finger_link': 81, 'l_gripper_r_finger_tip_link': 82, 'l_gripper_l_finger_tip_frame': 83, 'l_forearm_cam_frame': 84, 'l_forearm_cam_optical_frame': 85, 'torso_lift_motor_screw_link': 86, 'base_footprint': -1}\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" + "{'base_link': , 'base_footprint': , 'base_bellow_link': , 'base_laser_link': , 'fl_caster_rotation_link': , 'fl_caster_l_wheel_link': , 'fl_caster_r_wheel_link': , 'fr_caster_rotation_link': , 'fr_caster_l_wheel_link': , 'fr_caster_r_wheel_link': , 'bl_caster_rotation_link': , 'bl_caster_l_wheel_link': , 'bl_caster_r_wheel_link': , 'br_caster_rotation_link': , 'br_caster_l_wheel_link': , 'br_caster_r_wheel_link': , 'torso_lift_link': , 'l_torso_lift_side_plate_link': , 'r_torso_lift_side_plate_link': , 'torso_lift_motor_screw_link': , 'imu_link': , 'head_pan_link': , 'head_tilt_link': , 'head_plate_frame': , 'sensor_mount_link': , 'high_def_frame': , 'high_def_optical_frame': , 'double_stereo_link': , 'wide_stereo_link': , 'wide_stereo_optical_frame': , 'wide_stereo_l_stereo_camera_frame': , 'wide_stereo_l_stereo_camera_optical_frame': , 'wide_stereo_r_stereo_camera_frame': , 'wide_stereo_r_stereo_camera_optical_frame': , 'narrow_stereo_link': , 'narrow_stereo_optical_frame': , 'narrow_stereo_l_stereo_camera_frame': , 'narrow_stereo_l_stereo_camera_optical_frame': , 'narrow_stereo_r_stereo_camera_frame': , 'narrow_stereo_r_stereo_camera_optical_frame': , 'projector_wg6802418_frame': , 'projector_wg6802418_child_frame': , 'laser_tilt_mount_link': , 'laser_tilt_link': , 'r_shoulder_pan_link': , 'r_shoulder_lift_link': , 'r_upper_arm_roll_link': , 'r_upper_arm_link': , 'r_forearm_roll_link': , 'r_elbow_flex_link': , 'r_forearm_link': , 'r_wrist_flex_link': , 'r_wrist_roll_link': , 'r_gripper_palm_link': , 'r_gripper_led_frame': , 'r_gripper_motor_accelerometer_link': , 'r_gripper_tool_frame': , 'r_gripper_motor_slider_link': , 'r_gripper_motor_screw_link': , 'r_gripper_l_finger_link': , 'r_gripper_r_finger_link': , 'r_gripper_l_finger_tip_link': , 'r_gripper_r_finger_tip_link': , 'r_gripper_l_finger_tip_frame': , 'l_shoulder_pan_link': , 'l_shoulder_lift_link': , 'l_upper_arm_roll_link': , 'l_upper_arm_link': , 'l_forearm_roll_link': , 'l_elbow_flex_link': , 'l_forearm_link': , 'l_wrist_flex_link': , 'l_wrist_roll_link': , 'l_gripper_palm_link': , 'l_gripper_led_frame': , 'l_gripper_motor_accelerometer_link': , 'l_gripper_tool_frame': , 'l_gripper_motor_slider_link': , 'l_gripper_motor_screw_link': , 'l_gripper_l_finger_link': , 'l_gripper_r_finger_link': , 'l_gripper_l_finger_tip_link': , 'l_gripper_r_finger_tip_link': , 'l_gripper_l_finger_tip_frame': , 'l_forearm_cam_frame': , 'l_forearm_cam_optical_frame': , 'r_forearm_cam_frame': , 'r_forearm_cam_optical_frame': }\n" ] } ], @@ -313,9 +404,14 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 12, "id": "51b59ffd", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:26.283263579Z", + "start_time": "2024-01-25T18:30:26.259577324Z" + } + }, "outputs": [ { "name": "stdout", @@ -334,8 +430,8 @@ "header: \n", " seq: 0\n", " stamp: \n", - " secs: 1690203043\n", - " nsecs: 303540229\n", + " secs: 1706207426\n", + " nsecs: 257559537\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -374,9 +470,14 @@ }, { "cell_type": "code", - "execution_count": 16, + "execution_count": 13, "id": "38a8aef4", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:51.633276335Z", + "start_time": "2024-01-25T18:30:51.590393104Z" + } + }, "outputs": [ { "name": "stdout", @@ -391,11 +492,11 @@ "source": [ "print(f\"Joint limits: {pr2.get_joint_limits('torso_lift_joint')}\")\n", "\n", - "print(f\"Current Joint state: {pr2.get_joint_state('torso_lift_joint')}\")\n", + "print(f\"Current Joint state: {pr2.get_joint_position('torso_lift_joint')}\")\n", "\n", - "pr2.set_joint_state(\"torso_lift_joint\", 0.2)\n", + "pr2.set_joint_position(\"torso_lift_joint\", 0.2)\n", "\n", - "print(f\"New Joint state: {pr2.get_joint_state('torso_lift_joint')}\")" + "print(f\"New Joint state: {pr2.get_joint_position('torso_lift_joint')}\")" ] }, { @@ -409,15 +510,20 @@ }, { "cell_type": "code", - "execution_count": 17, + "execution_count": 14, "id": "5eee9b8a", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:57.200886492Z", + "start_time": "2024-01-25T18:30:57.198278714Z" + } + }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "Pr2 forearm color: (0.699999988079071, 0.699999988079071, 0.699999988079071, 1.0)\n" + "Pr2 forearm color: Color(R=0.699999988079071, G=0.699999988079071, B=0.699999988079071, A=1.0)\n" ] } ], @@ -427,9 +533,14 @@ }, { "cell_type": "code", - "execution_count": 18, + "execution_count": 15, "id": "07aa1c3f", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:30:59.531448130Z", + "start_time": "2024-01-25T18:30:59.523750241Z" + } + }, "outputs": [], "source": [ "pr2.set_color([1, 0, 0], \"r_forearm_link\")" @@ -445,24 +556,26 @@ }, { "cell_type": "code", - "execution_count": 19, + "execution_count": 16, "id": "2a78715b", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-25T18:31:05.700993547Z", + "start_time": "2024-01-25T18:31:05.677436085Z" + } + }, "outputs": [ { "data": { - "text/plain": [ - "((-0.0015000000000000005, -0.0015000000000000005, 0.06949999999999999),\n", - " (0.0015000000000000005, 0.0015000000000000005, 0.0725))" - ] + "text/plain": "AxisAlignedBoundingBox(min_x=-0.0015000000000000005, min_y=-0.0015000000000000005, min_z=0.06949999999999999, max_x=0.0015000000000000005, max_y=0.0015000000000000005, max_z=0.0725)" }, - "execution_count": 19, + "execution_count": 16, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "pr2.get_AABB()" + "pr2.get_aabb()" ] } ], diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 74766046c..4d85f7c5b 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -10,12 +10,10 @@ import rosgraph import rospy -from .enums import JointType, ObjectType +from .enums import JointType, ObjectType, WorldMode from .pose import Pose -from .world import World, Object -from .world_dataclasses import (Color, Constraint, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, - CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, - MeshVisualShape) +from .world import World, Object, Link +from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox, MultiBody, VisualShape from dataclasses import asdict @@ -30,16 +28,16 @@ class is the main interface to the Bullet Physics Engine and should be used to s if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): rospy.init_node('pycram') - def __init__(self, mode: str = "GUI", is_prospection_world: bool = False, sim_time_step=0.004167): # 240 Hz + def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False, sim_frequency=240): """ Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world. - :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" + :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default is "GUI" :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. """ - super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_time_step=sim_time_step) + super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_frequency=sim_frequency) self._gui_thread: Gui = Gui(self, mode) self._gui_thread.start() @@ -58,19 +56,13 @@ def __init__(self, mode: str = "GUI", is_prospection_world: bool = False, sim_ti if not is_prospection_world: plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def load_urdf_at_pose_and_get_object_id(self, path: str, pose: Pose) -> int: + def load_urdf_and_get_object_id(self, path: str, pose: Pose) -> int: return p.loadURDF(path, basePosition=pose.position_as_list(), baseOrientation=pose.orientation_as_list(), physicsClientId=self.client_id) - def remove_object(self, obj_id: int) -> None: - """ - Remove an object by its ID. - - :param obj_id: The unique id of the object to be removed. - """ - - p.removeBody(obj_id, self.client_id) + def remove_object(self, obj: Object) -> None: + p.removeBody(obj.id, self.client_id) def add_constraint(self, constraint: Constraint) -> int: """ @@ -99,7 +91,7 @@ def remove_constraint(self, constraint_id): print("Removing constraint with id: ", constraint_id) p.removeConstraint(constraint_id, physicsClientId=self.client_id) - def get_object_joint_rest_pose(self, obj: Object, joint_name: str) -> float: + def get_joint_rest_pose(self, obj: Object, joint_name: str) -> float: """ Get the joint rest pose of an articulated object @@ -108,7 +100,7 @@ def get_object_joint_rest_pose(self, obj: Object, joint_name: str) -> float: """ return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[0] - def get_object_joint_damping(self, obj: Object, joint_name: str) -> float: + def get_joint_damping(self, obj: Object, joint_name: str) -> float: """ Get the joint damping of an articulated object @@ -167,11 +159,11 @@ def get_object_joint_position(self, obj: Object, joint_name: str) -> float: """ return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[0] - def get_object_link_pose(self, obj_id: int, link_id: int) -> Pose: + def get_link_pose(self, link: Link) -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. """ - return Pose(*p.getLinkState(obj_id, link_id, physicsClientId=self.client_id)[4:6]) + return Pose(*p.getLinkState(link.get_object_id(), link.id, physicsClientId=self.client_id)[4:6]) def perform_collision_detection(self) -> None: """ @@ -180,13 +172,15 @@ def perform_collision_detection(self) -> None: p.performCollisionDetection(physicsClientId=self.client_id) def get_object_contact_points(self, obj: Object) -> List: - """l.update_transforms_for_object(self.milk) - Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the returned - list please look at `PyBullet Doc `_ + """ + Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the + returned list please look at: + `PyBullet Doc `_ :param obj: The object. :return: A list of all contact points with other objects """ + self.perform_collision_detection() return p.getContactPoints(obj.id, physicsClientId=self.client_id) def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: @@ -197,39 +191,40 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> :param obj2: The second object. :return: A list of all contact points between the two objects. """ + self.perform_collision_detection() return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.client_id) - def get_object_joint_names(self, obj_id: int) -> List[str]: + def get_joint_names(self, obj: Object) -> List[str]: """ Get the names of all joints of an articulated object. """ - num_joints = self.get_object_number_of_joints(obj_id) - return [p.getJointInfo(obj_id, i, physicsClientId=self.client_id)[1].decode('utf-8') for i in range(num_joints)] + num_joints = self.get_number_of_joints(obj) + return [p.getJointInfo(obj.id, i, physicsClientId=self.client_id)[1].decode('utf-8') for i in range(num_joints)] - def get_object_link_names(self, obj_id: int) -> List[str]: + def get_link_names(self, obj: Object) -> List[str]: """ Get the names of all joints of an articulated object. """ - num_links = self.get_object_number_of_links(obj_id) - return [p.getJointInfo(obj_id, i, physicsClientId=self.client_id)[12].decode('utf-8') for i in range(num_links)] + num_links = self.get_number_of_links(obj) + return [p.getJointInfo(obj.id, i, physicsClientId=self.client_id)[12].decode('utf-8') for i in range(num_links)] - def get_object_number_of_links(self, obj_id: int) -> int: + def get_number_of_links(self, obj: Object) -> int: """ Get the number of links of an articulated object - :param obj_id: The object + :param obj: The object """ - return self.get_object_number_of_joints(obj_id) + return self.get_number_of_joints(obj) - def get_object_number_of_joints(self, obj_id: int) -> int: + def get_number_of_joints(self, obj: Object) -> int: """ Get the number of joints of an articulated object - :param obj_id: The object + :param obj: The object """ - return p.getNumJoints(obj_id, physicsClientId=self.client_id) + return p.getNumJoints(obj, physicsClientId=self.client_id) - def reset_object_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ Reset the joint position instantly without physics simulation @@ -256,23 +251,22 @@ def step(self): """ p.stepSimulation(physicsClientId=self.client_id) - def set_object_link_color(self, obj: Object, link_id: int, rgba_color: Color): + def set_link_color(self, link: Link, rgba_color: Color): """ - Changes the color of a link of this object, the color has to be given as a 4 element list + Changes the rgba_color of a link of this object, the rgba_color has to be given as a 4 element list of RGBA values. - :param obj: The object which should be colored - :param link_id: The link id of the link which should be colored - :param rgba_color: The color as RGBA values between 0 and 1 + :param link: The link which should be colored. + :param rgba_color: The rgba_color as RGBA values between 0 and 1 """ - p.changeVisualShape(obj.id, link_id, rgbaColor=rgba_color, physicsClientId=self.client_id) + p.changeVisualShape(link.get_object_id(), link.id, rgbaColor=rgba_color, physicsClientId=self.client_id) - def get_color_of_object_link(self, obj: Object, link_name: str) -> Color: - return self.get_colors_of_all_links_of_object(obj)[link_name] + def get_link_color(self, link: Link) -> Color: + return self.get_colors_of_all_links_of_object(link.object)[link.name] def get_colors_of_all_links_of_object(self, obj: Object) -> Dict[str, Color]: """ - Get the RGBA colors of each link in the object as a dictionary from link name to color. + Get the RGBA colors of each link in the object as a dictionary from link name to rgba_color. :param obj: The object :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. @@ -403,8 +397,8 @@ def remove_vis_axis(self) -> None: """ Removes all spawned vis axis objects that are currently in this BulletWorld. """ - for id in self.vis_axis: - p.removeBody(id, physicsClientId=self.client_id) + for vis_id in self.vis_axis: + p.removeBody(vis_id, physicsClientId=self.client_id) self.vis_axis = [] def ray_test(self, from_position: List[float], to_position: List[float]) -> int: @@ -468,14 +462,15 @@ def get_images_for_target(self, class Gui(threading.Thread): """ - For internal use only. Creates a new thread for the physics simulation that is active until closed by :func:`~World.exit` + For internal use only. Creates a new thread for the physics simulation that is active until closed by + :func:`~World.exit` Also contains the code for controlling the camera. """ - def __init__(self, world: World, mode: str): + def __init__(self, world: World, mode: WorldMode): threading.Thread.__init__(self) self.world = world - self.mode: str = mode + self.mode: WorldMode = mode def run(self): """ @@ -483,7 +478,7 @@ def run(self): if it is still active. If it is the thread will be suspended for 1/80 seconds, if it is not the method and thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. """ - if self.mode != "GUI": + if self.mode == WorldMode.DIRECT: self.world.client_id = p.connect(p.DIRECT) else: self.world.client_id = p.connect(p.GUI) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 6e0154e2e..50049d873 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -747,7 +747,7 @@ def plot_grid(data: np.ndarray) -> None: fig, ax = plt.subplots() ax.imshow(data, cmap=cmap) # draw gridlines - # ax.grid(which='major', axis='both', linestyle='-', color='k', linewidth=1) + # ax.grid(which='major', axis='both', linestyle='-', rgba_color='k', linewidth=1) ax.set_xticks(np.arange(0.5, rows, 1)); ax.set_yticks(np.arange(0.5, cols, 1)); plt.tick_params(axis='both', labelsize=0, length=0) diff --git a/src/pycram/enums.py b/src/pycram/enums.py index dffdfe8b1..f9f87b798 100644 --- a/src/pycram/enums.py +++ b/src/pycram/enums.py @@ -78,3 +78,11 @@ class Shape(Enum): MESH = 5 PLANE = 6 CAPSULE = 7 + + +class WorldMode(Enum): + """ + Enum for the different modes of the world. + """ + GUI = "GUI" + DIRECT = "DIRECT" diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 9d629aa52..0eb2cb5b2 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -77,7 +77,7 @@ def _make_marker_array(self) -> MarkerArray: link_pose_with_origin = link_pose * link_origin msg.pose = link_pose_with_origin.to_pose().pose - color = [1, 1, 1, 1] if obj.link_name_to_id[link] == -1 else obj.get_color(link) + color = [1, 1, 1, 1] if obj.link_name_to_id[link] == -1 else obj.get_color() msg.color = ColorRGBA(*color) msg.lifetime = rospy.Duration(1) diff --git a/src/pycram/world.py b/src/pycram/world.py index c1c41b206..a9e0994a0 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -11,8 +11,8 @@ from queue import Queue import tf from tf.transformations import quaternion_from_euler -from typing import List, Optional, Dict, Tuple, Callable -from typing import Union +from typing_extensions import List, Optional, Dict, Tuple, Callable +from typing_extensions import Union import numpy as np import rospkg @@ -24,7 +24,7 @@ from .event import Event from .robot_descriptions import robot_description -from .enums import JointType, ObjectType +from .enums import JointType, ObjectType, WorldMode from .local_transformer import LocalTransformer from sensor_msgs.msg import JointState @@ -39,10 +39,17 @@ class World(ABC): """ - The World Class represents the physics Simulation and belief state. + The World Class represents the physics Simulation and belief state, it is the main interface for reasoning about + the World. This is implemented as a singleton, the current World can be accessed via the static variable + current_world which is managed by the World class itself. """ - current_world: World = None + simulation_frequency: float + """ + Global reference for the simulation frequency (Hz), used in calculating the equivalent real time in the simulation. + """ + + current_world: Optional[World] = None """ Global reference to the currently used World, usually this is the graphical one. However, if you are inside a UseProspectionWorld() environment the current_world points to the @@ -50,24 +57,18 @@ class World(ABC): used at the moment. """ - robot: Object = None + robot: Optional[Object] = None """ Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the URDF with the name of the URDF on the parameter server. """ - data_directory: List[str] = [os.path.dirname(__file__) + "/../../resources"] + data_directory: List[str] = [os.path.join(os.path.dirname(__file__), '..', '..', 'resources')] """ Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. """ - simulation_time_step: float = None - """ - Global reference for the simulation time step, this is used to calculate the frequency of the simulation, - and also for calculating the equivalent real time for the simulation. - """ - - def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: float): + 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 background. There can only be one rendered simulation. @@ -79,7 +80,7 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: if World.current_world is None: World.current_world = self - World.simulation_time_step = simulation_time_step + World.simulation_frequency = simulation_frequency self.is_prospection_world: bool = is_prospection_world self._init_and_sync_prospection_world() @@ -93,7 +94,7 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: self.client_id: int = -1 # This is used to connect to the physics server (allows multiple clients) - self.mode: str = mode + self.mode: WorldMode = mode # The mode of the simulation, can be "GUI" or "DIRECT" self.coll_callbacks: Dict[Tuple[Object, Object], CollisionCallbacks] = {} @@ -103,27 +104,44 @@ def __init__(self, mode: str, is_prospection_world: bool, simulation_time_step: self.saved_states: List[int] = [] def _init_events(self): + """ + Initializes dynamic events that can be used to react to changes in the World. + """ self.detachment_event: Event = Event() self.attachment_event: Event = Event() self.manipulation_event: Event = Event() def _init_and_sync_prospection_world(self): + """ + Initializes the prospection world and the synchronization between the main and the prospection world. + """ self._init_prospection_world() self._sync_prospection_world() def _update_local_transformer_worlds(self): + """ + Updates the local transformer worlds with the current world and prospection world. + """ self.local_transformer.world = self self.local_transformer.prospection_world = self.prospection_world def _init_prospection_world(self): + """ + Initializes the prospection world, if this is a prospection world itself it will not create another prospection, + world, but instead set the prospection world to None, else it will create a prospection world. + """ if self.is_prospection_world: # then no need to add another prospection world self.prospection_world = None else: - self.prospection_world: World = self.__class__("DIRECT", + self.prospection_world: World = self.__class__(WorldMode.DIRECT, True, - World.simulation_time_step) + World.simulation_frequency) def _sync_prospection_world(self): + """ + Synchronizes the prospection world with the main world, this means that every object in the main world will be + added to the prospection world and vice versa. + """ if self.is_prospection_world: # then no need to add another prospection world self.world_sync = None else: @@ -131,11 +149,21 @@ def _sync_prospection_world(self): self.world_sync.start() @property - def simulation_frequency(self): - return int(1/World.simulation_time_step) + def simulation_time_step(self): + """ + The time step of the simulation in seconds. + """ + return 1/World.simulation_frequency @abstractmethod - def load_urdf_at_pose_and_get_object_id(self, path: str, pose: Pose) -> int: + def load_urdf_and_get_object_id(self, path: str, pose: Pose) -> int: + """ + Loads a URDF file at the given pose and returns the id of the loaded object. + + :param path: The path to the URDF file. + :param pose: The pose at which the object should be loaded. + :return: The id of the loaded object. + """ pass def get_objects_by_name(self, name: str) -> List[Object]: @@ -166,11 +194,11 @@ def get_object_by_id(self, obj_id: int) -> Object: return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] @abstractmethod - def remove_object(self, obj_id: int) -> None: + def remove_object(self, obj: Object) -> None: """ - Remove an object by its ID. + Remove an object from the world. - :param obj_id: The unique id of the object to be removed. + :param obj: The object to be removed. """ pass @@ -178,9 +206,12 @@ def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_par """ Creates a fixed joint constraint between the given parent and child links, the joint frame will be at the origin of the child link frame, and would have the same orientation - as the child link frame. if no link is given, the base link will be used (id = -1). + as the child link frame. - returns the constraint id + :param parent_link: The constrained link of the parent object. + :param child_link: The constrained link of the child object. + :param child_to_parent_transform: The transform from the child link frame to the parent link frame. + :return: The unique id of the created constraint. """ constraint = Constraint(parent_obj_id=parent_link.get_object_id(), @@ -199,12 +230,19 @@ def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_par @abstractmethod def add_constraint(self, constraint: Constraint) -> int: """ - Add a constraint between two objects so that they become attached + Add a constraint between two objects links so that they become attached for example. + + :param constraint: The constraint data used to create the constraint. """ pass @abstractmethod - def remove_constraint(self, constraint_id): + def remove_constraint(self, constraint_id) -> None: + """ + Remove a constraint by its ID. + + :param constraint_id: The unique id of the constraint to be removed. + """ pass def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: @@ -213,7 +251,7 @@ def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, :param obj: The object. :param joint_name: The name of the joint. - :return: A tuple containing the upper and the lower limits of the joint. + :return: A tuple containing the upper and the lower limits of the joint respectively. """ return self.get_object_joint_upper_limit(obj, joint_name), self.get_object_joint_lower_limit(obj, joint_name) @@ -242,11 +280,11 @@ def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: @abstractmethod def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: """ - Returns the axis along which a joint is moving. The given joint_name has to be part of this object. + Returns the axis along/around which a joint is moving. The given joint_name has to be part of this object. - :param obj: The object + :param obj: The object. :param joint_name: Name of the joint for which the axis should be returned. - :return: The axis a vector of xyz + :return: The axis which is a 3D vector of xyz values. """ pass @@ -255,26 +293,30 @@ def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: """ Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - :param obj: The object - :param joint_name: Joint name for which the type should be returned - :return: The type of the joint + :param obj: The object. + :param joint_name: Joint name for which the type should be returned. + :return: The type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. """ pass @abstractmethod def get_object_joint_position(self, obj: Object, joint_name: str) -> float: """ - Get the state of a joint of an articulated object + Get the position of a joint of an articulated object - :param obj: The object - :param joint_name: The name of the joint + :param obj: The object. + :param joint_name: The name of the joint. + :return: The joint position as a float. """ pass @abstractmethod - def get_object_link_pose(self, obj_id: int, link_id: int) -> Pose: + def get_link_pose(self, link: Link) -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. + + :param link: The link as a Link object. + :return: The pose of the link as a Pose object. """ pass @@ -296,7 +338,6 @@ def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: elif callbacks.no_collision_cb is not None: callbacks.no_collision_cb() if real_time: - # Simulation runs at 240 Hz time.sleep(self.simulation_time_step) @abstractmethod @@ -309,7 +350,7 @@ def perform_collision_detection(self) -> None: @abstractmethod def get_object_contact_points(self, obj: Object) -> List: """ - Returns a list of contact points of this Object with other Objects. + Returns a list of contact points of this Object with all other Objects. :param obj: The object. :return: A list of all contact points with other objects @@ -335,65 +376,75 @@ def get_joint_ranges(self) -> Tuple[List, List, List, List, List]: :return: The lists for the upper and lower limits, joint ranges, rest poses and joint damping """ ll, ul, jr, rp, jd = [], [], [], [], [] - joint_names = self.get_object_joint_names(self.robot.id) + joint_names = self.get_joint_names(self.robot) for name in joint_names: joint_type = self.get_object_joint_type(self.robot, name) if joint_type != JointType.FIXED: ll.append(self.get_object_joint_lower_limit(self.robot, name)) ul.append(self.get_object_joint_upper_limit(self.robot, name)) jr.append(ul[-1] - ll[-1]) - rp.append(self.get_object_joint_rest_pose(self.robot, name)) - jd.append(self.get_object_joint_damping(self.robot, name)) + rp.append(self.get_joint_rest_pose(self.robot, name)) + jd.append(self.get_joint_damping(self.robot, name)) return ll, ul, jr, rp, jd - def get_object_joint_rest_pose(self, obj: Object, joint_name: str) -> float: + def get_joint_rest_pose(self, obj: Object, joint_name: str) -> float: """ Get the rest pose of a joint of an articulated object - :param obj: The object - :param joint_name: The name of the joint + :param obj: The object. + :param joint_name: The name of the joint. + :return: The rest pose of the joint. """ pass - def get_object_joint_damping(self, obj: Object, joint_name: str) -> float: + def get_joint_damping(self, obj: Object, joint_name: str) -> float: """ Get the damping of a joint of an articulated object - :param obj: The object - :param joint_name: The name of the joint + :param obj: The object. + :param joint_name: The name of the joint. + :return: The damping of the joint. """ pass @abstractmethod - def get_object_joint_names(self, obj_id: int) -> List[str]: + def get_joint_names(self, obj: Object) -> List[str]: """ Get the names of all joints of an articulated object. + :param obj: The object. + :return: A list of all joint names of the object. """ pass @abstractmethod - def get_object_link_names(self, obj_id: int) -> List[str]: + def get_link_names(self, obj: Object) -> List[str]: """ Get the names of all links of an articulated object. + :param obj: The object. + :return: A list of all link names of the object. """ pass @abstractmethod - def get_object_number_of_joints(self, obj_id: int) -> int: + def get_number_of_joints(self, obj: Object) -> int: """ Get the number of joints of an articulated object + :param obj: The object. + :return: The number of joints of the object. """ pass - def get_object_number_of_links(self, obj_id: int) -> int: + def get_number_of_links(self, obj: Object) -> int: """ Get the number of links of an articulated object + :param obj: The object. + :return: The number of links of the object. """ pass @abstractmethod - def reset_object_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ Reset the joint position instantly without physics simulation @@ -422,53 +473,52 @@ def step(self): """ pass - def set_object_color(self, obj: Object, color: Color, link: Optional[str] = ""): + def set_object_color(self, obj: Object, rgba_color: Color): """ Changes the color of this object, the color has to be given as a list - of RGBA values. Optionally a link name can can be provided, if no link - name is provided all links of this object will be colored. + of RGBA values. :param obj: The object which should be colored - :param color: The color as Color object with RGBA values between 0 and 1 - :param link: The link name of the link which should be colored - """ - if link == "": - # Check if there is only one link, this is the case for primitive - # forms or if loaded from an .stl or .obj file - if obj.link_name_to_id != {}: - for link_id in obj.link_name_to_id.values(): - self.set_object_link_color(obj, link_id, color) - else: - self.set_object_link_color(obj, -1, color) + :param rgba_color: The color as Color object with RGBA values between 0 and 1 + """ + # Check if there is only one link, this is the case for primitive + # forms or if loaded from an .stl or .obj file + if obj.links != {}: + for link in obj.links.values(): + self.set_link_color(link, rgba_color) else: - self.set_object_link_color(obj, obj.link_name_to_id[link], color) + self.set_link_color(obj.get_root_link(), rgba_color) @abstractmethod - def set_object_link_color(self, obj: Object, link_id: int, rgba_color: Color): + def set_link_color(self, link: Link, rgba_color: Color): """ - Changes the color of a link of this object, the color has to be given as Color object. + Changes the rgba_color of a link of this object, the rgba_color has to be given as Color object. - :param obj: The object which should be colored - :param link_id: The link id of the link which should be colored - :param rgba_color: The color as Color object with RGBA values between 0 and 1 + :param link: The link which should be colored. + :param rgba_color: The rgba_color as Color object with RGBA values between 0 and 1. """ pass @abstractmethod - def get_color_of_object_link(self, obj: Object, link_name: str) -> Color: + def get_link_color(self, link: Link) -> Color: + """ + This method returns the rgba_color of this link. + :param link: The link for which the rgba_color should be returned. + :return: The rgba_color as Color object with RGBA values between 0 and 1. + """ pass def get_color_of_object(self, obj: Object) -> Union[Color, Dict[str, Color]]: """ - This method returns the color of this object. The return is either: + This method returns the rgba_color of this object. The return is either: 1. A Color object with RGBA values, this is the case if the object only has one link (this happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the color as value. The color is given as a Color Object. - Please keep in mind that not every link may have a color. This is dependent on the URDF from which the + 2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object. + Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which the object is spawned. - :param obj: The object for which the color should be returned. + :param obj: The object for which the rgba_color should be returned. """ link_to_color_dict = self.get_colors_of_all_links_of_object(obj) @@ -480,7 +530,7 @@ def get_color_of_object(self, obj: Object) -> Union[Color, Dict[str, Color]]: @abstractmethod def get_colors_of_all_links_of_object(self, obj: Object) -> Dict[str, Color]: """ - Get the RGBA colors of each link in the object as a dictionary from link name to color. + Get the RGBA colors of each link in the object as a dictionary from link name to rgba_color. :param obj: The object :return: A dictionary with link names as keys and a Color object for each link as value. @@ -698,7 +748,7 @@ def _copy(self) -> World: :return: The reference to the new World """ - world = World("DIRECT", False, World.simulation_time_step) + world = World(WorldMode.DIRECT, False, World.simulation_frequency) for obj in self.objects: obj_pose = Pose(obj.get_position_as_list(), obj.get_orientation_as_list()) o = Object(obj.name, obj.obj_type, obj.path, obj_pose, world, @@ -856,11 +906,11 @@ class UseProspectionWorld: """ def __init__(self): - self.prev_world: World = None + self.prev_world: Optional[World] = None def __enter__(self): if not World.current_world.is_prospection_world: - time.sleep(20 * World.simulation_time_step) + time.sleep(20 * World.current_world.simulation_time_step) # blocks until the adding queue is ready World.current_world.world_sync.add_obj_queue.join() @@ -913,7 +963,7 @@ def run(self): # self.equal_states = False for i in range(self.add_obj_queue.qsize()): obj = self.add_obj_queue.get() - # [name, type, path, position, orientation, self.world.prospection_world, color, world object] + # [name, type, path, position, orientation, self.world.prospection_world, rgba_color, world object] o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) # Maps the World object to the prospection world object self.object_mapping[obj[7]] = o @@ -1065,7 +1115,7 @@ def pose(self) -> Pose: """ The pose of the link relative to the world frame. """ - return self.world.get_object_link_pose(self.object.id, self.id) + return self.world.get_link_pose(self) @property def pose_as_list(self) -> List[List[float]]: @@ -1091,11 +1141,11 @@ def collision(self) -> Collision: @property def color(self) -> Color: - return self.world.get_color_of_object_link(self.object, self.name) + return self.world.get_link_color(self) @color.setter - def color(self, color: Color) -> None: - self.world.set_object_link_color(self.object, self.id, color) + def color(self, color: List[float]) -> None: + self.world.set_link_color(self, Color.from_rgba(color)) class RootLink(Link): @@ -1131,7 +1181,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, """ The constructor loads the urdf file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. - The color parameter is only used when loading .stl or .obj files, + The rgba_color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. :param name: The name of the object @@ -1141,7 +1191,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, :param pose: The pose at which the Object should be spawned :param world: The World in which the object should be spawned, if no world is specified the :py:attr:`~World.current_world` will be used. - :param color: The color with which the object should be spawned. + :param color: The rgba_color with which the object should be spawned. :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ @@ -1235,7 +1285,7 @@ def remove(self) -> None: self.world.world_sync.remove_obj_queue.put(self) self.world.world_sync.remove_obj_queue.join() - self.world.remove_object(self.id) + self.world.remove_object(self) if World.robot == self: World.robot = None @@ -1474,7 +1524,7 @@ def _get_joint_name_to_id_map(self) -> Dict[str, int]: """ Creates a dictionary which maps the joint names to their unique ids. """ - joint_names = self.world.get_object_joint_names(self.id) + joint_names = self.world.get_joint_names(self) n_joints = len(joint_names) return dict(zip(joint_names, range(n_joints))) @@ -1482,7 +1532,7 @@ def _get_link_name_to_id_map(self) -> Dict[str, int]: """ Creates a dictionary which maps the link names to their unique ids. """ - link_names = self.world.get_object_link_names(self.id) + link_names = self.world.get_link_names(self) n_links = len(link_names) return dict(zip(link_names, range(n_links))) @@ -1552,7 +1602,7 @@ def set_positions_of_all_joints(self, joint_poses: dict) -> None: :return: """ for joint_name, joint_position in joint_poses.items(): - self.world.reset_object_joint_position(self, joint_name, joint_position) + self.world.reset_joint_position(self, joint_name, joint_position) self._current_joints_positions[joint_name] = joint_position self._set_attached_objects_poses() @@ -1575,7 +1625,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: logging.error(f"The given joint position was: {joint_position}") # Temporarily disabled because kdl outputs values exciting joint limits # return - self.world.reset_object_joint_position(self, joint_name, joint_position) + self.world.reset_joint_position(self, joint_name, joint_position) self._current_joints_positions[joint_name] = joint_position self._set_attached_objects_poses() @@ -1640,22 +1690,17 @@ def update_pose_from_tf(self, frame: str) -> None: position[0][2]] self.set_position(Pose(position, orientation)) - def set_color(self, color: Color, link: Optional[str] = "") -> None: + def set_color(self, color: Color) -> None: """ - Changes the color of this object, the color has to be given as a list - of RGBA values. Optionally a link name can can be provided, if no link - name is provided all links of this object will be colored. + Changes the rgba_color of this object, the rgba_color has to be given as a list + of RGBA values. All links of this object will be colored. - :param color: The color as RGBA values between 0 and 1 - :param link: The link name of the link which should be colored + :param color: The rgba_color as RGBA values between 0 and 1 """ - self.world.set_object_color(self, color, link) + self.world.set_object_color(self, color) - def get_color(self, link: Optional[str] = None) -> Union[Color, Dict[str, Color], None]: - if link is None: - return self.world.get_color_of_object(self) - else: - return self.world.get_color_of_object_link(self, link) + def get_color(self) -> Union[Color, Dict[str, Color]]: + return self.world.get_color_of_object(self) def get_aabb(self) -> AxisAlignedBoundingBox: return self.world.get_object_aabb(self) @@ -1707,7 +1752,7 @@ def get_joint_type(self, joint_name: str) -> JointType: def find_joint_above(self, link_name: str, joint_type: JointType) -> str: """ - Traverses the chain from 'link_name' to the URDF origin and returns the first joint that is of type 'joint_type'. + Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. :param link_name: Link name above which the joint should be found :param joint_type: Joint type that should be searched for @@ -1859,7 +1904,7 @@ def _load_object(name: str, color: Color, ignore_cached_files: bool) -> Tuple[int, str]: """ - Loads an object to the given World with the given position and orientation. The color will only be + Loads an object to the given World with the given position and orientation. The rgba_color will only be used when an .obj or .stl file is given. If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created and this URDf file will be loaded instead. @@ -1872,7 +1917,7 @@ def _load_object(name: str, :param position: The position in which the object should be spawned :param orientation: The orientation in which the object should be spawned :param world: The World to which the Object should be spawned - :param color: The color of the object, only used when .obj or .stl file is given + :param color: The rgba_color of the object, only used when .obj or .stl file is given :param ignore_cached_files: Whether to ignore files in the cache directory. :return: The unique id of the object and the path to the file used for spawning """ @@ -1924,7 +1969,7 @@ def _load_object(name: str, path = cach_dir + name + ".urdf" try: - obj = world.load_urdf_at_pose_and_get_object_id(path, Pose(position, orientation)) + obj = world.load_urdf_and_get_object_id(path, Pose(position, orientation)) return obj, path except Exception as e: logging.error( @@ -2043,13 +2088,13 @@ def fix_link_attributes(urdf_string: str) -> str: def _generate_urdf_file(name: str, path: str, color: Color, cach_dir: str) -> str: """ - Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given color will be + Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with the name as filename. :param name: The name of the object :param path: The path to the .obj or .stl file - :param color: The color which should be used for the material tag + :param color: The rgba_color which should be used for the material tag :param cach_dir The absolute file path to the cach directory in the pycram package :return: The absolute path of the created file """ @@ -2061,7 +2106,7 @@ def _generate_urdf_file(name: str, path: str, color: Color, cach_dir: str) -> st \n \ \n \ \n \ - \n \ + \n \ \n \ \n \ \n \ diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 5c4e20c9e..474c6f86d 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -6,7 +6,7 @@ @dataclass class Color: """ - Dataclass for storing color as an RGBA value. + Dataclass for storing rgba_color as an RGBA value. """ R: float = 1 G: float = 1 @@ -16,7 +16,7 @@ class Color: @classmethod def from_rgba(cls, rgba: List[float]): """ - Sets the color from a list of RGBA values. + Sets the rgba_color from a list of RGBA values. :param rgba: The list of RGBA values """ @@ -24,9 +24,9 @@ def from_rgba(cls, rgba: List[float]): def get_rgba(self) -> List[float]: """ - Returns the color as a list of RGBA values. + Returns the rgba_color as a list of RGBA values. - :return: The color as a list of RGBA values + :return: The rgba_color as a list of RGBA values """ return [self.R, self.G, self.B, self.A] diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index b3fb307eb..755dfbb9a 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -4,7 +4,7 @@ from pycram.pose import Pose from pycram.robot_descriptions import robot_description from pycram.process_module import ProcessModule -from pycram.enums import ObjectType +from pycram.enums import ObjectType, WorldMode class BulletWorldTestCase(unittest.TestCase): @@ -13,7 +13,7 @@ class BulletWorldTestCase(unittest.TestCase): @classmethod def setUpClass(cls): - cls.world = BulletWorld("DIRECT") + cls.world = BulletWorld(WorldMode.DIRECT) cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf") cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 881412e1c..91639d8ca 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -4,11 +4,8 @@ from bullet_world_testcase import BulletWorldTestCase from pycram.pose import Pose -import numpy as np from pycram.world import fix_missing_inertial import xml.etree.ElementTree as ET -import tf - class BulletWorldTest(BulletWorldTestCase): @@ -51,4 +48,3 @@ def test_inertial(self): resulting_tree = ET.ElementTree(ET.fromstring(result)) for element in resulting_tree.iter("link"): self.assertTrue(len([*element.iter("inertial")]) > 0) - diff --git a/test/test_database_merger.py b/test/test_database_merger.py index 56bd90bce..cbd26b731 100644 --- a/test/test_database_merger.py +++ b/test/test_database_merger.py @@ -8,7 +8,7 @@ from pycram.designators.action_designator import * from pycram.designators.location_designator import * from pycram.process_module import simulated_robot -from pycram.enums import Arms, ObjectType +from pycram.enums import Arms, ObjectType, WorldMode from pycram.task import with_tree import pycram.task from pycram.bullet_world import Object @@ -27,7 +27,7 @@ class Configuration: class ExamplePlans: def __init__(self): - self.world = BulletWorld("DIRECT") + self.world = BulletWorld(WorldMode.DIRECT) self.pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") self.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") self.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 18d7c047b..6df2a71bc 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -13,7 +13,7 @@ from pycram.pose import Pose from pycram.robot_descriptions import robot_description from pycram.task import with_tree -from pycram.enums import ObjectType +from pycram.enums import ObjectType, WorldMode # check if jpt is installed jpt_installed = True @@ -42,7 +42,7 @@ class DatabaseResolverTestCase(unittest.TestCase,): @classmethod def setUpClass(cls) -> None: global pycrorm_uri - cls.world = World("DIRECT") + cls.world = World(WorldMode.DIRECT) cls.milk = Object("milk", "milk", "milk.stl", pose=Pose([1.3, 1, 0.9])) cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf") ProcessModule.execution_delay = False diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index d23a9d93b..7a97d308a 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -5,7 +5,7 @@ from pycram.bullet_world import BulletWorld, Object from pycram.designator import ActionDesignatorDescription from pycram.designators.action_designator import ParkArmsAction -from pycram.enums import ObjectType, Arms +from pycram.enums import ObjectType, Arms, WorldMode from pycram.failure_handling import Retry from pycram.plan_failures import PlanFailure from pycram.process_module import ProcessModule, simulated_robot @@ -29,7 +29,7 @@ class FailureHandlingTest(unittest.TestCase): @classmethod def setUpClass(cls): - cls.world = BulletWorld("DIRECT") + cls.world = BulletWorld(WorldMode.DIRECT) cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf") ProcessModule.execution_delay = True diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index ff80a0714..4ffad3d4c 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -12,6 +12,7 @@ from pycram.process_module import simulated_robot from pycram.robot_descriptions import robot_description from pycram.pose import Pose +from pycram.enums import WorldMode # check if jpt is installed jpt_installed = True @@ -53,7 +54,7 @@ def setUpClass(cls) -> None: cls.model = mlflow.pyfunc.load_model( model_uri="mlflow-artifacts:/0/9150dd1fb353494d807261928cea6e8c/artifacts/grasping").unwrap_python_model() \ .model - cls.world = BulletWorld("DIRECT") + cls.world = BulletWorld(WorldMode.DIRECT) cls.milk = Object("milk", "milk", "milk.stl", pose=Pose([3, 3, 0.75])) cls.robot = Object(robot_description.name, "pr2", robot_description.name + ".urdf") ProcessModule.execution_delay = False From 1aaa11e5ec4f0f2e409e5ac8988beadaeb5d8669 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 26 Jan 2024 23:22:09 +0100 Subject: [PATCH 40/69] [Abstract World] documenting Object class and cleaning it. --- examples/bullet_world.ipynb | 4 +- src/pycram/bullet_world.py | 16 +- src/pycram/costmaps.py | 8 +- src/pycram/designators/location_designator.py | 4 +- src/pycram/enums.py | 3 - src/pycram/pose.py | 3 +- .../resolver/location/giskard_location.py | 2 +- src/pycram/world.py | 607 ++++++++++++------ src/pycram/world_dataclasses.py | 38 +- src/pycram/world_reasoning.py | 20 +- 10 files changed, 463 insertions(+), 242 deletions(-) diff --git a/examples/bullet_world.ipynb b/examples/bullet_world.ipynb index d36ce6737..c487ec212 100644 --- a/examples/bullet_world.ipynb +++ b/examples/bullet_world.ipynb @@ -528,7 +528,7 @@ } ], "source": [ - "print(f\"Pr2 forearm color: {pr2.get_color('r_forearm_link')}\")" + "print(f\"Pr2 forearm color: {pr2.links['r_forearm_link'].color}\")" ] }, { @@ -543,7 +543,7 @@ }, "outputs": [], "source": [ - "pr2.set_color([1, 0, 0], \"r_forearm_link\")" + "pr2.links[\"r_forearm_link\"].color = [1, 0, 0]" ] }, { diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 4d85f7c5b..82e57c3b8 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -61,7 +61,7 @@ def load_urdf_and_get_object_id(self, path: str, pose: Pose) -> int: basePosition=pose.position_as_list(), baseOrientation=pose.orientation_as_list(), physicsClientId=self.client_id) - def remove_object(self, obj: Object) -> None: + def remove_object_from_simulator(self, obj: Object) -> None: p.removeBody(obj.id, self.client_id) def add_constraint(self, constraint: Constraint) -> int: @@ -78,7 +78,7 @@ def add_constraint(self, constraint: Constraint) -> int: parent_link_id, constraint.child_obj_id, child_link_id, - constraint.joint_type.as_int(), + constraint.joint_type.value, constraint.joint_axis_in_child_link_frame, constraint.joint_frame_position_wrt_parent_origin, constraint.joint_frame_position_wrt_child_origin, @@ -222,7 +222,7 @@ def get_number_of_joints(self, obj: Object) -> int: :param obj: The object """ - return p.getNumJoints(obj, physicsClientId=self.client_id) + return p.getNumJoints(obj.id, physicsClientId=self.client_id) def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ @@ -262,9 +262,9 @@ def set_link_color(self, link: Link, rgba_color: Color): p.changeVisualShape(link.get_object_id(), link.id, rgbaColor=rgba_color, physicsClientId=self.client_id) def get_link_color(self, link: Link) -> Color: - return self.get_colors_of_all_links_of_object(link.object)[link.name] + return self.get_colors_of_object_links(link.object)[link.name] - def get_colors_of_all_links_of_object(self, obj: Object) -> Dict[str, Color]: + def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: """ Get the RGBA colors of each link in the object as a dictionary from link name to rgba_color. @@ -278,7 +278,7 @@ def get_colors_of_all_links_of_object(self, obj: Object) -> Dict[str, Color]: link_to_color = dict(zip(links, colors)) return link_to_color - def get_object_aabb(self, obj: Object) -> AxisAlignedBoundingBox: + def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. @@ -288,12 +288,12 @@ def get_object_aabb(self, obj: Object) -> AxisAlignedBoundingBox: """ return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj.id, physicsClientId=self.client_id)) - def get_object_link_aabb(self, obj_id: int, link_id: int) -> AxisAlignedBoundingBox: + def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. """ - return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj_id, link_id, physicsClientId=self.client_id)) + return AxisAlignedBoundingBox.from_min_max(*p.getAABB(link.get_object_id(), link.id, physicsClientId=self.client_id)) def set_realtime(self, real_time: bool) -> None: """ diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 50049d873..c56812631 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -394,9 +394,9 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: r_t = self.world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) j += len(n) if World.robot: - shadow_robot = World.current_world.get_prospection_object_from_object(World.robot) + shadow_robot = World.current_world.get_prospection_object_for_object(World.robot) attached_objs = World.robot.attachments.keys() - attached_objs_shadow_id = [World.current_world.get_prospection_object_from_object(x).id for x + attached_objs_shadow_id = [World.current_world.get_prospection_object_for_object(x).id for x in attached_objs] res[i:j] = [ @@ -724,13 +724,13 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox: :return: Two points in world coordinate space, which span a rectangle """ - prospection_object = World.current_world.get_prospection_object_from_object(self.object) + prospection_object = World.current_world.get_prospection_object_for_object(self.object) with UseProspectionWorld(): prospection_object.set_orientation(Pose(orientation=[0, 0, 0, 1])) link_pose_trans = self.link.transform inverse_trans = link_pose_trans.invert() prospection_object.set_orientation(inverse_trans.to_pose()) - return self.link.get_aabb() + return self.link.get_axis_aligned_bounding_box() cmap = colors.ListedColormap(['white', 'black', 'green', 'red', 'blue']) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 8e55b147f..6060d0205 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -178,7 +178,7 @@ def __iter__(self): if self.visible_for or self.reachable_for: robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object - test_robot = World.current_world.get_prospection_object_from_object(robot_object) + test_robot = World.current_world.get_prospection_object_for_object(robot_object) with UseProspectionWorld(): @@ -254,7 +254,7 @@ def __iter__(self) -> Location: final_map = occupancy + gaussian - test_robot = World.current_world.get_prospection_object_from_object(self.robot) + test_robot = World.current_world.get_prospection_object_for_object(self.robot) # Find a Joint of type prismatic which is above the handle in the URDF tree container_joint = self.handle.world_object.find_joint_above(self.handle.name, JointType.PRISMATIC) diff --git a/src/pycram/enums.py b/src/pycram/enums.py index f9f87b798..3b7280ad6 100644 --- a/src/pycram/enums.py +++ b/src/pycram/enums.py @@ -30,9 +30,6 @@ class JointType(Enum): PLANAR = 3 FIXED = 4 - def as_int(self): - return self.value - class Grasp(Enum): """ diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 3a6cf54bc..8cdd39d11 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -307,7 +307,8 @@ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional @classmethod def from_pose_and_child_frame(cls, pose: Pose, child_frame_name: str) -> Transform: - return cls(pose.position_as_list(), pose.orientation_as_list(), pose.frame, child_frame_name) + return cls(pose.position_as_list(), pose.orientation_as_list(), pose.frame, child_frame_name, + time=pose.header.stamp) @staticmethod def from_transform_stamped(transform_stamped: TransformStamped) -> Transform: diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index 88479c50d..9d6122aa2 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -28,7 +28,7 @@ def __iter__(self) -> CostmapLocation.Location: pose_left, end_config_left = self._get_reachable_pose_for_arm(self.target, robot_description.get_tool_frame("left")) - test_robot = BulletWorld.current_world.get_prospection_object_from_object(BulletWorld.robot) + test_robot = BulletWorld.current_world.get_prospection_object_for_object(BulletWorld.robot) with UseProspectionWorld(): valid, arms = reachability_validator(pose_right, test_robot, self.target, {}) if valid: diff --git a/src/pycram/world.py b/src/pycram/world.py index a9e0994a0..6ca2bd4bc 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -31,10 +31,9 @@ from .pose import Pose, Transform from abc import ABC, abstractmethod -from dataclasses import dataclass from .world_dataclasses import (Color, Constraint, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, - CapsuleVisualShape, PlaneVisualShape, MeshVisualShape) + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, LinkState, ObjectState) class World(ABC): @@ -194,13 +193,36 @@ def get_object_by_id(self, obj_id: int) -> Object: return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] @abstractmethod + def remove_object_from_simulator(self, obj: Object) -> None: + """ + Removes an object from the physics simulator. + :param obj: The object to be removed. + """ + pass + def remove_object(self, obj: Object) -> None: """ - Remove an object from the world. + Removes this object from the current world. + For the object to be removed it has to be detached from all objects it + is currently attached to. After this is done a call to world remove object is done + to remove this Object from the simulation/world. :param obj: The object to be removed. """ - pass + obj.detach_all() + + self.objects.remove(obj) + + # This means the current world of the object is not the prospection world, since it + # has a reference to the prospection world + if self.prospection_world is not None: + self.world_sync.remove_obj_queue.put(self) + self.world_sync.remove_obj_queue.join() + + self.remove_object_from_simulator(obj) + + if World.robot == self: + World.robot = None def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: """ @@ -508,19 +530,21 @@ def get_link_color(self, link: Link) -> Color: """ pass - def get_color_of_object(self, obj: Object) -> Union[Color, Dict[str, Color]]: + def get_object_color(self, obj: Object) -> Union[Color, Dict[str, Color]]: """ This method returns the rgba_color of this object. The return is either: 1. A Color object with RGBA values, this is the case if the object only has one link (this happens for example if the object is spawned from a .obj or .stl file) 2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object. - Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which the - object is spawned. + Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which + the object is spawned. :param obj: The object for which the rgba_color should be returned. + :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and + the rgba_color as value. """ - link_to_color_dict = self.get_colors_of_all_links_of_object(obj) + link_to_color_dict = self.get_colors_of_object_links(obj) if len(link_to_color_dict) == 1: return list(link_to_color_dict.values())[0] @@ -528,7 +552,7 @@ def get_color_of_object(self, obj: Object) -> Union[Color, Dict[str, Color]]: return link_to_color_dict @abstractmethod - def get_colors_of_all_links_of_object(self, obj: Object) -> Dict[str, Color]: + def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: """ Get the RGBA colors of each link in the object as a dictionary from link name to rgba_color. @@ -538,7 +562,7 @@ def get_colors_of_all_links_of_object(self, obj: Object) -> Dict[str, Color]: pass @abstractmethod - def get_object_aabb(self, obj: Object) -> AxisAlignedBoundingBox: + def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. @@ -549,7 +573,7 @@ def get_object_aabb(self, obj: Object) -> AxisAlignedBoundingBox: pass @abstractmethod - def get_object_link_aabb(self, obj_id: int, link_id: int) -> AxisAlignedBoundingBox: + def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. @@ -630,6 +654,7 @@ def robot_is_set() -> bool: def exit(self, wait_time_before_exit_in_secs: Optional[float] = None) -> None: """ Closes the World as well as the prospection world, also collects any other thread that is running. + :param wait_time_before_exit_in_secs: The time to wait before exiting the world in seconds. """ if wait_time_before_exit_in_secs is not None: time.sleep(wait_time_before_exit_in_secs) @@ -640,6 +665,9 @@ def exit(self, wait_time_before_exit_in_secs: Optional[float] = None) -> None: self.join_threads() def exit_prospection_world_if_exists(self): + """ + Exits the prospection world if it exists. + """ if self.prospection_world: self.terminate_world_sync() self.prospection_world.exit() @@ -652,10 +680,16 @@ def disconnect_from_physics_server(self): pass def reset_current_world(self): + """ + Resets the current world to None if this is the current world. + """ if World.current_world == self: World.current_world = None def reset_robot(self): + """ + Sets the robot class variable to None. + """ self.set_robot(None) @abstractmethod @@ -666,13 +700,16 @@ def join_threads(self): pass def terminate_world_sync(self): + """ + Terminates the world sync thread. + """ self.world_sync.terminate = True self.world_sync.join() def save_state(self) -> int: """ - Returns the id of the saved state of the World. The saved state contains the position, orientation and joint - position of every Object in the World, the objects attachments and the constraint ids. + Returns the id of the saved state of the World. The saved state contains the states of all the objects and + the state of the physics simulator. :return: A unique id of the state """ @@ -682,16 +719,21 @@ def save_state(self) -> int: return state_id def save_objects_state(self, state_id: int): + """ + Saves the state of all objects in the World according to the given state using the unique state id. + :param state_id: The unique id representing the state. + """ for obj in self.objects: obj.save_state(state_id) def restore_state(self, state_id) -> None: """ - Restores the state of the World according to the given state using the unique state id. This includes position, - orientation, and joint states. However, restore can not respawn objects if there are objects that were deleted - between creation of the state and restoring, they will be skipped. + Restores the state of the World according to the given state using the unique state id. This includes the state + of the physics simulator and the state of all objects. + However, restore can not respawn objects if there are objects that were deleted between creation of the state + and restoring, they will be skipped. - :param state_id: The unique id representing the state, as returned by :func:`~save_state` + :param state_id: The unique id representing the state. """ self.restore_physics_simulator_state(state_id) self.restore_objects_states(state_id) @@ -700,6 +742,7 @@ def restore_state(self, state_id) -> None: def save_physics_simulator_state(self) -> int: """ Saves the state of the physics simulator and returns the unique id of the state. + :return: The unique id representing the state. """ pass @@ -707,6 +750,7 @@ def save_physics_simulator_state(self) -> int: def remove_physics_simulator_state(self, state_id: int): """ Removes the state of the physics simulator with the given id. + :param state_id: The unique id representing the state. """ pass @@ -715,10 +759,15 @@ def restore_physics_simulator_state(self, state_id: int): """ Restores the objects and environment state in the physics simulator according to the given state using the unique state id. + :param state_id: The unique id representing the state. """ pass def restore_objects_states(self, state_id: int): + """ + Restores the state of all objects in the World according to the given state using the unique state id. + :param state_id: The unique id representing the state. + """ for obj in self.objects: obj.restore_state(state_id) @@ -733,30 +782,13 @@ def get_images_for_target(self, 2. A depth image 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object. - :param cam_pose: The pose of the camera - :param target_pose: The pose to which the camera should point to - :param size: The height and width of the images in pixel + :param target_pose: The pose to which the camera should point. + :param cam_pose: The pose of the camera. + :param size: The height and width of the images in pixels. :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. """ pass - def _copy(self) -> World: - """ - Copies this World into another and returns it. The other World - will be in Direct mode. The prospection world should always be preferred instead of creating a new World. - This method should only be used if necessary since there can be unforeseen problems. - - :return: The reference to the new World - """ - world = World(WorldMode.DIRECT, False, World.simulation_frequency) - for obj in self.objects: - obj_pose = Pose(obj.get_position_as_list(), obj.get_orientation_as_list()) - o = Object(obj.name, obj.obj_type, obj.path, obj_pose, world, - obj.color) - for joint in obj.joint_name_to_id: - o.set_joint_position(joint, obj.get_joint_position(joint)) - return world - def register_two_objects_collision_callbacks(self, object_a: Object, object_b: Object, @@ -784,7 +816,7 @@ def add_resource_path(cls, path: str) -> None: """ cls.data_directory.append(path) - def get_prospection_object_from_object(self, obj: Object) -> Object: + def get_prospection_object_for_object(self, obj: Object) -> Object: """ Returns the corresponding object from the prospection world for a given object in the main world. If the given Object is already in the prospection world, it is returned. @@ -804,7 +836,7 @@ def get_prospection_object_from_object(self, obj: Object) -> Object: f" the object isn't anymore in the main (graphical) World" f" or if the given object is already a prospection object. ") - def get_object_from_prospection_object(self, prospection_object: Object) -> Object: + def get_object_for_prospection_object(self, prospection_object: Object) -> Object: """ Returns the corresponding object from the main World for a given object in the prospection world. If the given object is not in the prospection @@ -825,6 +857,7 @@ def reset_world(self, remove_saved_states=True) -> None: All attached objects will be detached, all joints will be set to the default position of 0 and all objects will be set to the position and orientation in which they were spawned. + :param remove_saved_states: If the saved states should be removed. """ if remove_saved_states: @@ -834,6 +867,9 @@ def reset_world(self, remove_saved_states=True) -> None: obj.reset(remove_saved_states) def remove_saved_states(self): + """ + Removes all saved states of the World. + """ for state_id in self.saved_states: self.remove_physics_simulator_state(state_id) self.saved_states = [] @@ -850,9 +886,9 @@ def update_transforms_for_objects_in_current_world(self) -> None: def ray_test(self, from_position: List[float], to_position: List[float]) -> int: """ Cast a ray and return the first object hit, if any. - param from_position: The starting position of the ray in Cartesian world coordinates. - param to_position: The ending position of the ray in Cartesian world coordinates. - return: The object id of the first object hit, or -1 if no object was hit. + :param from_position: The starting position of the ray in Cartesian world coordinates. + :param to_position: The ending position of the ray in Cartesian world coordinates. + :return: The object id of the first object hit, or -1 if no object was hit. """ pass @@ -864,34 +900,81 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L to compute the ray intersections for the batch. Specify 0 to let simulator decide, 1 (default) for single core execution, 2 or more to select the number of threads to use. - param from_positions: The starting positions of the rays in Cartesian world coordinates. - param to_positions: The ending positions of the rays in Cartesian world coordinates. - param num_threads: The number of threads to use to compute the ray intersections for the batch. + :param from_positions: The starting positions of the rays in Cartesian world coordinates. + :param to_positions: The ending positions of the rays in Cartesian world coordinates. + :param num_threads: The number of threads to use to compute the ray intersections for the batch. """ pass def create_visual_shape(self, visual_shape: VisualShape) -> int: + """ + Creates a visual shape in the physics simulator and returns the unique id of the created shape. + :param visual_shape: The visual shape to be created, uses the VisualShape dataclass defined in world_dataclasses + :return: The unique id of the created shape. + """ raise NotImplementedError def create_multi_body(self, multi_body: MultiBody) -> int: + """ + Creates a multi body in the physics simulator and returns the unique id of the created multi body. The multibody + is created by joining multiple links/shapes together with joints. + :param multi_body: The multi body to be created, uses the MultiBody dataclass defined in world_dataclasses. + :return: The unique id of the created multi body. + """ raise NotImplementedError 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. + :return: The unique id of the created shape. + """ raise NotImplementedError 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. + :return: The unique id of the created shape. + """ raise NotImplementedError 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. + :return: The unique id of the created shape. + """ raise NotImplementedError 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. + :return: The unique id of the created shape. + """ raise NotImplementedError 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. + :return: The unique id of the created shape. + """ raise NotImplementedError def create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: + """ + Creates a mesh visual shape in the physics simulator and returns the unique id of the created shape. + :param shape_data: The parameters that define the mesh visual shape to be created, + uses the MeshVisualShape dataclass defined in world_dataclasses. + :return: The unique id of the created shape. + """ raise NotImplementedError @@ -907,8 +990,12 @@ class UseProspectionWorld: def __init__(self): self.prev_world: Optional[World] = None + # The previous world is saved to restore it after the with block is exited. def __enter__(self): + """ + This method is called when entering the with block, it will set the current world to the prospection world + """ if not World.current_world.is_prospection_world: time.sleep(20 * World.current_world.simulation_time_step) # blocks until the adding queue is ready @@ -919,6 +1006,9 @@ def __enter__(self): World.current_world = World.current_world.prospection_world def __exit__(self, *args): + """ + This method is called when exiting the with block, it will restore the previous world to be the current world. + """ if self.prev_world is not None: World.current_world = self.prev_world World.current_world.world_sync.pause_sync = False @@ -1014,41 +1104,48 @@ def check_for_equal(self) -> None: self.equal_states = eql -@dataclass -class LinkState: - constraint_ids: Dict[Link, int] - - class Link: - + """ + Represents a link of an Object in the World. + """ def __init__(self, _id: int, urdf_link: urdf_parser_py.urdf.Link, obj: Object): - self.id = _id - self.urdf_link = urdf_link - self.object = obj - self.world = obj.world - self.local_transformer = LocalTransformer() + self.id: int = _id + self.urdf_link: urdf_parser_py.urdf.Link = urdf_link + self.object: Object = obj + self.world: World = obj.world + self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} self.saved_states: Dict[int, LinkState] = {} def save_state(self, state_id: int) -> None: """ - Saves the state of this link, this includes the pose of the link. + Saves the state of this link. + :param state_id: The unique id of the state. """ self.saved_states[state_id] = self.get_current_state() def restore_state(self, state_id: int) -> None: """ - Restores the state of this link, this includes the pose of the link. + Restores the state of this link. + :param state_id: The unique id of the state. """ self.constraint_ids = self.saved_states[state_id].constraint_ids def get_current_state(self): + """ + :return: The current state of this link as a LinkState object. + """ return LinkState(self.constraint_ids.copy()) def add_fixed_constraint_with_link(self, child_link: Link) -> 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. + :return: The unique id of the constraint. + """ constraint_id = self.world.add_fixed_constraint(self, child_link, child_link.get_transform_from_link(self)) @@ -1057,125 +1154,217 @@ def add_fixed_constraint_with_link(self, child_link: Link) -> int: return constraint_id def remove_constraint_with_link(self, child_link: Link) -> None: + """ + Removes the constraint between this link and the given link. + :param child_link: The child link of the constraint that should be removed. + """ self.world.remove_constraint(self.constraint_ids[child_link]) del self.constraint_ids[child_link] del child_link.constraint_ids[self] def get_object_id(self) -> int: + """ + Returns the id of the object to which this link belongs. + :return: The integer id of the object to which this link belongs. + """ return self.object.id @property def tf_frame(self) -> str: + """ + Returns the tf frame of this link. + :return: The tf frame of this link as a string. + """ return f"{self.object.tf_frame}/{self.urdf_link.name}" @property def is_root(self) -> bool: + """ + Returns whether this link is the root link of the object. + :return: True if this link is the root link, False otherwise. + """ return self.object.get_root_link_id() == self.id def update_transform(self, transform_time: Optional[rospy.Time] = None): + """ + Updates the transformation of this link at the given time. + :param transform_time: The time at which the transformation should be updated. + """ self.local_transformer.update_transforms([self.transform], transform_time) @property def transform(self) -> Transform: """ The transformation from the world frame to this link frame. + :return: A Transform object with the transformation from the world frame to this link frame. """ return self.pose.to_transform(self.tf_frame) - def get_transform_to_link(self, link: Link): + def get_transform_to_link(self, link: Link) -> Transform: + """ + Returns the transformation from this link to the given link. + :param link: The link to which the transformation should be returned. + :return: A Transform object with the transformation from this link to the given link. + """ return link.get_transform_from_link(self) def get_transform_from_link(self, link: Link) -> Transform: + """ + Returns the transformation from the given link to this link. + :param link: The link from which the transformation should be returned. + :return: A Transform object with the transformation from the given link to this link. + """ return self.get_pose_wrt_link(link).to_transform(self.tf_frame) def get_pose_wrt_link(self, link: Link) -> Pose: + """ + Returns the pose of this link with respect to the given link. + :param link: The link with respect to which the pose should be returned. + :return: A Pose object with the pose of this link with respect to the given link. + """ return self.local_transformer.transform_pose(self.pose, link.tf_frame) - def get_aabb(self) -> AxisAlignedBoundingBox: - return self.world.get_object_link_aabb(self.object.id, self.id) + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + Returns the axis aligned bounding box of this link. + :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. + """ + return self.world.get_link_axis_aligned_bounding_box(self) @property - def position(self) -> Pose.position: + def position(self) -> Point: + """ + The getter for the position of the link relative to the world frame. + :return: A Point object containing the position of the link relative to the world frame. + """ return self.pose.position @property def position_as_list(self) -> List[float]: + """ + The getter for the position of the link relative to the world frame as a list. + :return: A list containing the position of the link relative to the world frame. + """ return self.pose.position_as_list() @property def orientation(self) -> Quaternion: + """ + The getter for the orientation of the link relative to the world frame. + :return: A Quaternion object containing the orientation of the link relative to the world frame. + """ return self.pose.orientation @property def orientation_as_list(self) -> List[float]: + """ + The getter for the orientation of the link relative to the world frame as a list. + :return: A list containing the orientation of the link relative to the world frame. + """ return self.pose.orientation_as_list() @property def pose(self) -> Pose: """ The pose of the link relative to the world frame. + :return: A Pose object containing the pose of the link relative to the world frame. """ return self.world.get_link_pose(self) @property def pose_as_list(self) -> List[List[float]]: + """ + The pose of the link relative to the world frame as a list. + :return: A list containing the position and orientation of the link relative to the world frame. + """ return self.pose.to_list() @property def name(self) -> str: + """ + The name of the link as defined in the URDF. + :return: The name of the link as a string. + """ return self.urdf_link.name def get_geometry(self) -> GeometricType: + """ + Returns the geometry type of the URDF collision element of this link. + """ return None if not self.collision else self.collision.geometry - def get_origin_transform(self): + def get_origin_transform(self) -> Transform: + """ + Returns the transformation between the link frame and the origin frame of this link. + """ return Transform(self.origin.xyz, list(quaternion_from_euler(*self.origin.rpy))) @property - def origin(self): + def origin(self) -> urdf_parser_py.urdf.Pose: + """ + The URDF origin pose of this link. + :return: A '~urdf_parser_py.urdf.Pose' object containing the origin pose of this link. + """ return self.collision.origin @property def collision(self) -> Collision: + """ + The URDF collision element of this link which has a geometry, and origin. + :return: A '~urdf_parser_py.urdf.Collision' object containing the collision element of this link. + """ return self.urdf_link.collision @property def color(self) -> Color: + """ + The getter for the rgba_color of this link. + :return: A Color object containing the rgba_color of this link. + """ return self.world.get_link_color(self) @color.setter def color(self, color: List[float]) -> None: - self.world.set_link_color(self, Color.from_rgba(color)) + """ + The setter for the color of this link, could be rgb or rgba. + :param color: The color as a list of floats, either rgb or rgba. + """ + self.world.set_link_color(self, Color.from_list(color)) class RootLink(Link): - + """ + Represents the root link of an Object in the World. + It differs from the normal Link class in that the pose ande the tf_frame is the same as that of the object. + """ def __init__(self, obj: Object): super().__init__(obj.get_root_link_id(), obj.get_root_urdf_link(), obj) @property def tf_frame(self) -> str: + """ + Returns the tf frame of the root link, which is the same as the tf frame of the object. + :return: A string containing the tf frame of the root link. + """ return self.object.tf_frame @property def pose(self) -> Pose: + """ + Returns the pose of the root link, which is the same as the pose of the object. + :return: A Pose object containing the pose of the root link. + """ return self.object.get_pose() -@dataclass -class ObjectState: - state_id: int - attachments: Dict[Object, Attachment] - - class Object: """ Represents a spawned Object in the World. """ - def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, - pose: Pose = None, - world: World = None, + def __init__(self, name: str, obj_type: ObjectType, path: str, + pose: Optional[Pose] = None, + world: Optional[World] = None, color: Optional[Color] = Color(), ignore_cached_files: Optional[bool] = False): """ @@ -1185,7 +1374,7 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, for URDFs :func:`~Object.set_color` can be used. :param name: The name of the object - :param obj_type: The type of the object + :param obj_type: The type of the object as an ObjectType enum. :param path: The path to the source file, if only a filename is provided then the resourcer directories will be searched :param pose: The pose at which the Object should be spawned @@ -1201,44 +1390,143 @@ def __init__(self, name: str, obj_type: Union[str, ObjectType], path: str, self.world: World = world if world is not None else World.current_world self.name: str = name - self.obj_type: Union[str, ObjectType] = obj_type + self.obj_type: ObjectType = obj_type self.color: Color = color self.local_transformer = LocalTransformer() self.original_pose = self.local_transformer.transform_pose(pose, "map") - position, orientation = self.original_pose.to_list() - self.id, self.path = _load_object(name, path, position, orientation, self.world, color, ignore_cached_files) self._current_pose = self.original_pose - self.joint_name_to_id: Dict[str, int] = self._get_joint_name_to_id_map() - self.link_name_to_id: Dict[str, int] = self._get_link_name_to_id_map() - self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) + self.id, self.path = self._load_object(path, ignore_cached_files) + + self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) + + self._init_urdf_object() + if self.urdf_object.name == robot_description.name: + self.world.set_robot_if_not_set(self) + + self._init_joint_name_and_id_map() + + self._init_link_name_and_id_map() + + self._init_links() + self.update_link_transforms() self.attachments: Dict[Object, Attachment] = {} self.saved_states: Dict[int, ObjectState] = {} # takes the state id as key and returns the attachments of the object at that state - self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) - if not self.world.is_prospection_world: - self.world.world_sync.add_obj_queue.put( - [name, obj_type, path, position, orientation, self.world.prospection_world, color, self]) + self._add_to_world_sync_obj_queue(path) + self._init_current_positions_of_joint() + + self.world.objects.append(self) + + def _load_object(self, path: str, ignore_cached_files: bool) -> Tuple[int, str]: + """ + Loads an object to the given World with the given position and orientation. The rgba_color will only be + used when an .obj or .stl file is given. + If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created + and this URDf file will be loaded instead. + When spawning a URDf file a new file will be created in the cache directory, if there exists none. + This new file will have resolved mesh file paths, meaning there will be no references + to ROS packges instead there will be absolute file paths. + + :param path: The path to the source file or the name on the ROS parameter server + :param ignore_cached_files: Whether to ignore files in the cache directory. + :return: The unique id of the object and the path to the file used for spawning + """ + pa = pathlib.Path(path) + extension = pa.suffix + if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): + for data_dir in World.data_directory: + path = get_path_from_data_dir(path, data_dir) + if path: + break + + if not path: + raise FileNotFoundError( + f"File {pa.name} could not be found in the resource directory {World.data_directory}") + # rospack = rospkg.RosPack() + # cach_dir = rospack.get_path('pycram') + '/resources/cached/' + cach_dir = World.data_directory[0] + '/cached/' + if not pathlib.Path(cach_dir).exists(): + os.mkdir(cach_dir) + + # if file is not yet cached corrcet the urdf and save if in the cache directory + if not _is_cached(path, self.name, cach_dir) or ignore_cached_files: + if extension == ".obj" or extension == ".stl": + path = _generate_urdf_file(self.name, path, self.color, cach_dir) + elif extension == ".urdf": + with open(path, mode="r") as f: + urdf_string = fix_missing_inertial(f.read()) + urdf_string = remove_error_tags(urdf_string) + urdf_string = fix_link_attributes(urdf_string) + try: + urdf_string = _correct_urdf_string(urdf_string) + except rospkg.ResourceNotFound as e: + rospy.logerr(f"Could not find resource package linked in this URDF") + raise e + path = cach_dir + pa.name + with open(path, mode="w") as f: + f.write(urdf_string) + else: # Using the urdf from the parameter server + urdf_string = rospy.get_param(path) + path = cach_dir + self.name + ".urdf" + with open(path, mode="w") as f: + f.write(_correct_urdf_string(urdf_string)) + # save correct path in case the file is already in the cache directory + elif extension == ".obj" or extension == ".stl": + path = cach_dir + pa.stem + ".urdf" + elif extension == ".urdf": + path = cach_dir + pa.name + else: + path = cach_dir + self.name + ".urdf" + + try: + obj_id = self.world.load_urdf_and_get_object_id(path, Pose(self.get_position_as_list(), + self.get_orientation_as_list())) + return obj_id, path + except Exception as e: + logging.error( + "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" + " the name of an URDF string on the parameter server.") + os.remove(path) + raise (e) + + def _init_urdf_object(self): + """ + Initializes the URDF object from the URDF file. + """ with open(self.path) as f: self.urdf_object = URDF.from_xml_string(f.read()) - if self.urdf_object.name == robot_description.name: - self.world.set_robot_if_not_set(self) - self.link_name_to_id[self.urdf_object.get_root()] = -1 - self.link_id_to_name[-1] = self.urdf_object.get_root() - self.links: Dict[str, Link] = self._init_links() - self.update_link_transforms() - self._current_joints_positions = {} - self._init_current_positions_of_joint() + def _init_joint_name_and_id_map(self) -> None: + """ + Creates a dictionary which maps the joint names to their unique ids and vice versa. + """ + joint_names = self.world.get_joint_names(self) + n_joints = len(joint_names) + self.joint_name_to_id = dict(zip(joint_names, range(n_joints))) + self.joint_id_to_name = dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys())) - self.world.objects.append(self) + def _init_link_name_and_id_map(self) -> None: + """ + Creates a dictionary which maps the link names to their unique ids and vice versa. + """ + link_names = self.world.get_link_names(self) + n_links = len(link_names) + self.link_name_to_id: Dict[str, int] = dict(zip(link_names, range(n_links))) + self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) + self.link_name_to_id[self.urdf_object.get_root()] = -1 + self.link_id_to_name[-1] = self.urdf_object.get_root() def _init_links(self): + """ + Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the + corresponding link objects. + """ links = {} for urdf_link in self.urdf_object.links: link_name = urdf_link.name @@ -1247,12 +1535,21 @@ def _init_links(self): links[link_name] = RootLink(self) else: links[link_name] = Link(link_id, urdf_link, self) - return links + self.links = links + + def _add_to_world_sync_obj_queue(self, path: str): + """ + Adds this object to the objects queue of the WorldSync object of the World. + """ + self.world.world_sync.add_obj_queue.put( + [self.name, self.obj_type, path, self.get_position_as_list(), self.get_orientation_as_list(), + self.world.prospection_world, self.color, self]) def _init_current_positions_of_joint(self) -> None: """ Initialize the cached joint position for each joint. """ + self._current_joints_positions = {} for joint_name in self.joint_name_to_id.keys(): self._current_joints_positions[joint_name] = self.world.get_object_joint_position(self, joint_name) @@ -1260,6 +1557,7 @@ def _init_current_positions_of_joint(self) -> None: def base_origin_shift(self) -> np.ndarray: """ The shift between the base of the object and the origin of the object. + :return: A numpy array with the shift between the base of the object and the origin of the object. """ return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) @@ -1275,21 +1573,8 @@ def remove(self) -> None: is currently attached to. After this is done a call to world remove object is done to remove this Object from the simulation/world. """ - self.detach_all() - - self.world.objects.remove(self) - - # This means the current world of the object is not the prospection world, since it - # has a reference to the prospection world - if self.world.prospection_world is not None: - self.world.world_sync.remove_obj_queue.put(self) - self.world.world_sync.remove_obj_queue.join() - self.world.remove_object(self) - if World.robot == self: - World.robot = None - def reset(self, remove_saved_states=True) -> None: """ Resets the Object to the state it was first spawned in. @@ -1520,22 +1805,6 @@ def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: pose.pose.orientation = target_orientation self.set_pose(pose) - def _get_joint_name_to_id_map(self) -> Dict[str, int]: - """ - Creates a dictionary which maps the joint names to their unique ids. - """ - joint_names = self.world.get_joint_names(self) - n_joints = len(joint_names) - return dict(zip(joint_names, range(n_joints))) - - def _get_link_name_to_id_map(self) -> Dict[str, int]: - """ - Creates a dictionary which maps the link names to their unique ids. - """ - link_names = self.world.get_link_names(self) - n_links = len(link_names) - return dict(zip(link_names, range(n_links))) - def get_joint_id(self, name: str) -> int: """ Returns the unique id for a joint name. As used by the world/simulator. @@ -1700,10 +1969,10 @@ def set_color(self, color: Color) -> None: self.world.set_object_color(self, color) def get_color(self) -> Union[Color, Dict[str, Color]]: - return self.world.get_color_of_object(self) + return self.world.get_object_color(self) def get_aabb(self) -> AxisAlignedBoundingBox: - return self.world.get_object_aabb(self) + return self.world.get_object_axis_aligned_bounding_box(self) def get_base_origin(self) -> Pose: """ @@ -1711,7 +1980,7 @@ def get_base_origin(self) -> Pose: :return: The position of the bottom of this Object """ - aabb = self.get_link_by_id(-1).get_aabb() + aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() base_width = np.absolute(aabb.min_x - aabb.max_x) base_length = np.absolute(aabb.min_y - aabb.max_y) return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], @@ -1896,88 +2165,6 @@ def __del__(self): self.remove_constraint_if_exists() -def _load_object(name: str, - path: str, - position: List[float], - orientation: List[float], - world: World, - color: Color, - ignore_cached_files: bool) -> Tuple[int, str]: - """ - Loads an object to the given World with the given position and orientation. The rgba_color will only be - used when an .obj or .stl file is given. - If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created - and this URDf file will be loaded instead. - When spawning a URDf file a new file will be created in the cache directory, if there exists none. - This new file will have resolved mesh file paths, meaning there will be no references - to ROS packges instead there will be absolute file paths. - - :param name: The name of the object which should be spawned - :param path: The path to the source file or the name on the ROS parameter server - :param position: The position in which the object should be spawned - :param orientation: The orientation in which the object should be spawned - :param world: The World to which the Object should be spawned - :param color: The rgba_color of the object, only used when .obj or .stl file is given - :param ignore_cached_files: Whether to ignore files in the cache directory. - :return: The unique id of the object and the path to the file used for spawning - """ - pa = pathlib.Path(path) - extension = pa.suffix - world, world_id = _world_and_id(world) - if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): - for dir in world.data_directory: - path = get_path_from_data_dir(path, dir) - if path: break - - if not path: - raise FileNotFoundError( - f"File {pa.name} could not be found in the resource directory {world.data_directory}") - # rospack = rospkg.RosPack() - # cach_dir = rospack.get_path('pycram') + '/resources/cached/' - cach_dir = world.data_directory[0] + '/cached/' - if not pathlib.Path(cach_dir).exists(): - os.mkdir(cach_dir) - - # if file is not yet cached corrcet the urdf and save if in the cache directory - if not _is_cached(path, name, cach_dir) or ignore_cached_files: - if extension == ".obj" or extension == ".stl": - path = _generate_urdf_file(name, path, color, cach_dir) - elif extension == ".urdf": - with open(path, mode="r") as f: - urdf_string = fix_missing_inertial(f.read()) - urdf_string = remove_error_tags(urdf_string) - urdf_string = fix_link_attributes(urdf_string) - try: - urdf_string = _correct_urdf_string(urdf_string) - except rospkg.ResourceNotFound as e: - rospy.logerr(f"Could not find resource package linked in this URDF") - raise e - path = cach_dir + pa.name - with open(path, mode="w") as f: - f.write(urdf_string) - else: # Using the urdf from the parameter server - urdf_string = rospy.get_param(path) - path = cach_dir + name + ".urdf" - with open(path, mode="w") as f: - f.write(_correct_urdf_string(urdf_string)) - # save correct path in case the file is already in the cache directory - elif extension == ".obj" or extension == ".stl": - path = cach_dir + pa.stem + ".urdf" - elif extension == ".urdf": - path = cach_dir + pa.name - else: - path = cach_dir + name + ".urdf" - - try: - obj = world.load_urdf_and_get_object_id(path, Pose(position, orientation)) - return obj, path - except Exception as e: - logging.error( - "The File could not be loaded. Plese note that the path has to be either a URDF, stl or obj file or the name of an URDF string on the parameter server.") - os.remove(path) - raise (e) - - def _is_cached(path: str, name: str, cach_dir: str) -> bool: """ Checks if the file in the given path is already cached or if diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 474c6f86d..6eff03e2a 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from typing import List, Optional, Tuple, Callable +from typing_extensions import List, Optional, Tuple, Callable, Dict from .enums import JointType, Shape @@ -7,12 +7,37 @@ class Color: """ Dataclass for storing rgba_color as an RGBA value. + The values are stored as floats between 0 and 1. + The default rgba_color is white. 'A' stands for the opacity. """ R: float = 1 G: float = 1 B: float = 1 A: float = 1 + @classmethod + def from_list(cls, color: List[float]): + """ + Sets the rgba_color from a list of RGBA values. + + :param color: The list of RGBA values + """ + if len(color) == 3: + return cls.from_rgb(color) + elif len(color) == 4: + return cls.from_rgba(color) + else: + raise ValueError("Color list must have 3 or 4 elements") + + @classmethod + def from_rgb(cls, rgb: List[float]): + """ + Sets the rgba_color from a list of RGB values. + + :param rgb: The list of RGB values + """ + return cls(rgb[0], rgb[1], rgb[2], 1) + @classmethod def from_rgba(cls, rgba: List[float]): """ @@ -235,3 +260,14 @@ class MeshVisualShape(VisualShape): class PlaneVisualShape(VisualShape): shape_data: PlaneShapeData visual_geometry_type = Shape.PLANE + + +@dataclass +class LinkState: + constraint_ids: Dict['Link', int] + + +@dataclass +class ObjectState: + state_id: int + attachments: Dict['Object', 'Attachment'] diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 35fca3d44..f9fc06319 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -29,7 +29,7 @@ def stable(obj: Object) -> bool: :param obj: The object which should be checked :return: True if the given object is stable in the world False else """ - prospection_obj = World.current_world.get_prospection_object_from_object(obj) + prospection_obj = World.current_world.get_prospection_object_for_object(obj) with UseProspectionWorld(): coords_prev = prospection_obj.get_position_as_list() World.current_world.set_gravity([0, 0, -9.8]) @@ -58,8 +58,8 @@ def contact( """ with UseProspectionWorld(): - prospection_obj1 = World.current_world.get_prospection_object_from_object(object1) - prospection_obj2 = World.current_world.get_prospection_object_from_object(object2) + prospection_obj1 = World.current_world.get_prospection_object_for_object(object1) + prospection_obj2 = World.current_world.get_prospection_object_for_object(object2) World.current_world.perform_collision_detection() con_points = World.current_world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) @@ -108,9 +108,9 @@ def visible( :return: True if the object is visible from the camera_position False if not """ with UseProspectionWorld(): - prospection_obj = World.current_world.get_prospection_object_from_object(obj) + prospection_obj = World.current_world.get_prospection_object_for_object(obj) if World.robot: - prospection_robot = World.current_world.get_prospection_object_from_object(World.robot) + prospection_robot = World.current_world.get_prospection_object_for_object(World.robot) state_id = World.current_world.save_state() for obj in World.current_world.objects: @@ -176,7 +176,7 @@ def occluding( occluding_obj_ids.append(seg_mask[c[0]][c[1]]) occ_objects = list(set(map(World.current_world.get_object_by_id, occluding_obj_ids))) - occ_objects = list(map(World.current_world.get_object_from_prospection_object, occ_objects)) + occ_objects = list(map(World.current_world.get_object_for_prospection_object, occ_objects)) return occ_objects @@ -198,7 +198,7 @@ def reachable( :return: True if the end effector is closer than the threshold to the target position, False in every other case """ - prospection_robot = World.current_world.get_prospection_object_from_object(robot) + prospection_robot = World.current_world.get_prospection_object_for_object(robot) with UseProspectionWorld(): target_pose = try_to_reach(pose_or_object, prospection_robot, gripper_name) @@ -230,7 +230,7 @@ def blocking( or object is not reachable. """ - prospection_robot = World.current_world.get_prospection_object_from_object(robot) + prospection_robot = World.current_world.get_prospection_object_for_object(robot) with UseProspectionWorld(): if grasp: try_to_reach_with_grasp(pose_or_object, prospection_robot, gripper_name, grasp) @@ -240,7 +240,7 @@ def blocking( block = [] for obj in World.current_world.objects: if contact(prospection_robot, obj): - block.append(World.current_world.get_object_from_prospection_object(obj)) + block.append(World.current_world.get_object_for_prospection_object(obj)) return block @@ -272,7 +272,7 @@ def link_pose_for_joint_config( :param link_name: Name of the link for which the pose should be returned :return: The pose of the link after applying the joint configuration """ - prospection_object = World.current_world.get_prospection_object_from_object(obj) + prospection_object = World.current_world.get_prospection_object_for_object(obj) with UseProspectionWorld(): for joint, pose in joint_config.items(): prospection_object.set_joint_position(joint, pose) From eace428cd55f1ba1049af6889b13e73d11e5a426 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 30 Jan 2024 17:26:01 +0100 Subject: [PATCH 41/69] [Abstract World] Cleaned Object constructor, added more internal methods to Object class. --- src/pycram/world.py | 209 ++++++++++++++++++++------------------------ 1 file changed, 95 insertions(+), 114 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 6ca2bd4bc..94ac24184 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -67,6 +67,11 @@ class World(ABC): Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. """ + cach_dir = data_directory[0] + '/cached/' + """ + Global reference for the cache directory, this is used to cache the URDF files of the robot and the objects. + """ + 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 @@ -390,26 +395,6 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ pass - def get_joint_ranges(self) -> Tuple[List, List, List, List, List]: - """ - Calculates the lower and upper limits, the joint ranges and the joint damping. For a given robot Object. - Fixed joints will be skipped because they don't have limits or ranges. - - :return: The lists for the upper and lower limits, joint ranges, rest poses and joint damping - """ - ll, ul, jr, rp, jd = [], [], [], [], [] - joint_names = self.get_joint_names(self.robot) - for name in joint_names: - joint_type = self.get_object_joint_type(self.robot, name) - if joint_type != JointType.FIXED: - ll.append(self.get_object_joint_lower_limit(self.robot, name)) - ul.append(self.get_object_joint_upper_limit(self.robot, name)) - jr.append(ul[-1] - ll[-1]) - rp.append(self.get_joint_rest_pose(self.robot, name)) - jd.append(self.get_joint_damping(self.robot, name)) - - return ll, ul, jr, rp, jd - def get_joint_rest_pose(self, obj: Object, joint_name: str) -> float: """ Get the rest pose of a joint of an articulated object @@ -664,7 +649,7 @@ def exit(self, wait_time_before_exit_in_secs: Optional[float] = None) -> None: self.reset_robot() self.join_threads() - def exit_prospection_world_if_exists(self): + def exit_prospection_world_if_exists(self) -> None: """ Exits the prospection world if it exists. """ @@ -673,33 +658,33 @@ def exit_prospection_world_if_exists(self): self.prospection_world.exit() @abstractmethod - def disconnect_from_physics_server(self): + def disconnect_from_physics_server(self) -> None: """ Disconnects the world from the physics server. """ pass - def reset_current_world(self): + def reset_current_world(self) -> None: """ Resets the current world to None if this is the current world. """ if World.current_world == self: World.current_world = None - def reset_robot(self): + def reset_robot(self) -> None: """ Sets the robot class variable to None. """ self.set_robot(None) @abstractmethod - def join_threads(self): + def join_threads(self) -> None: """ Join any running threads. Useful for example when exiting the world. """ pass - def terminate_world_sync(self): + def terminate_world_sync(self) -> None: """ Terminates the world sync thread. """ @@ -718,7 +703,7 @@ def save_state(self) -> int: self.saved_states.append(state_id) return state_id - def save_objects_state(self, state_id: int): + def save_objects_state(self, state_id: int) -> None: """ Saves the state of all objects in the World according to the given state using the unique state id. :param state_id: The unique id representing the state. @@ -747,7 +732,7 @@ def save_physics_simulator_state(self) -> int: pass @abstractmethod - def remove_physics_simulator_state(self, state_id: int): + def remove_physics_simulator_state(self, state_id: int) -> None: """ Removes the state of the physics simulator with the given id. :param state_id: The unique id representing the state. @@ -755,7 +740,7 @@ def remove_physics_simulator_state(self, state_id: int): pass @abstractmethod - def restore_physics_simulator_state(self, state_id: int): + def restore_physics_simulator_state(self, state_id: int) -> None: """ Restores the objects and environment state in the physics simulator according to the given state using the unique state id. @@ -763,7 +748,7 @@ def restore_physics_simulator_state(self, state_id: int): """ pass - def restore_objects_states(self, state_id: int): + def restore_objects_states(self, state_id: int) -> None: """ Restores the state of all objects in the World according to the given state using the unique state id. :param state_id: The unique id representing the state. @@ -866,7 +851,7 @@ def reset_world(self, remove_saved_states=True) -> None: for obj in self.objects: obj.reset(remove_saved_states) - def remove_saved_states(self): + def remove_saved_states(self) -> None: """ Removes all saved states of the World. """ @@ -1134,7 +1119,7 @@ def restore_state(self, state_id: int) -> None: """ self.constraint_ids = self.saved_states[state_id].constraint_ids - def get_current_state(self): + def get_current_state(self) -> LinkState: """ :return: The current state of this link as a LinkState object. """ @@ -1185,7 +1170,7 @@ def is_root(self) -> bool: """ return self.object.get_root_link_id() == self.id - def update_transform(self, transform_time: Optional[rospy.Time] = None): + def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: """ Updates the transformation of this link at the given time. :param transform_time: The time at which the transformation should be updated. @@ -1393,15 +1378,14 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.obj_type: ObjectType = obj_type self.color: Color = color - self.local_transformer = LocalTransformer() - self.original_pose = self.local_transformer.transform_pose(pose, "map") - self._current_pose = self.original_pose + self._init_local_transformer_and_pose(pose) - self.id, self.path = self._load_object(path, ignore_cached_files) + self._load_object_and_set_id_and_path(path, ignore_cached_files) self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) self._init_urdf_object() + if self.urdf_object.name == robot_description.name: self.world.set_robot_if_not_set(self) @@ -1423,7 +1407,16 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) - def _load_object(self, path: str, ignore_cached_files: bool) -> Tuple[int, str]: + def _init_local_transformer_and_pose(self, pose: Pose) -> None: + """ + Initializes the local transformer and the pose of this object at the given pose and transforms it to the map + frame. + """ + self.local_transformer = LocalTransformer() + self.original_pose = self.local_transformer.transform_pose(pose, "map") + self._current_pose = self.original_pose + + def _load_object_and_set_id_and_path(self, path, ignore_cached_files: bool) -> None: """ Loads an object to the given World with the given position and orientation. The rgba_color will only be used when an .obj or .stl file is given. @@ -1433,9 +1426,7 @@ def _load_object(self, path: str, ignore_cached_files: bool) -> Tuple[int, str]: This new file will have resolved mesh file paths, meaning there will be no references to ROS packges instead there will be absolute file paths. - :param path: The path to the source file or the name on the ROS parameter server :param ignore_cached_files: Whether to ignore files in the cache directory. - :return: The unique id of the object and the path to the file used for spawning """ pa = pathlib.Path(path) extension = pa.suffix @@ -1448,16 +1439,13 @@ def _load_object(self, path: str, ignore_cached_files: bool) -> Tuple[int, str]: if not path: raise FileNotFoundError( f"File {pa.name} could not be found in the resource directory {World.data_directory}") - # rospack = rospkg.RosPack() - # cach_dir = rospack.get_path('pycram') + '/resources/cached/' - cach_dir = World.data_directory[0] + '/cached/' - if not pathlib.Path(cach_dir).exists(): - os.mkdir(cach_dir) + if not pathlib.Path(World.cach_dir).exists(): + os.mkdir(World.cach_dir) # if file is not yet cached corrcet the urdf and save if in the cache directory - if not _is_cached(path, self.name, cach_dir) or ignore_cached_files: + if not _is_cached(path) or ignore_cached_files: if extension == ".obj" or extension == ".stl": - path = _generate_urdf_file(self.name, path, self.color, cach_dir) + path = self._generate_urdf_file(path) elif extension == ".urdf": with open(path, mode="r") as f: urdf_string = fix_missing_inertial(f.read()) @@ -1468,26 +1456,27 @@ def _load_object(self, path: str, ignore_cached_files: bool) -> Tuple[int, str]: except rospkg.ResourceNotFound as e: rospy.logerr(f"Could not find resource package linked in this URDF") raise e - path = cach_dir + pa.name + path = World.cach_dir + pa.name with open(path, mode="w") as f: f.write(urdf_string) else: # Using the urdf from the parameter server urdf_string = rospy.get_param(path) - path = cach_dir + self.name + ".urdf" + path = World.cach_dir + self.name + ".urdf" with open(path, mode="w") as f: f.write(_correct_urdf_string(urdf_string)) # save correct path in case the file is already in the cache directory elif extension == ".obj" or extension == ".stl": - path = cach_dir + pa.stem + ".urdf" + path = World.cach_dir + pa.stem + ".urdf" elif extension == ".urdf": - path = cach_dir + pa.name + path = World.cach_dir + pa.name else: - path = cach_dir + self.name + ".urdf" + path = World.cach_dir + self.name + ".urdf" try: obj_id = self.world.load_urdf_and_get_object_id(path, Pose(self.get_position_as_list(), self.get_orientation_as_list())) - return obj_id, path + self.id = obj_id + self.path = path except Exception as e: logging.error( "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" @@ -1495,7 +1484,42 @@ def _load_object(self, path: str, ignore_cached_files: bool) -> Tuple[int, str]: os.remove(path) raise (e) - def _init_urdf_object(self): + def _generate_urdf_file(self, path) -> str: + """ + Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be + used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with + the name as filename. + + :return: The absolute path of the created file + """ + urdf_template = ' \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + ' + urdf_template = fix_missing_inertial(urdf_template) + rgb = " ".join(list(map(str, self.color.get_rgba()))) + pathlib_obj = pathlib.Path(path) + path = str(pathlib_obj.resolve()) + content = urdf_template.replace("~a", self.name).replace("~b", path).replace("~c", rgb) + with open(World.cach_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: + file.write(content) + return World.cach_dir + pathlib_obj.stem + ".urdf" + + def _init_urdf_object(self) -> None: """ Initializes the URDF object from the URDF file. """ @@ -1522,7 +1546,7 @@ def _init_link_name_and_id_map(self) -> None: self.link_name_to_id[self.urdf_object.get_root()] = -1 self.link_id_to_name[-1] = self.urdf_object.get_root() - def _init_links(self): + def _init_links(self) -> None: """ Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the corresponding link objects. @@ -2165,29 +2189,6 @@ def __del__(self): self.remove_constraint_if_exists() -def _is_cached(path: str, name: str, cach_dir: str) -> bool: - """ - Checks if the file in the given path is already cached or if - there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from - the parameter server is used. - - :param path: The path given by the user to the source file. - :param name: The name for this object. - :param cach_dir: The absolute path the cach directory in the pycram package. - :return: True if there already exists a chached file, False in any other case. - """ - file_name = pathlib.Path(path).name - full_path = pathlib.Path(cach_dir + file_name) - if full_path.exists(): - return True - # Returns filename without the filetype, e.g. returns "test" for "test.txt" - file_stem = pathlib.Path(path).stem - full_path = pathlib.Path(cach_dir + file_stem + ".urdf") - if full_path.exists(): - return True - return False - - def _correct_urdf_string(urdf_string: str) -> str: """ Changes paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legac) @@ -2273,44 +2274,24 @@ def fix_link_attributes(urdf_string: str) -> str: return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') -def _generate_urdf_file(name: str, path: str, color: Color, cach_dir: str) -> str: +def _is_cached(path) -> bool: """ - Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be - used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with the name - as filename. - - :param name: The name of the object - :param path: The path to the .obj or .stl file - :param color: The rgba_color which should be used for the material tag - :param cach_dir The absolute file path to the cach directory in the pycram package - :return: The absolute path of the created file + Checks if the file in the given path is already cached or if + there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from + the parameter server is used. + + :return: True if there already exists a cached file, False in any other case. """ - urdf_template = ' \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - ' - urdf_template = fix_missing_inertial(urdf_template) - rgb = " ".join(list(map(str, color.get_rgba()))) - pathlib_obj = pathlib.Path(path) - path = str(pathlib_obj.resolve()) - content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) - with open(cach_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: - file.write(content) - return cach_dir + pathlib_obj.stem + ".urdf" + file_name = pathlib.Path(path).name + full_path = pathlib.Path(World.cach_dir + file_name) + if full_path.exists(): + return True + # Returns filename without the filetype, e.g. returns "test" for "test.txt" + file_stem = pathlib.Path(path).stem + full_path = pathlib.Path(World.cach_dir + file_stem + ".urdf") + if full_path.exists(): + return True + return False def _world_and_id(world: World) -> Tuple[World, int]: From 2a8d4fa1c9d55525569657e10dbdc6dfcafa44b0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jan 2024 10:26:44 +0100 Subject: [PATCH 42/69] [Abstract World] Documented internal functions. --- src/pycram/designators/location_designator.py | 2 +- src/pycram/world.py | 150 +++++++++++++----- 2 files changed, 111 insertions(+), 41 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 6060d0205..f525593bd 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -336,7 +336,7 @@ def __iter__(self): sem_costmap = SemanticCostmap(self.part_of.world_object, self.urdf_link_name) height_offset = 0 if self.for_object: - min_p, max_p = self.for_object.world_object.get_aabb().get_min_max_points() + min_p, max_p = self.for_object.world_object.get_axis_aligned_bounding_box().get_min_max_points() height_offset = (max_p.z - min_p.z) / 2 for maybe_pose in pose_generator(sem_costmap): maybe_pose.position.z += height_offset diff --git a/src/pycram/world.py b/src/pycram/world.py index 94ac24184..0a859b2a7 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -1378,7 +1378,9 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.obj_type: ObjectType = obj_type self.color: Color = color - self._init_local_transformer_and_pose(pose) + self.local_transformer = LocalTransformer() + self.original_pose = self.local_transformer.transform_pose(pose, "map") + self._current_pose = self.original_pose self._load_object_and_set_id_and_path(path, ignore_cached_files) @@ -1407,14 +1409,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) - def _init_local_transformer_and_pose(self, pose: Pose) -> None: - """ - Initializes the local transformer and the pose of this object at the given pose and transforms it to the map - frame. - """ - self.local_transformer = LocalTransformer() - self.original_pose = self.local_transformer.transform_pose(pose, "map") - self._current_pose = self.original_pose def _load_object_and_set_id_and_path(self, path, ignore_cached_files: bool) -> None: """ @@ -1561,9 +1555,10 @@ def _init_links(self) -> None: links[link_name] = Link(link_id, urdf_link, self) self.links = links - def _add_to_world_sync_obj_queue(self, path: str): + def _add_to_world_sync_obj_queue(self, path: str) -> None: """ Adds this object to the objects queue of the WorldSync object of the World. + :param path: The path of the URDF file of this object. """ self.world.world_sync.add_obj_queue.put( [self.name, self.obj_type, path, self.get_position_as_list(), self.get_orientation_as_list(), @@ -1605,6 +1600,7 @@ def reset(self, remove_saved_states=True) -> None: All attached objects will be detached, all joints will be set to the default position of 0 and the object will be set to the position and orientation in which it was spawned. + :param remove_saved_states: If True the saved states will be removed. """ self.detach_all() self.reset_all_joints_positions() @@ -1734,26 +1730,49 @@ def move_base_to_origin_pos(self) -> None: """ self.set_pose(self.get_pose(), base=True) - def save_state(self, state_id): + def save_state(self, state_id) -> None: + """ + Saves the state of this object by saving the state of all links and attachments. + :param state_id: The unique id of the state. + """ self.save_links_states(state_id) self.saved_states[state_id] = ObjectState(state_id, self.attachments.copy()) - def save_links_states(self, state_id: int): + def save_links_states(self, state_id: int) -> None: + """ + Saves the state of all links of this object. + :param state_id: The unique id of the state. + """ for link in self.links.values(): link.save_state(state_id) - def restore_state(self, state_id: int): + def restore_state(self, state_id: int) -> None: + """ + Restores the state of this object by restoring the state of all links and attachments. + :param state_id: The unique id of the state. + """ self.restore_links_states(state_id) self.restore_attachments(state_id) - def restore_attachments(self, state_id): + def restore_attachments(self, state_id) -> None: + """ + Restores the attachments of this object from a saved state using the given state id. + :param state_id: The unique id of the state. + """ self.attachments = self.saved_states[state_id].attachments - def restore_links_states(self, state_id): + def restore_links_states(self, state_id) -> None: + """ + Restores the states of all links of this object from a saved state using the given state id. + :param state_id: The unique id of the state. + """ for link in self.links.values(): link.restore_state(state_id) - def remove_saved_states(self): + def remove_saved_states(self) -> None: + """ + Removes all saved states of this object. + """ self.saved_states = {} for link in self.links.values(): link.saved_states = {} @@ -1766,7 +1785,8 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec After this the _set_attached_objects method of all attached objects will be called. - :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops in the update. + :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent + loops in the update. """ if already_moved_objects is None: @@ -1841,6 +1861,7 @@ def get_joint_id(self, name: str) -> int: def get_root_urdf_link(self) -> urdf_parser_py.urdf.Link: """ Returns the root link of the URDF of this object. + :return: The root link as defined in the URDF of this object. """ link_name = self.urdf_object.get_root() for link in self.urdf_object.links: @@ -1850,15 +1871,22 @@ def get_root_urdf_link(self) -> urdf_parser_py.urdf.Link: def get_root_link(self) -> Link: """ Returns the root link of this object. + :return: The root link of this object. """ return self.links[self.urdf_object.get_root()] def get_root_link_id(self) -> int: + """ + Returns the unique id of the root link of this object. + :return: The unique id of the root link of this object. + """ return self.get_link_id(self.urdf_object.get_root()) def get_link_id(self, link_name: str) -> int: """ Returns a unique id for a link name. + :param link_name: The name of the link. + :return: The unique id of the link. """ assert link_name is not None return self.link_name_to_id[link_name] @@ -1866,6 +1894,8 @@ def get_link_id(self, link_name: str) -> int: def get_link_by_id(self, link_id: int) -> Link: """ Returns the link for a given unique link id + :param link_id: The unique id of the link. + :return: The link object. """ return self.links[self.link_id_to_name[link_id]] @@ -1892,7 +1922,6 @@ def set_positions_of_all_joints(self, joint_poses: dict) -> None: multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. :param joint_poses: - :return: """ for joint_name, joint_position in joint_poses.items(): self.world.reset_joint_position(self, joint_name, joint_position) @@ -1993,16 +2022,20 @@ def set_color(self, color: Color) -> None: self.world.set_object_color(self, color) def get_color(self) -> Union[Color, Dict[str, Color]]: + """ + :return: The rgba_color of this object or a dictionary of link names and their colors. + """ return self.world.get_object_color(self) - def get_aabb(self) -> AxisAlignedBoundingBox: + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis aligned bounding box of this object. + """ return self.world.get_object_axis_aligned_bounding_box(self) def get_base_origin(self) -> Pose: """ - Returns the origin of the base/bottom of an object - - :return: The position of the bottom of this Object + :return: the origin of the base/bottom of this object. """ aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() base_width = np.absolute(aabb.min_x - aabb.max_x) @@ -2070,7 +2103,10 @@ def get_positions_of_all_joints(self) -> Dict[str: float]: """ return self._current_joints_positions - def update_link_transforms(self, transform_time: Optional[rospy.Time] = None): + def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: + """ + Updates the transforms of all links of this object using time 'transform_time' or the current ros time. + """ for link in self.links.values(): link.update_transform(transform_time) @@ -2124,44 +2160,71 @@ def __init__(self, parent_to_child_transform: Optional[Transform] = None, constraint_id: Optional[int] = None): """ - Creates an attachment between the parent object and the child object. + Creates an attachment between the parent object link and the child object link. + This could be a bidirectional attachment, meaning that both objects will move when one moves. + :param parent_link: The parent object link. + :param child_link: The child object link. + :param bidirectional: If true, both objects will move when one moves. + :param parent_to_child_transform: The transform from the parent link to the child object link. + :param constraint_id: The id of the constraint in the simulator. """ - self.parent_link = parent_link - self.child_link = child_link - self.bidirectional = bidirectional - self._loose = False and not bidirectional + self.parent_link: Link = parent_link + self.child_link: Link = child_link + self.bidirectional: bool = bidirectional + self._loose: bool = False and not bidirectional - self.parent_to_child_transform = parent_to_child_transform + self.parent_to_child_transform: Transform = parent_to_child_transform if self.parent_to_child_transform is None: self.update_transform() - self.constraint_id = constraint_id + self.constraint_id: int = constraint_id if self.constraint_id is None: self.add_fixed_constraint() - def update_transform_and_constraint(self): + def update_transform_and_constraint(self) -> None: + """ + Updates the transform and constraint of this attachment. + """ self.update_transform() self.update_constraint() - def update_transform(self): + def update_transform(self) -> None: + """ + Updates the transform of this attachment by calculating the transform from the parent link to the child link. + """ self.parent_to_child_transform = self.calculate_transform() - def update_constraint(self): + def update_constraint(self) -> None: + """ + Updates the constraint of this attachment by removing the old constraint if one exists and adding a new one. + """ self.remove_constraint_if_exists() self.add_fixed_constraint() - def add_fixed_constraint(self): + def add_fixed_constraint(self) -> None: + """ + Adds a fixed constraint between the parent link and the child link. + """ cid = self.parent_link.add_fixed_constraint_with_link(self.child_link) self.constraint_id = cid - def calculate_transform(self): + def calculate_transform(self) -> Transform: + """ + Calculates the transform from the parent link to the child link. + """ return self.parent_link.get_transform_to_link(self.child_link) - def remove_constraint_if_exists(self): + def remove_constraint_if_exists(self) -> None: + """ + Removes the constraint between the parent and the child links if one exists. + """ if self.child_link in self.parent_link.constraint_ids: self.parent_link.remove_constraint_with_link(self.child_link) - def get_inverse(self): + def get_inverse(self) -> Attachment: + """ + :return: A new Attachment object with the parent and child links swapped. + """ attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, constraint_id=self.constraint_id) attachment.loose = False if self.loose else True @@ -2175,17 +2238,24 @@ def loose(self) -> bool: return self._loose @loose.setter - def loose(self, loose: bool): + def loose(self, loose: bool) -> None: + """ + Sets the loose property of this attachment. + :param loose: If true, then the child object will not move when parent moves. + """ self._loose = loose and not self.bidirectional @property def is_reversed(self) -> bool: """ - If true means that when child moves, parent moves not the other way around. + :return: True if the parent and child links are swapped. """ return self.loose - def __del__(self): + def __del__(self) -> None: + """ + Removes the constraint between the parent and the child links if one exists when the attachment is deleted. + """ self.remove_constraint_if_exists() From efdf22a2599268c98b8eb07722e288a2abbd10da Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jan 2024 19:40:36 +0100 Subject: [PATCH 43/69] [Abstract World] Removed all mentions and dependencies of bullet world and pybullet from all places except bullet_world.py Applied some cleaning and removed duplications. Added extra functionallity to World class like force torque sensor. --- src/pycram/bullet_world.py | 2 +- src/pycram/costmaps.py | 50 ++-- src/pycram/designator.py | 44 ++-- src/pycram/designators/action_designator.py | 2 +- src/pycram/designators/location_designator.py | 2 +- src/pycram/designators/motion_designator.py | 50 ++-- src/pycram/designators/object_designator.py | 4 +- src/pycram/event.py | 2 +- src/pycram/external_interfaces/giskard.py | 47 ++-- src/pycram/external_interfaces/ik.py | 22 +- src/pycram/external_interfaces/knowrob.py | 2 +- src/pycram/external_interfaces/robokudo.py | 8 +- src/pycram/fluent.py | 2 +- src/pycram/helper.py | 4 +- src/pycram/language.py | 2 +- src/pycram/local_transformer.py | 6 +- src/pycram/orm/action_designator.py | 2 +- src/pycram/orm/base.py | 3 +- src/pycram/orm/motion_designator.py | 2 +- src/pycram/orm/task.py | 2 +- src/pycram/pose.py | 4 +- src/pycram/pose_generator_and_validator.py | 45 ++-- src/pycram/process_module.py | 53 +++-- .../process_modules/boxy_process_modules.py | 215 ++---------------- .../default_process_modules.py | 194 ++-------------- .../process_modules/donbot_process_modules.py | 103 +++------ .../process_modules/hsr_process_modules.py | 9 +- .../process_modules/pr2_process_modules.py | 153 +++++++------ .../resolver/location/giskard_location.py | 8 +- src/pycram/resolver/location/jpt_location.py | 52 ++--- src/pycram/robot_description.py | 2 +- src/pycram/ros/force_torque_sensor.py | 15 +- src/pycram/ros/joint_state_publisher.py | 15 +- src/pycram/ros/robot_state_updater.py | 28 +-- src/pycram/ros/tf_broadcaster.py | 12 +- src/pycram/ros/viz_marker_publisher.py | 9 +- src/pycram/task.py | 18 +- src/pycram/world.py | 121 ++++++++-- src/pycram/world_dataclasses.py | 64 ++---- src/pycram/world_reasoning.py | 11 +- 40 files changed, 529 insertions(+), 860 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 82e57c3b8..d268d37f4 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -3,7 +3,7 @@ import threading import time -from typing import List, Optional, Dict, Tuple +from typing_extensions import List, Optional, Dict, Tuple import numpy as np import pybullet as p diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index c56812631..aac666802 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -1,7 +1,7 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -from typing import Tuple, List, Optional +from typing_extensions import Tuple, List, Optional import matplotlib.pyplot as plt import numpy as np @@ -14,7 +14,7 @@ from .local_transformer import LocalTransformer from .pose import Pose, Transform from .world import World -from .world_dataclasses import AxisAlignedBoundingBox, BoxVisualShape, BoxShapeData, MultiBody, Color, JointType +from .world_dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color class Costmap: @@ -54,8 +54,7 @@ def __init__(self, resolution: float, def visualize(self) -> None: """ Visualizes a costmap in the World, the visualisation works by - subdividing the costmap in rectangles which are then visualized as pybullet - visual shapes. + subdividing the costmap in rectangles which are then visualized as world visual shapes. """ if self.vis_ids: return @@ -75,43 +74,26 @@ def visualize(self) -> None: map[i:i + curr_height, j:j + curr_width] = 0 cells = [] # Creation of the visual shapes, for documentation of the visual shapes - # please look here: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.q1gn7v6o58bf + # please look here: + # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.q1gn7v6o58bf for box in boxes: - box_shape_data = BoxShapeData([(box[1] * self.resolution) / 2, (box[2] * self.resolution) / 2, 0.001]) - visual_frame_position = [(box[0][0] + box[1] / 2) * self.resolution, - (box[0][1] + box[2] / 2) * self.resolution, 0.] - visual_shape = BoxVisualShape(Color(1, 0, 0, 0.6), visual_frame_position, box_shape_data) + visual_shape = BoxVisualShape(Color(1, 0, 0, 0.6), + visual_frame_position=[(box[0][0] + box[1] / 2) * self.resolution, + (box[0][1] + box[2] / 2) * self.resolution, 0.], + half_extents=[(box[1] * self.resolution) / 2, + (box[2] * self.resolution) / 2, 0.001]) visual = self.world.create_visual_shape(visual_shape) cells.append(visual) # Set to 127 for since this is the maximal amount of links in a multibody for cell_parts in self._chunks(cells, 127): - # Dummy paramater since these are needed to spawn visual shapes as a - # multibody. - link_poses = [[0, 0, 0] for c in cell_parts] - link_orientations = [[0, 0, 0, 1] for c in cell_parts] - link_masses = [1.0 for c in cell_parts] - link_parent = [0 for c in cell_parts] - link_joints = [JointType.FIXED for c in cell_parts] - link_collision = [-1 for c in cell_parts] - link_joint_axis = [[1, 0, 0] for c in cell_parts] - offset = Transform([-self.height / 2 * self.resolution, -self.width / 2 * self.resolution, 0.05], [0, 0, 0, 1]) origin = Transform(self.origin.position_as_list(), self.origin.orientation_as_list()) new_transform = origin * offset new_pose = new_transform.to_pose().to_list() - multi_body = MultiBody(base_visual_shape_index=-1, base_position=new_pose[0], base_orientation=new_pose[1], - link_visual_shape_indices=cell_parts, link_positions=link_poses, - link_orientations=link_orientations, link_masses=link_masses, - link_inertial_frame_positions=link_poses, - link_inertial_frame_orientations=link_orientations, - link_parent_indices=link_parent, link_joint_types=link_joints, - link_joint_axis=link_joint_axis, - link_collision_shape_indices=link_collision) - - map_obj = self.world.create_multi_body(multi_body) + map_obj = self.world.create_multi_body_from_visual_shapes(cell_parts, Pose(*new_pose)) self.vis_ids.append(map_obj) def _chunks(self, lst: List, n: int) -> List: @@ -130,7 +112,7 @@ def close_visualization(self) -> None: Removes the visualization from the World. """ for v_id in self.vis_ids: - self.world.remove_object(v_id) + self.world.remove_object(self.world.get_object_by_id(v_id)) self.vis_ids = [] def _find_consectuive_line(self, start: Tuple[int, int], map: np.ndarray) -> int: @@ -266,7 +248,7 @@ def __init__(self, distance_to_obstacle: float, self.origin = Pose() if not origin else origin self.resolution = resolution self.distance_obstacle = max(int(distance_to_obstacle / self.resolution), 1) - self.map = self._create_from_bullet(size, resolution) + self.map = self._create_from_world(size, resolution) Costmap.__init__(self, resolution, size, size, self.origin, self.map) def _calculate_diff_origin(self, height: int, width: int) -> Pose: @@ -361,7 +343,7 @@ def create_sub_map(self, sub_origin: Pose, size: int) -> Costmap: sub_map = np.rot90(np.flip(self._convert_map(sub_map), 0)) return Costmap(self.resolution, size, size, Pose(list(sub_origin * -1)), sub_map) - def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: + def _create_from_world(self, size: int, resolution: float) -> np.ndarray: """ Creates an Occupancy Costmap for the specified World. This map marks every position as valid that has no object above it. After @@ -382,7 +364,7 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray: rays = np.dstack(np.dstack((indices_0, indices_10))).T res = np.zeros(len(rays)) - # Using the PyBullet rayTest to check if there is an object above the position + # Using the World rayTest to check if there is an object above the position # if there is no object the position is marked as valid # 16383 is the maximal number of rays that can be processed in a batch i = 0 @@ -520,7 +502,7 @@ def _create_images(self) -> List[np.ndarray]: def _depth_buffer_to_meter(self, buffer: np.ndarray) -> np.ndarray: """ - Converts the depth images generated by PyBullet to represent + Converts the depth images generated by the World to represent each position in metre. :return: The depth image in metre diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 8f5c70a8a..a22f51a74 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -12,12 +12,13 @@ from .helper import GeneratorList, bcolors from threading import Lock from time import time -from typing import List, Dict, Any, Type, Optional, Union, get_type_hints, Callable, Tuple, Iterable +from typing_extensions import List, Dict, Any, Optional, Union, get_type_hints, Callable, Iterable from .local_transformer import LocalTransformer from .language import Language from .pose import Pose from .robot_descriptions import robot_description +from .enums import ObjectType import logging @@ -25,7 +26,7 @@ from .orm.object_designator import (Object as ORMObjectDesignator) from .orm.motion_designator import (Motion as ORMMotionDesignator) -from .orm.base import Quaternion, Position, Base, RobotState, ProcessMetaData +from .orm.base import RobotState, ProcessMetaData from .task import with_tree @@ -72,11 +73,11 @@ class Designator(ABC): argument and return a list of solutions. A solution can also be a generator. """ - def __init__(self, description: Type[DesignatorDescription], parent: Optional[Designator] = None): + def __init__(self, description: DesignatorDescription, parent: Optional[Designator] = None): """Create a new desginator. Arguments: - :param properties: A list of tuples (key-value pairs) describing this designator. + :param description: A list of tuples (key-value pairs) describing this designator. :param parent: The parent to equate with (default is None). """ self._mutex: Lock = Lock() @@ -87,7 +88,7 @@ def __init__(self, description: Type[DesignatorDescription], parent: Optional[De self._solutions = None self._index: int = 0 self.timestamp = None - self._description: Type[DesignatorDescription] = description + self._description: DesignatorDescription = description if parent is not None: self.equate(parent) @@ -358,7 +359,7 @@ def get_slots(self) -> List[str]: """ return list(self.__dict__.keys()) - def copy(self) -> Type[DesignatorDescription]: + def copy(self) -> DesignatorDescription: return self @@ -432,7 +433,8 @@ def get_slots(self): """ return list(self.__dict__.keys()).remove('cmd') - def _check_properties(self, desig: str, exclude: List[str] = []) -> None: + # TODO: Not used anymore, remove? + def _check_properties(self, desig: str, exclude: List[str] = None) -> None: """ Checks the properties of this description. It will be checked if any attribute is None and if any attribute has to wrong type according to the type hints in @@ -443,6 +445,7 @@ def _check_properties(self, desig: str, exclude: List[str] = []) -> None: Exception as output. :param exclude: A list of properties which should not be checked. """ + exclude = exclude if exclude else [] right_types = get_type_hints(self.Motion) attributes = self.__dict__.copy() del attributes["resolve"] @@ -479,7 +482,7 @@ class Action: The torso height of the robot at the start of the action. """ - robot_type: str = dataclasses.field(init=False) + robot_type: ObjectType = dataclasses.field(init=False) """ The type of the robot at the start of the action. """ @@ -605,7 +608,7 @@ class Object: Name of the object """ - obj_type: str + obj_type: ObjectType """ Type of the object """ @@ -715,26 +718,7 @@ def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: return pose_in_object return pose - # def special_knowledge(self, grasp, pose): - # """ - # Returns t special knowledge for "grasp front". - # """ - # - # special_knowledge = [] # Initialize as an empty list - # if self.type in SPECIAL_KNOWLEDGE: - # special_knowledge = SPECIAL_KNOWLEDGE[self.type] - # - # for key, value in special_knowledge: - # if key == grasp: - # # Adjust target pose based on special knowledge - # pose.pose.position.x += value[0] - # pose.pose.position.y += value[1] - # pose.pose.position.z += value[2] - # print("Adjusted target pose based on special knowledge for grasp: ", grasp) - # return pose - # return pose - - def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] = None, + def __init__(self, names: Optional[List[str]] = None, types: Optional[List[ObjectType]] = None, resolver: Optional[Callable] = None): """ Base of all object designator descriptions. Every object designator has the name and type of the object. @@ -744,7 +728,7 @@ def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] :param resolver: An alternative resolver that returns an object designator for the list of names and types """ super().__init__(resolver) - self.types: Optional[List[str]] = types + self.types: Optional[List[ObjectType]] = types self.names: Optional[List[str]] = names def ground(self) -> Union[Object, bool]: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 142b21bdf..beea5f255 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1,5 +1,5 @@ import itertools -from typing import Any, Union +from typing_extensions import Any, Union import sqlalchemy.orm diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index f525593bd..4b2a0a931 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -1,5 +1,5 @@ import dataclasses -from typing import List, Union, Iterable, Optional, Callable +from typing_extensions import List, Union, Iterable, Optional, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart from ..world import World, UseProspectionWorld diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 247f1d508..4efa64e49 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -2,20 +2,19 @@ from sqlalchemy.orm import Session from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject -from ..world import Object -from pycram.bullet_world import BulletWorld +from ..world import Object, World from ..designator import DesignatorError from ..plan_failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager from ..robot_descriptions import robot_description from ..designator import MotionDesignatorDescription -from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, AccessingMotion as ORMAccessingMotion, +from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion, MoveGripperMotion as ORMMoveGripperMotion, DetectingMotion as ORMDetectingMotion, - WorldStateDetectingMotion as ORMWorldStateDetectingMotion, OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion) +from ..enums import ObjectType -from typing import List, Dict, Callable, Optional +from typing_extensions import List, Dict, Callable, Optional from ..pose import Pose from ..task import with_tree @@ -231,7 +230,8 @@ def __init__(self, target: Pose, arm: Optional[str] = None, :param target: Target pose for the TCP :param arm: Arm that should be moved :param resolver: Alternative resolver which returns a resolved motion designator - :param allow_gripper_collision: If the gripper should be allowed to collide with something, only used on the real robot + :param allow_gripper_collision: If the gripper should be allowed to collide with something, + only used on the real robot """ super().__init__(resolver) self.cmd: str = 'move-tcp' @@ -278,25 +278,25 @@ def insert(self, session: Session, *args, **kwargs) -> ORMLookingMotion: return motion - def __init__(self, target: Optional[Pose] = None, object: Optional[ObjectDesignatorDescription.Object] = None, + def __init__(self, target: Optional[Pose] = None, obj: Optional[ObjectDesignatorDescription.Object] = None, resolver: Optional[Callable] = None): """ Moves the head of the robot such that the camera points towards the given location. If ``target`` and ``object`` are given ``target`` will be preferred. :param target: Position and orientation of the target - :param object: An Object in the BulletWorld + :param obj: An Object in the World :param resolver: Alternative resolver that returns a resolved motion designator for parameter """ super().__init__(resolver) self.cmd: str = 'looking' self.target: Optional[Pose] = target - self.object: Object = object.world_object if object else object + self.object: Object = obj.world_object if obj else obj def ground(self) -> Motion: """ - Default resolver for looking, chooses which pose to take if ``target`` and ``object`` are given. If both are given - ``target`` will be preferred. + Default resolver for looking, chooses which pose to take if ``target`` and ``object`` are given. + If both are given ``target`` will be preferred. :return: A resolved motion designator """ @@ -373,7 +373,7 @@ class DetectingMotion(MotionDesignatorDescription): @dataclasses.dataclass class Motion(MotionDesignatorDescription.Motion): # cmd: str - object_type: str + object_type: ObjectType """ Type of the object that should be detected """ @@ -381,16 +381,16 @@ class Motion(MotionDesignatorDescription.Motion): @with_tree def perform(self): pm_manager = ProcessModuleManager.get_manager() - bullet_world_object = pm_manager.detecting().execute(self) - if not bullet_world_object: + world_object = pm_manager.detecting().execute(self) + if not world_object: raise PerceptionObjectNotFound( f"Could not find an object with the type {self.object_type} in the FOV of the robot") if ProcessModuleManager.execution_type == "real": - return RealObject.Object(bullet_world_object.name, bullet_world_object.obj_type, - bullet_world_object, bullet_world_object.get_pose()) + return RealObject.Object(world_object.name, world_object.obj_type, + world_object, world_object.get_pose()) - return ObjectDesignatorDescription.Object(bullet_world_object.name, bullet_world_object.obj_type, - bullet_world_object) + return ObjectDesignatorDescription.Object(world_object.name, world_object.obj_type, + world_object) def to_sql(self) -> ORMDetectingMotion: return ORMDetectingMotion(self.object_type) @@ -401,7 +401,7 @@ def insert(self, session: Session, *args, **kwargs) -> ORMDetectingMotion: session.commit() return motion - def __init__(self, object_type: str, resolver: Optional[Callable] = None): + def __init__(self, object_type: ObjectType, resolver: Optional[Callable] = None): """ Checks for every object in the FOV of the robot if it fits the given object type. If the types match an object designator describing the object will be returned. @@ -411,7 +411,7 @@ def __init__(self, object_type: str, resolver: Optional[Callable] = None): """ super().__init__(resolver) self.cmd: str = 'detecting' - self.object_type: str = object_type + self.object_type: ObjectType = object_type def ground(self) -> Motion: """ @@ -504,7 +504,7 @@ def perform(self): def __init__(self, object_type: str, resolver: Optional[Callable] = None): """ - Tries to find an object using the belief state (BulletWorld), if there is an object in the belief state matching + Tries to find an object using the belief state (World), if there is an object in the belief state matching the given object type an object designator will be returned. :param object_type: The object type which should be detected @@ -568,10 +568,11 @@ def ground(self) -> Motion: if len(self.names) != len(self.positions): raise DesignatorError("[Motion Designator][Move Joints] The length of names and positions does not match") for i in range(len(self.names)): - lower, upper = BulletWorld.robot.get_joint_limits(self.names[i]) + lower, upper = World.robot.get_joint_limits(self.names[i]) if self.positions[i] < lower or self.positions[i] > upper: raise DesignatorError( - f"[Motion Designator][Move Joints] The given configuration for the Joint {self.names[i]} violates its limits") + f"[Motion Designator][Move Joints] The given configuration for the Joint {self.names[i]}" + f" violates its limits") return self.Motion(self.cmd, self.names, self.positions) @@ -672,7 +673,8 @@ def insert(self, session: Session, *args, **kwargs) -> ORMClosingMotion: def __init__(self, object_part: ObjectPart.Object, arm: str, resolver: Optional[Callable] = None): """ - Lets the robot close a container specified by the given parameter. This assumes that the handle is already grasped + Lets the robot close a container specified by the given parameter. This assumes that the handle is already + grasped. :param object_part: Object designator describing the handle of the drawer :param arm: Arm that should be used diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index d7d28507a..e52be0235 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,7 +1,7 @@ import dataclasses -from typing import List, Optional, Callable +from typing_extensions import List, Optional, Callable import sqlalchemy.orm -from pycram.world import World, Object as WorldObject +from ..world import World, Object as WorldObject from ..designator import ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) diff --git a/src/pycram/event.py b/src/pycram/event.py index 7b52b4a44..462e3180f 100644 --- a/src/pycram/event.py +++ b/src/pycram/event.py @@ -1,7 +1,7 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -from typing import Callable, List, Optional, Any +from typing_extensions import Callable, List, Optional, Any class Event: diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index a5d921de5..25e9bc833 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -11,11 +11,10 @@ from ..pose import Pose from ..robot_descriptions import robot_description -from ..bullet_world import BulletWorld -from pycram.world import Object +from ..world import World, Object from ..robot_description import ManipulatorDescription -from typing import List, Tuple, Dict, Callable, Optional +from typing_extensions import List, Tuple, Dict, Callable, Optional from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Vector3Stamped from threading import Lock, RLock @@ -96,11 +95,11 @@ def wrapper(*args, **kwargs): @init_giskard_interface def initial_adding_objects() -> None: """ - Adds object that are loaded in the BulletWorld to the Giskard belief state, if they are not present at the moment. + Adds object that are loaded in the World to the Giskard belief state, if they are not present at the moment. """ groups = giskard_wrapper.get_group_names() - for obj in BulletWorld.current_world.objects: - if obj is BulletWorld.robot: + for obj in World.current_world.objects: + if obj is World.robot: continue name = obj.name + "_" + str(obj.id) if name not in groups: @@ -110,11 +109,11 @@ def initial_adding_objects() -> None: @init_giskard_interface def removing_of_objects() -> None: """ - Removes objects that are present in the Giskard belief state but not in the BulletWorld from the Giskard belief state. + Removes objects that are present in the Giskard belief state but not in the World from the Giskard belief state. """ groups = giskard_wrapper.get_group_names() object_names = list( - map(lambda obj: object_names.name + "_" + str(obj.id), BulletWorld.current_world.objects)) + map(lambda obj: object_names.name + "_" + str(obj.id), World.current_world.objects)) diff = list(set(groups) - set(object_names)) for grp in diff: giskard_wrapper.remove_group(grp) @@ -123,19 +122,19 @@ def removing_of_objects() -> None: @init_giskard_interface def sync_worlds() -> None: """ - Synchronizes the BulletWorld and the Giskard belief state, this includes adding and removing objects to the Giskard - belief state such that it matches the objects present in the BulletWorld and moving the robot to the position it is - currently at in the BulletWorld. + Synchronizes the World and the Giskard belief state, this includes adding and removing objects to the Giskard + belief state such that it matches the objects present in the World and moving the robot to the position it is + currently at in the World. """ add_gripper_groups() - bullet_object_names = set() - for obj in BulletWorld.current_world.objects: + world_object_names = set() + for obj in World.current_world.objects: if obj.name != robot_description.name and len(obj.link_name_to_id) != 1: - bullet_object_names.add(obj.name + "_" + str(obj.id)) + world_object_names.add(obj.name + "_" + str(obj.id)) giskard_object_names = set(giskard_wrapper.get_group_names()) robot_name = {robot_description.name} - if not bullet_object_names.union(robot_name).issubset(giskard_object_names): + if not world_object_names.union(robot_name).issubset(giskard_object_names): giskard_wrapper.clear_world() initial_adding_objects() @@ -155,9 +154,9 @@ def update_pose(object: Object) -> 'UpdateWorldResponse': @init_giskard_interface def spawn_object(object: Object) -> None: """ - Spawns a BulletWorld Object in the giskard belief state. + Spawns a World Object in the giskard belief state. - :param object: BulletWorld object that should be spawned + :param object: World object that should be spawned """ if len(object.link_name_to_id) == 1: geometry = object.urdf_object.link_map[object.urdf_object.get_root()].collision.geometry @@ -173,7 +172,7 @@ def remove_object(object: Object) -> 'UpdateWorldResponse': """ Removes an object from the giskard belief state. - :param object: The BulletWorld Object that should be removed + :param object: The World Object that should be removed """ return giskard_wrapper.remove_group(object.name + "_" + str(object.id)) @@ -240,7 +239,7 @@ def _manage_par_motion_goals(goal_func, *args) -> Optional['MoveResult']: if "tip_link" in par_value_pair.keys() and "root_link" in par_value_pair.keys(): if par_value_pair["tip_link"] == robot_description.base_link: continue - chain = BulletWorld.robot.urdf_object.get_chain(par_value_pair["root_link"], + chain = World.robot.urdf_object.get_chain(par_value_pair["root_link"], par_value_pair["tip_link"]) if set(chain).intersection(used_joints) != set(): giskard_wrapper.cmd_seq = tmp @@ -530,8 +529,8 @@ def avoid_collisions(object1: Object, object2: Object) -> None: """ Will avoid collision between the two objects for the next goal. - :param object1: The first BulletWorld Object - :param object2: The second BulletWorld Object + :param object1: The first World Object + :param object2: The second World Object """ giskard_wrapper.avoid_collision(-1, object1.name + "_" + str(object1.id), object2.name + "_" + str(object2.id)) @@ -541,10 +540,10 @@ def avoid_collisions(object1: Object, object2: Object) -> None: @init_giskard_interface def make_world_body(object: Object) -> 'WorldBody': """ - Creates a WorldBody message for a BulletWorld Object. The WorldBody will contain the URDF of the BulletWorld Object + Creates a WorldBody message for a World Object. The WorldBody will contain the URDF of the World Object - :param object: The BulletWorld Object - :return: A WorldBody message for the BulletWorld Object + :param object: The World Object + :return: A WorldBody message for the World Object """ urdf_string = "" with open(object.path) as f: diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index b7d1fb50b..48657fa97 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -1,6 +1,5 @@ -from typing import List, Union +from typing_extensions import List, Union -import pybullet as p import rospy from moveit_msgs.msg import PositionIKRequest from moveit_msgs.msg import RobotState @@ -10,26 +9,11 @@ from ..world import Object from ..helper import calculate_wrist_tool_offset, _apply_ik from ..local_transformer import LocalTransformer -from ..pose import Pose, Transform +from ..pose import Pose from ..robot_descriptions import robot_description from ..plan_failures import IKError -def _get_position_for_joints(robot, joints): - """ - Returns a list with all joint positions for the joint names specified in - the joints parameter - - :param robot: The robot the joint states should be taken from - :param joints: The list of joint names that should be in the output - :return: A list of joint states according and in the same order as the joint - names in the joints parameter - """ - return list( - map(lambda x: p.getJointState(robot.id, robot.get_joint_id(x), physicsClientId=robot.world.client_id)[0], - joints)) - - def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_object: Object, joints: List[str]) -> PositionIKRequest: """ @@ -50,7 +34,7 @@ def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_ob robot_state = RobotState() joint_state = JointState() joint_state.name = joints - joint_state.position = _get_position_for_joints(robot_object, joints) + joint_state.position = [robot_object.get_joint_position(joint) for joint in joints] # joint_state.velocity = [0.0 for x in range(len(joints))] # joint_state.effort = [0.0 for x in range(len(joints))] robot_state.joint_state = joint_state diff --git a/src/pycram/external_interfaces/knowrob.py b/src/pycram/external_interfaces/knowrob.py index 3b9372b1b..9bd5055b6 100644 --- a/src/pycram/external_interfaces/knowrob.py +++ b/src/pycram/external_interfaces/knowrob.py @@ -3,7 +3,7 @@ import sys import rosservice -from typing import Dict, List, Union +from typing_extensions import Dict, List, Union SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__)) sys.path.append(os.path.join(SCRIPT_DIR, os.pardir, os.pardir, "neem-interface", "src")) diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index fbfad9b5a..4ecc35085 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -1,5 +1,5 @@ import sys -from typing import Callable +from typing_extensions import Callable import rosnode import rospy @@ -9,7 +9,7 @@ from ..designator import ObjectDesignatorDescription from ..pose import Pose from ..local_transformer import LocalTransformer -from ..bullet_world import BulletWorld +from ..world import World from ..enums import ObjectType try: @@ -99,7 +99,7 @@ def make_query_goal_msg(obj_desc: ObjectDesignatorDescription) -> 'QueryGoal': """ goal_msg = QueryGoal() goal_msg.obj.uid = str(id(obj_desc)) - goal_msg.obj.obj_type = str(obj_desc.types[0].name) # For testing purposes + goal_msg.obj.obj_type = str(obj_desc.types[0].name) # For testing purposes if ObjectType.JEROEN_CUP == obj_desc.types[0]: goal_msg.obj.color.append("blue") elif ObjectType.BOWL == obj_desc.types[0]: @@ -125,7 +125,7 @@ def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescripti for i in range(0, len(query_result.res[0].pose)): pose = Pose.from_pose_stamped(query_result.res[0].pose[i]) - pose.frame = BulletWorld.current_world.robot.links[pose.frame].tf_frame # TODO: pose.frame is a link name? + pose.frame = World.current_world.robot.links[pose.frame].tf_frame # TODO: pose.frame is a link name? source = query_result.res[0].poseSource[i] lt = LocalTransformer() diff --git a/src/pycram/fluent.py b/src/pycram/fluent.py index a8f2f3ef2..89706e3c8 100644 --- a/src/pycram/fluent.py +++ b/src/pycram/fluent.py @@ -16,7 +16,7 @@ from enum import Enum from threading import Condition, Lock from uuid import uuid4 -from typing import Any, Optional, List, Callable +from typing_extensions import Any, Optional, List, Callable class Behavior(Enum): diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 0ade114bc..61ab85e59 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -7,15 +7,13 @@ GeneratorList -- implementation of generator list wrappers. """ from inspect import isgeneratorfunction -from typing import List -from typing import Tuple, Callable +from typing_extensions import List, Tuple, Callable import numpy as np from pytransform3d.rotations import quaternion_wxyz_from_xyzw, quaternion_xyzw_from_wxyz from pytransform3d.transformations import transform_from_pq, transform_from, pq_from_transform from .world import Object as WorldObject -from .local_transformer import LocalTransformer from .pose import Transform, Pose import math diff --git a/src/pycram/language.py b/src/pycram/language.py index 081ba0cbe..80948cc8d 100644 --- a/src/pycram/language.py +++ b/src/pycram/language.py @@ -2,7 +2,7 @@ from __future__ import annotations import time -from typing import Iterable, Optional, Callable, Dict, Any, List, Union +from typing_extensions import Iterable, Optional, Callable, Dict, Any, List, Union from anytree import NodeMixin, Node, PreOrderIter, RenderTree from .enums import State diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index ebbe4e64d..3ea74dea2 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -10,7 +10,7 @@ from geometry_msgs.msg import TransformStamped from .pose import Pose, Transform -from typing import List, Optional, Union, Iterable +from typing_extensions import List, Optional, Union, Iterable class LocalTransformer(TransformerROS): @@ -22,7 +22,7 @@ class LocalTransformer(TransformerROS): This class uses the robots (currently only one! supported) URDF file to initialize the tfs for the robot. Moreover, the function update_local_transformer_from_btr - updates these tfs by copying the tfs state from the pybullet world. + updates these tfs by copying the tfs state from the world. This class extends the TransformerRos, you can find documentation for TransformerROS here: `TFDoc `_ @@ -47,7 +47,7 @@ def __init__(self): self.tf_stampeds: List[TransformStamped] = [] self.static_tf_stampeds: List[TransformStamped] = [] - # Since this file can't import world.py this holds the reference to the current_bullet_world + # Since this file can't import world.py this holds the reference to the current_world self.world = None # TODO: Ask Jonas if this is still needed self.prospection_world = None diff --git a/src/pycram/orm/action_designator.py b/src/pycram/orm/action_designator.py index 48b4197a3..d27642cf7 100644 --- a/src/pycram/orm/action_designator.py +++ b/src/pycram/orm/action_designator.py @@ -1,4 +1,4 @@ -from typing import Optional +from typing_extensions import Optional from .base import RobotState, Designator, MapperArgsMixin, PoseMixin from .object_designator import ObjectMixin diff --git a/src/pycram/orm/base.py b/src/pycram/orm/base.py index cc6356d5e..407270a17 100644 --- a/src/pycram/orm/base.py +++ b/src/pycram/orm/base.py @@ -1,8 +1,7 @@ """Implementation of base classes for orm modelling.""" import datetime import getpass -import os -from typing import Optional +from typing_extensions import Optional import git import rospkg diff --git a/src/pycram/orm/motion_designator.py b/src/pycram/orm/motion_designator.py index fb2e5d322..44624d33d 100644 --- a/src/pycram/orm/motion_designator.py +++ b/src/pycram/orm/motion_designator.py @@ -5,7 +5,7 @@ The MotionDesignator class is the base class that defines the polymorphic behavior of all other motion designator classes. """ -from typing import Optional +from typing_extensions import Optional from .base import MapperArgsMixin, Designator, PoseMixin from .object_designator import Object, ObjectMixin diff --git a/src/pycram/orm/task.py b/src/pycram/orm/task.py index 5d2152c1f..333bd5d8e 100644 --- a/src/pycram/orm/task.py +++ b/src/pycram/orm/task.py @@ -1,5 +1,5 @@ """Implementation of ORM classes associated with pycram.task.""" -from typing import Optional +from typing_extensions import Optional from sqlalchemy import ForeignKey from sqlalchemy.orm import MappedAsDataclass, Mapped, mapped_column, relationship from .action_designator import Action diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 8cdd39d11..08987ae90 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -1,17 +1,15 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -import copy import math import datetime -from typing import List, Union, Optional +from typing_extensions import List, Union, Optional import numpy as np import rospy import sqlalchemy.orm from geometry_msgs.msg import PoseStamped, TransformStamped, Vector3, Point from geometry_msgs.msg import (Pose as GeoPose, Quaternion as GeoQuaternion) -from std_msgs.msg import Header from tf import transformations from .orm.base import Pose as ORMPose, Position, Quaternion, ProcessMetaData diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 131d87124..6f14c7a46 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -1,8 +1,7 @@ import tf import numpy as np -from .world import Object -from .bullet_world import World +from .world import Object, World from .world_reasoning import contact from .costmaps import Costmap from .pose import Pose, Transform @@ -10,7 +9,7 @@ from .external_interfaces.ik import request_ik from .plan_failures import IKError from .helper import _apply_ik -from typing import Tuple, List, Union, Dict, Iterable +from typing_extensions import Tuple, List, Union, Dict, Iterable def pose_generator(costmap: Costmap, number_of_samples=100, orientation_generator=None) -> Iterable: @@ -110,6 +109,28 @@ def visibility_validator(pose: Pose, return res +def _in_contact(robot: Object, obj: Object, allowed_collision: Dict[Object, List[str]], + allowed_robot_links: List[str]) -> bool: + """ + This method checks if a given robot is in contact with a given object. + :param robot: The robot object that should be checked for contact. + :param obj: The object that should be checked for contact with the robot. + :param allowed_collision: A dictionary that contains the allowed collisions for links of each object in the world. + :param allowed_robot_links: A list of links of the robot that are allowed to be in contact with the object. + :return: True if the robot is in contact with the object and False otherwise. + """ + in_contact, contact_links = contact(robot, obj, return_links=True) + allowed_links = allowed_collision[obj] if obj in allowed_collision.keys() else [] + + if in_contact: + for link in contact_links: + if link[0].name in allowed_robot_links or link[1].name in allowed_links: + in_contact = False + # TODO: in_contact is never set to True after it was set to False is that correct? + # TODO: If it is correct, then this loop should break after the first contact is found + return in_contact + + def reachability_validator(pose: Pose, robot: Object, target: Union[Object, Pose], @@ -150,7 +171,7 @@ def reachability_validator(pose: Pose, if robot in allowed_collision.keys(): allowed_robot_links = allowed_collision[robot] - joint_state_before_ik=robot._current_joints_positions + joint_state_before_ik = robot._current_joints_positions try: # resp = request_ik(base_link, end_effector, target_diff, robot, left_joints) resp = request_ik(target, robot, left_joints, left_gripper) @@ -159,13 +180,7 @@ def reachability_validator(pose: Pose, for obj in World.current_world.objects: if obj.name == "floor": continue - in_contact, contact_links = contact(robot, obj, return_links=True) - allowed_links = allowed_collision[obj] if obj in allowed_collision.keys() else [] - - if in_contact: - for link in contact_links: - if link[0].name in allowed_robot_links or link[1].name in allowed_links: - in_contact = False + in_contact = _in_contact(robot, obj, allowed_collision, allowed_robot_links) if not in_contact: arms.append("left") @@ -183,13 +198,7 @@ def reachability_validator(pose: Pose, for obj in World.current_world.objects: if obj.name == "floor": continue - in_contact, contact_links = contact(robot, obj, return_links=True) - allowed_links = allowed_collision[obj] if obj in allowed_collision.keys() else [] - - if in_contact: - for link in contact_links: - if link[0].name in allowed_robot_links or link[1].name in allowed_links: - in_contact = False + in_contact = _in_contact(robot, obj, allowed_collision, allowed_robot_links) if not in_contact: arms.append("right") diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index 351798350..a9626bb05 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -10,21 +10,19 @@ import threading import time from abc import ABC -from threading import Lock +from typing_extensions import Callable, Type, Any, Union import rospy from .designator import MotionDesignatorDescription -from .fluent import Fluent -from typing import Callable, List, Type, Any, Union from .language import Language - from .robot_descriptions import robot_description class ProcessModule: """ - Implementation of process modules. Process modules are the part that communicate with the outer world to execute designators. + Implementation of process modules. Process modules are the part that communicate with the outer world to execute + designators. """ execution_delay = True """ @@ -78,6 +76,7 @@ class RealRobot: with real_robot: some designators """ + def __init__(self): self.pre: str = "" @@ -89,7 +88,7 @@ def __enter__(self): self.pre = ProcessModuleManager.execution_type ProcessModuleManager.execution_type = "real" - def __exit__(self, type, value, traceback): + def __exit__(self, _type, value, traceback): """ Exit method for the 'with' scope, sets the :py:attr:`~ProcessModuleManager.execution_type` to the previously used one. @@ -102,8 +101,8 @@ def __call__(self): class SimulatedRobot: """ - Management class for executing designators on the simulated robot. This is intended to be used in a with environment. - When importing this class an instance is imported instead. + Management class for executing designators on the simulated robot. This is intended to be used in + a with environment. When importing this class an instance is imported instead. Example: @@ -112,6 +111,7 @@ class SimulatedRobot: with simulated_robot: some designators """ + def __init__(self): self.pre: str = "" @@ -123,7 +123,7 @@ def __enter__(self): self.pre = ProcessModuleManager.execution_type ProcessModuleManager.execution_type = "simulated" - def __exit__(self, type, value, traceback): + def __exit__(self, _type, value, traceback): """ Exit method for the 'with' scope, sets the :py:attr:`~ProcessModuleManager.execution_type` to the previously used one. @@ -149,6 +149,7 @@ def plan(): :param func: Function this decorator is annotating :return: The decorated function wrapped into the decorator """ + def wrapper(*args, **kwargs): pre = ProcessModuleManager.execution_type ProcessModuleManager.execution_type = "real" @@ -174,6 +175,7 @@ def plan(): :param func: Function this decorator is annotating :return: The decorated function wrapped into the decorator """ + def wrapper(*args, **kwargs): pre = ProcessModuleManager.execution_type ProcessModuleManager.execution_type = "simulated" @@ -237,6 +239,7 @@ def get_manager() -> Union[ProcessModuleManager, None]: :return: ProcessModuleManager instance of the current robot """ manager = None + _default_manager = None if not ProcessModuleManager.execution_type: rospy.logerr( f"No execution_type is set, did you use the with_simulated_robot or with_real_robot decorator?") @@ -246,17 +249,23 @@ def get_manager() -> Union[ProcessModuleManager, None]: if pm_manager.robot_name == robot_description.name: manager = pm_manager if pm_manager.robot_name == "default": - default_manager = pm_manager + _default_manager = pm_manager if manager: return manager + elif _default_manager: + rospy.logwarn_once(f"No Process Module Manager found for robot: '{robot_description.name}'" + f", using default process modules") + return _default_manager else: - rospy.logwarn_once(f"No Process Module Manager found for robot: '{robot_description.name}', using default process modules") - return default_manager + rospy.logerr(f"No Process Module Manager found for robot: '{robot_description.name}'" + f", and no default process modules available") + return None def navigate(self) -> Type[ProcessModule]: """ - Returns the Process Module for navigating the robot with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Returns the Process Module for navigating the robot with respect to + the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for navigating """ @@ -283,7 +292,8 @@ def place(self) -> Type[ProcessModule]: def looking(self) -> Type[ProcessModule]: """ - Returns the Process Module for looking at a point with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Returns the Process Module for looking at a point with respect to + the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for looking at a specific point """ @@ -292,7 +302,8 @@ def looking(self) -> Type[ProcessModule]: def detecting(self) -> Type[ProcessModule]: """ - Returns the Process Module for detecting an object with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Returns the Process Module for detecting an object with respect to + the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for detecting an object """ @@ -301,7 +312,8 @@ def detecting(self) -> Type[ProcessModule]: def move_tcp(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving the Tool Center Point with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Returns the Process Module for moving the Tool Center Point with respect to + the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving the TCP """ @@ -340,7 +352,8 @@ def move_joints(self) -> Type[ProcessModule]: def move_gripper(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving the gripper with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Returns the Process Module for moving the gripper with respect to + the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving the gripper """ @@ -349,7 +362,8 @@ def move_gripper(self) -> Type[ProcessModule]: def open(self) -> Type[ProcessModule]: """ - Returns the Process Module for opening drawers with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Returns the Process Module for opening drawers with respect to + the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for opening drawers """ @@ -358,7 +372,8 @@ def open(self) -> Type[ProcessModule]: def close(self) -> Type[ProcessModule]: """ - Returns the Process Module for closing drawers with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Returns the Process Module for closing drawers with respect to + the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for closing drawers """ diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index a8185b760..eafdd360c 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -1,78 +1,17 @@ -import time from threading import Lock -from typing import Tuple, List import numpy as np -import pybullet as p - -import pycram.world_reasoning as btr -import pycram.helper as helper -from ..bullet_world import BulletWorld, Object as BulletWorldObject -from ..designators.motion_designator import * -from ..enums import JointType -from ..external_interfaces.ik import request_ik -from ..local_transformer import LocalTransformer as local_tf, LocalTransformer + +from ..world import World +from ..designators.motion_designator import PlaceMotion +from ..local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description -from tf.transformations import euler_from_quaternion, quaternion_from_euler - - -def _transform_to_torso(pose_and_rotation: Tuple[List[float], List[float]], robot: BulletWorldObject) -> Tuple[ - List[float], List[float]]: - map_T_torso = robot.links[robot_description.torso_link].pose_as_list - torso_T_map = p.invertTransform(map_T_torso[0], map_T_torso[1]) - map_T_target = pose_and_rotation - torso_T_target = p.multiplyTransforms(torso_T_map[0], torso_T_map[1], map_T_target[0], map_T_target[1]) - return torso_T_target - - -def _park_arms(arm): - """ - Defines the joint poses for the parking positions of the arms of Boxy and applies them to the - in the BulletWorld defined robot. - :return: None - """ - - robot = BulletWorld.robot - if arm == "right": - for joint, pose in robot_description.get_static_joint_chain("right", "park").items(): - robot.set_joint_position(joint, pose) - if arm == "left": - for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): - robot.set_joint_position(joint, pose) - - -class BoxyNavigation(ProcessModule): - """ - The process module to move the robot from one position to another. - """ - - def _execute(self, desig: MoveMotion.Motion): - robot = BulletWorld.robot - robot.set_pose(desig.target) - - -class BoxyPickUp(ProcessModule): - """ - This process module is for picking up a given object. - The object has to be reachable for this process module to succeed. - """ - - def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - arm = desig.arm - - _move_arm_tcp(target, robot, arm) - tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) +from ..process_modules.pr2_process_modules import (_park_arms, Pr2Navigation as BoxyNavigation, + Pr2PickUp as BoxyPickUp, Pr2Open as BoxyOpen, + Pr2Close as BoxyClose, Pr2Detecting as BoxyDetecting, + Pr2MoveTCP as BoxyMoveTCP, Pr2MoveArmJoints as BoxyMoveArmJoints, + Pr2WorldStateDetecting as BoxyWorldStateDetecting, _move_arm_tcp) class BoxyPlace(ProcessModule): @@ -86,58 +25,19 @@ def _execute(self, desig: PlaceMotion.Motion): :param desig: A PlaceMotion :return: """ - object = desig.object.world_object - robot = BulletWorld.robot + obj = desig.object.world_object + robot = World.robot arm = desig.arm # Transformations such that the target position is the position of the object and not the tcp - object_pose = object.get_pose() + object_pose = obj.get_pose() local_tf = LocalTransformer() tcp_to_object = local_tf.transform_pose(object_pose, - robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) + robot.links[robot_description.get_tool_frame(arm)].tf_frame) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, arm) - robot.detach(object) - - -class BoxyOpen(ProcessModule): - """ - Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. - """ - - def _execute(self, desig: OpeningMotion.Motion): - part_of_object = desig.object_part.world_object - - container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) - - goal_pose = btr.link_pose_for_joint_config(part_of_object, { - container_joint: part_of_object.get_joint_limits(container_joint)[1] - 0.05}, desig.object_part.name) - - _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - - desig.object_part.world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( - container_joint)[1]) - - -class BoxyClose(ProcessModule): - """ - Low-level implementation that lets the robot close a grasped container, in simulation - """ - def _execute(self, desig: ClosingMotion.Motion): - part_of_object = desig.object_part.world_object - - container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) - - goal_pose = btr.link_pose_for_joint_config(part_of_object, { - container_joint: part_of_object.get_joint_limits(container_joint)[0]}, desig.object_part.name) - - _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - - desig.object_part.world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( - container_joint)[0]) + robot.detach(obj) class BoxyParkArms(ProcessModule): @@ -160,26 +60,27 @@ class BoxyMoveHead(ProcessModule): def _execute(self, desig): target = desig.target - robot = BulletWorld.robot + robot = World.robot local_transformer = LocalTransformer() - pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link")) + pose_in_shoulder = local_transformer.transform_pose(target, robot.links["neck_shoulder_link"].tf_frame) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_joint_states(robot_description.get_static_joint_chain("neck", "front")) + robot.set_positions_of_all_joints(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_states(robot_description.get_static_joint_chain("neck", "neck_right")) + robot.set_positions_of_all_joints(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_states(robot_description.get_static_joint_chain("neck", "back")) + robot.set_positions_of_all_joints(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_states(robot_description.get_static_joint_chain("neck", "neck_left")) + robot.set_positions_of_all_joints(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")) + pose_in_shoulder = local_transformer.transform_pose(target, robot.links["neck_shoulder_link"].tf_frame) new_pan = np.arctan2(pose_in_shoulder.position.y, pose_in_shoulder.position.x) - robot.set_joint_state("neck_shoulder_pan_joint", new_pan + robot.get_joint_state("neck_shoulder_pan_joint")) + robot.set_joint_position("neck_shoulder_pan_joint", + new_pan + robot.get_joint_position("neck_shoulder_pan_joint")) class BoxyMoveGripper(ProcessModule): @@ -189,76 +90,10 @@ class BoxyMoveGripper(ProcessModule): """ def _execute(self, desig): - robot = BulletWorld.robot + robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_joint_states(robot_description.get_static_gripper_chain(gripper, motion)) - - -class BoxyDetecting(ProcessModule): - """ - This process module tries to detect an object with the given type. To be detected the object has to be in - the field of view of the robot. - """ - - def _execute(self, desig): - robot = BulletWorld.robot - object_type = desig.object_type - # Should be "wide_stereo_optical_frame" - cam_frame_name = robot_description.get_camera_frame() - # should be [0, 0, 1] - front_facing_axis = robot_description.front_facing_axis - - objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type) - for obj in objects: - if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): - return obj - - -class BoxyMoveTCP(ProcessModule): - """ - This process moves the tool center point of either the right or the left arm. - """ - - def _execute(self, desig: MoveTCPMotion.Motion): - target = desig.target - robot = BulletWorld.robot - - _move_arm_tcp(target, robot, desig.arm) - - -class BoxyMoveArmJoints(ProcessModule): - """ - This process modules moves the joints of either the right or the left arm. The joint states can be given as - list that should be applied or a pre-defined position can be used, such as "parking" - """ - - def _execute(self, desig: MoveArmJointsMotion.Motion): - - robot = BulletWorld.robot - if desig.right_arm_poses: - robot.set_joint_states(desig.right_arm_poses) - if desig.left_arm_poses: - robot.set_joint_states(desig.left_arm_poses) - - -class BoxyWorldStateDetecting(ProcessModule): - """ - This process module detectes an object even if it is not in the field of view of the robot. - """ - - def _execute(self, desig: WorldStateDetectingMotion.Motion): - obj_type = desig.object_type - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] - - -def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None: - gripper = robot_description.get_tool_frame(arm) - - joints = robot_description.chains[arm].joints - - inv = request_ik(target, robot, joints, gripper) - helper._apply_ik(robot, inv, joints) + robot.set_positions_of_all_joints(robot_description.get_static_gripper_chain(gripper, motion)) class BoxyManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 84567c3b9..ba763b7c7 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -1,68 +1,16 @@ from threading import Lock -import pycram.bullet_world_reasoning as btr import numpy as np -from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..bullet_world import BulletWorld -from ..external_interfaces.ik import request_ik, IKError -from ..helper import _apply_ik +from ..process_modules.pr2_process_modules import Pr2Navigation as DefaultNavigation, Pr2PickUp as DefaultPickUp, \ + Pr2Place as DefaultPlace, Pr2WorldStateDetecting as DefaultWorldStateDetecting, Pr2Open as DefaultOpen, \ + Pr2Close as DefaultClose, Pr2MoveGripper as DefaultMoveGripper, Pr2Detecting as DefaultDetecting, \ + Pr2MoveTCP as DefaultMoveTCP +from ..robot_descriptions import robot_description +from ..world import World from ..local_transformer import LocalTransformer -from ..designators.motion_designator import * -from ..enums import JointType - - -class DefaultNavigation(ProcessModule): - """ - The process module to move the robot from one position to another. - """ - - def _execute(self, desig: MoveMotion.Motion): - robot = BulletWorld.robot - robot.set_pose(desig.target) - - -class DefaultPickUp(ProcessModule): - """ - This process module is for picking up a given object. - The object has to be reachable for this process module to succeed. - """ - - def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - arm = desig.arm - - _move_arm_tcp(target, robot, arm) - tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) - - -class DefaultPlace(ProcessModule): - """ - This process module places an object at the given position in world coordinate frame. - """ - - def _execute(self, desig: PlaceMotion.Motion): - """ - - :param desig: A PlaceMotion - :return: - """ - object = desig.object.world_object - robot = BulletWorld.robot - arm = desig.arm - - _move_arm_tcp(desig.target, robot, arm) - robot.detach(object) +from ..designators.motion_designator import LookingMotion, MoveArmJointsMotion, MoveJointsMotion class DefaultMoveHead(ProcessModule): @@ -73,7 +21,7 @@ class DefaultMoveHead(ProcessModule): def _execute(self, desig: LookingMotion.Motion): target = desig.target - robot = BulletWorld.robot + robot = World.robot local_transformer = LocalTransformer() @@ -82,63 +30,17 @@ def _execute(self, desig: LookingMotion.Motion): pan_joint = robot_description.chains["neck"].joints[0] tilt_joint = robot_description.chains["neck"].joints[1] - pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame(pan_link)) - pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame(tilt_link)) + pose_in_pan = local_transformer.transform_pose(target, robot.links[pan_link].tf_frame) + pose_in_tilt = local_transformer.transform_pose(target, robot.links[tilt_link].tf_frame) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 - current_pan = robot.get_joint_state(pan_joint) - current_tilt = robot.get_joint_state(tilt_joint) - - robot.set_joint_state(pan_joint, new_pan + current_pan) - robot.set_joint_state(tilt_joint, new_tilt + current_tilt) - - -class DefaultMoveGripper(ProcessModule): - """ - This process module controls the gripper of the robot. They can either be opened or closed. - Furthermore, it can only moved one gripper at a time. - """ - - def _execute(self, desig: MoveGripperMotion.Motion): - robot = BulletWorld.robot - gripper = desig.gripper - motion = desig.motion - for joint, state in robot_description.get_static_gripper_chain(gripper, motion).items(): - robot.set_joint_state(joint, state) - - -class DefaultDetecting(ProcessModule): - """ - This process module tries to detect an object with the given type. To be detected the object has to be in - the field of view of the robot. - """ + current_pan = robot.get_joint_position(pan_joint) + current_tilt = robot.get_joint_position(tilt_joint) - def _execute(self, desig: DetectingMotion.Motion): - robot = BulletWorld.robot - object_type = desig.object_type - # Should be "wide_stereo_optical_frame" - cam_frame_name = robot_description.get_camera_frame() - # should be [0, 0, 1] - front_facing_axis = robot_description.front_facing_axis - - objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type) - for obj in objects: - if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): - return obj - - -class DefaultMoveTCP(ProcessModule): - """ - This process moves the tool center point of either the right or the left arm. - """ - - def _execute(self, desig: MoveTCPMotion.Motion): - target = desig.target - robot = BulletWorld.robot - - _move_arm_tcp(target, robot, desig.arm) + robot.set_joint_position(pan_joint, new_pan + current_pan) + robot.set_joint_position(tilt_joint, new_tilt + current_tilt) class DefaultMoveArmJoints(ProcessModule): @@ -149,78 +51,20 @@ class DefaultMoveArmJoints(ProcessModule): def _execute(self, desig: MoveArmJointsMotion.Motion): - robot = BulletWorld.robot + robot = World.robot if desig.right_arm_poses: for joint, pose in desig.right_arm_poses.items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) if desig.left_arm_poses: for joint, pose in desig.left_arm_poses.items(): - robot.set_joint_state(joint, pose) + robot.set_joint_position(joint, pose) class DefaultMoveJoints(ProcessModule): def _execute(self, desig: MoveJointsMotion.Motion): - robot = BulletWorld.robot + robot = World.robot for joint, pose in zip(desig.names, desig.positions): - robot.set_joint_state(joint, pose) - - -class DefaultWorldStateDetecting(ProcessModule): - """ - This process module detectes an object even if it is not in the field of view of the robot. - """ - - def _execute(self, desig: WorldStateDetectingMotion.Motion): - obj_type = desig.object_type - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] - - -class DefaultOpen(ProcessModule): - """ - Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. - """ - - def _execute(self, desig: OpeningMotion.Motion): - part_of_object = desig.object_part.world_object - - container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) - - goal_pose = btr.link_pose_for_joint_config(part_of_object, { - container_joint: part_of_object.get_joint_limits(container_joint)[1] - 0.05}, desig.object_part.name) - - _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - - desig.object_part.world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( - container_joint)[1]) - - -class DefaultClose(ProcessModule): - """ - Low-level implementation that lets the robot close a grasped container, in simulation - """ - def _execute(self, desig: ClosingMotion.Motion): - part_of_object = desig.object_part.world_object - - container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) - - goal_pose = btr.link_pose_for_joint_config(part_of_object, { - container_joint: part_of_object.get_joint_limits(container_joint)[0]}, desig.object_part.name) - - _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) - - desig.object_part.world_object.set_joint_state(container_joint, - part_of_object.get_joint_limits( - container_joint)[0]) - - -def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None: - gripper = robot_description.get_tool_frame(arm) - - joints = robot_description.chains[arm].joints - - inv = request_ik(target, robot, joints, gripper) - _apply_ik(robot, inv, joints) + robot.set_joint_position(joint, pose) class DefaultManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 45cf31899..936c5c57e 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -1,29 +1,24 @@ -import time from threading import Lock import numpy as np -import pybullet as p +from typing_extensions import Optional -import pycram.world_reasoning as btr -import pycram.helper as helper -from ..bullet_world import BulletWorld, Object +from ..bullet_world import World from ..designators.motion_designator import MoveArmJointsMotion, WorldStateDetectingMotion -from ..external_interfaces.ik import request_ik from ..local_transformer import LocalTransformer -from ..pose import Pose from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description -from tf.transformations import euler_from_quaternion, quaternion_from_euler +from ..process_modules.pr2_process_modules import Pr2PickUp, Pr2Detecting as DonbotDetecting, _move_arm_tcp def _park_arms(arm): """ Defines the joint poses for the parking positions of the arm of Donbot and applies them to the - in the BulletWorld defined robot. + in the World defined robot. :return: None """ - robot = BulletWorld.robot + robot = World.robot if arm == "left": for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): robot.set_joint_position(joint, pose) @@ -35,28 +30,17 @@ class DonbotNavigation(ProcessModule): """ def _execute(self, desig): - robot = BulletWorld.robot + robot = World.robot robot.set_pose(desig.target) -class DonbotPickUp(ProcessModule): + +class DonbotPickUp(Pr2PickUp): """ This process module is for picking up a given object. The object has to be reachable for this process module to succeed. """ - - def _execute(self, desig): - object = desig.object_desig.world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - _move_arm_tcp(target, robot, "left") - tool_frame = robot_description.get_tool_frame("left") - robot.attach(object, tool_frame) + def _execute(self, desig, used_arm: Optional[str] = "left"): + super()._execute(desig, "left") class DonbotPlace(ProcessModule): @@ -65,19 +49,18 @@ class DonbotPlace(ProcessModule): """ def _execute(self, desig): - object = desig.object.world_object - robot = BulletWorld.robot + obj = desig.object.world_object + robot = World.robot # Transformations such that the target position is the position of the object and not the tcp - object_pose = object.get_pose() + object_pose = obj.get_pose() local_tf = LocalTransformer() tcp_to_object = local_tf.transform_pose(object_pose, - robot.get_link_tf_frame(robot_description.get_tool_frame("left"))) + robot.links[robot_description.get_tool_frame("left")].tf_frame) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, "left") - robot.detach(object) - + robot.detach(obj) class DonbotMoveHead(ProcessModule): @@ -88,26 +71,26 @@ class DonbotMoveHead(ProcessModule): def _execute(self, desig): target = desig.target - robot = BulletWorld.robot + robot = World.robot local_transformer = LocalTransformer() - pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link")) + pose_in_shoulder = local_transformer.transform_pose(target, robot.links["ur5_shoulder_link"].tf_frame) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_joint_states(robot_description.get_static_joint_chain("left", "front")) + robot.set_positions_of_all_joints(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_states(robot_description.get_static_joint_chain("left", "arm_right")) + robot.set_positions_of_all_joints(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_states(robot_description.get_static_joint_chain("left", "back")) + robot.set_positions_of_all_joints(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_states(robot_description.get_static_joint_chain("left", "arm_left")) + robot.set_positions_of_all_joints(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")) + pose_in_shoulder = local_transformer.transform_pose(target, robot.links["ur5_shoulder_link"].tf_frame) new_pan = np.arctan2(pose_in_shoulder.position.y, pose_in_shoulder.position.x) - robot.set_joint_state("ur5_shoulder_pan_joint", new_pan + robot.get_joint_state("ur5_shoulder_pan_joint")) + robot.set_joint_position("ur5_shoulder_pan_joint", new_pan + robot.get_joint_position("ur5_shoulder_pan_joint")) class DonbotMoveGripper(ProcessModule): @@ -117,30 +100,10 @@ class DonbotMoveGripper(ProcessModule): """ def _execute(self, desig): - robot = BulletWorld.robot + robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_joint_states(robot_description.get_static_gripper_chain(gripper, motion)) - - -class DonbotDetecting(ProcessModule): - """ - This process module tries to detect an object with the given type. To be detected the object has to be in - the field of view of the robot. - """ - - def _execute(self, desig): - robot = BulletWorld.robot - object_type = desig.object_type - # Should be "wide_stereo_optical_frame" - cam_frame_name = robot_description.get_camera_frame() - # should be [0, 0, 1] - front_facing_axis = robot_description.front_facing_axis - - objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type) - for obj in objects: - if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): - return obj + robot.set_positions_of_all_joints(robot_description.get_static_gripper_chain(gripper, motion)) class DonbotMoveTCP(ProcessModule): @@ -150,7 +113,7 @@ class DonbotMoveTCP(ProcessModule): def _execute(self, desig): target = desig.target - robot = BulletWorld.robot + robot = World.robot _move_arm_tcp(target, robot, desig.arm) @@ -162,9 +125,9 @@ class DonbotMoveJoints(ProcessModule): """ def _execute(self, desig: MoveArmJointsMotion.Motion): - robot = BulletWorld.robot + robot = World.robot if desig.left_arm_poses: - robot.set_joint_states(desig.left_arm_poses) + robot.set_positions_of_all_joints(desig.left_arm_poses) class DonbotWorldStateDetecting(ProcessModule): @@ -174,15 +137,7 @@ class DonbotWorldStateDetecting(ProcessModule): def _execute(self, desig: WorldStateDetectingMotion.Motion): obj_type = desig.object_type - return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] - -def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None: - gripper = robot_description.get_tool_frame(arm) - - joints = robot_description.chains[arm].joints - - inv = request_ik(target, robot, joints, gripper) - helper._apply_ik(robot, inv, joints) + return list(filter(lambda obj: obj.type == obj_type, World.current_world.objects))[0] class DonbotManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 5bd04c89e..2467f522e 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -1,13 +1,13 @@ from threading import Lock -from typing import Optional +from typing_extensions import Optional from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager from ..world import World from ..pose import Pose, Point from ..helper import _apply_ik +from ..external_interfaces.ik import request_ik import pycram.world_reasoning as btr -import pybullet as p import logging import time @@ -16,10 +16,11 @@ def calculate_and_apply_ik(robot, gripper: str, target_position: Point, max_iter """ Calculates the inverse kinematics for the given target pose and applies it to the robot. """ - inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), target_position, - maxNumIterations=max_iterations) + target_position_l = [target_position.x, target_position.y, target_position.z] # TODO: Check if this is correct (getting the arm and using its joints), previously joints was not provided. arm = "right" if gripper == robot_description.get_tool_frame("right") else "left" + inv = request_ik(Pose(target_position_l, [0, 0, 0, 1]), + robot, robot_description.chains[arm].joints, gripper) joints = robot_description.chains[arm].joints _apply_ik(robot, inv, joints) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 88b5a1b6d..b8db116cc 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -1,18 +1,25 @@ from threading import Lock -from typing import Any +from typing_extensions import Any, Optional, Tuple +from abc import abstractmethod import actionlib -import pycram.world_reasoning as btr +from .. import world_reasoning as btr import numpy as np import rospy -from ..process_module import ProcessModule +from ..process_module import ProcessModule, ProcessModuleManager from ..external_interfaces.ik import request_ik from ..helper import _apply_ik from ..local_transformer import LocalTransformer -from ..designators.motion_designator import * +from ..designators.object_designator import ObjectDesignatorDescription +from ..designators.motion_designator import MoveMotion, PickUpMotion, PlaceMotion, LookingMotion, \ + DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ + MoveGripperMotion, OpeningMotion, ClosingMotion, MotionDesignatorDescription +from ..robot_descriptions import robot_description +from ..world import World, Object +from ..pose import Pose from ..enums import JointType, ObjectType from ..external_interfaces import giskard from ..external_interfaces.robokudo import query @@ -26,11 +33,11 @@ def _park_arms(arm): """ Defines the joint poses for the parking positions of the arms of PR2 and applies them to the - in the BulletWorld defined robot. + in the World defined robot. :return: None """ - robot = BulletWorld.robot + robot = World.robot if arm == "right": for joint, pose in robot_description.get_static_joint_chain("right", "park").items(): robot.set_joint_position(joint, pose) @@ -45,7 +52,7 @@ class Pr2Navigation(ProcessModule): """ def _execute(self, desig: MoveMotion.Motion): - robot = BulletWorld.robot + robot = World.robot robot.set_pose(desig.target) @@ -55,21 +62,21 @@ class Pr2PickUp(ProcessModule): The object has to be reachable for this process module to succeed. """ - def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.world_object - robot = BulletWorld.robot + def _execute(self, desig: PickUpMotion.Motion, used_arm: Optional[str] = None): + obj = desig.object_desig.world_object + robot = World.robot grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() + target = obj.get_pose() target.orientation.x = grasp[0] target.orientation.y = grasp[1] target.orientation.z = grasp[2] target.orientation.w = grasp[3] - arm = desig.arm + arm = desig.arm if used_arm is None else used_arm _move_arm_tcp(target, robot, arm) tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) + robot.attach(obj, tool_frame) class Pr2Place(ProcessModule): @@ -83,43 +90,69 @@ def _execute(self, desig: PlaceMotion.Motion): :param desig: A PlaceMotion :return: """ - object = desig.object.world_object - robot = BulletWorld.robot + obj = desig.object.world_object + robot = World.robot arm = desig.arm # Transformations such that the target position is the position of the object and not the tcp - object_pose = object.get_pose() + object_pose = obj.get_pose() local_tf = LocalTransformer() tool_name = robot_description.get_tool_frame(arm) tcp_to_object = local_tf.transform_pose(object_pose, robot.links[tool_name].tf_frame) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, arm) - robot.detach(object) + robot.detach(obj) -class Pr2MoveHead(ProcessModule): - """ - This process module moves the head to look at a specific point in the world coordinate frame. - This point can either be a position or an object. +class _Pr2MoveHead(ProcessModule): """ + This process module moves the head to look at a specific point in the world coordinate frame. + This point can either be a position or an object. + """ + def __init__(self, lock: Lock): + super().__init__(lock) + self.robot: Object = World.robot - def _execute(self, desig: LookingMotion.Motion): + def get_pan_and_tilt_goals(self, desig: LookingMotion.Motion) -> Tuple[float, float]: + """ + Calculates the pan and tilt angles to achieve the desired looking motion. + :param desig: The looking motion designator + :return: The pan and tilt angles + """ target = desig.target - robot = BulletWorld.robot local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose(target, robot.links["head_pan_link"].tf_frame) - pose_in_tilt = local_transformer.transform_pose(target, robot.links["head_tilt_link"].tf_frame) + pose_in_pan = local_transformer.transform_pose(target, self.robot.links["head_pan_link"].tf_frame) + pose_in_tilt = local_transformer.transform_pose(target, self.robot.links["head_tilt_link"].tf_frame) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 - current_pan = robot.get_joint_position("head_pan_joint") - current_tilt = robot.get_joint_position("head_tilt_joint") + current_pan = self.robot.get_joint_position("head_pan_joint") + current_tilt = self.robot.get_joint_position("head_tilt_joint") - robot.set_joint_position("head_pan_joint", new_pan + current_pan) - robot.set_joint_position("head_tilt_joint", new_tilt + current_tilt) + return new_pan + current_pan, new_tilt + current_tilt + + @abstractmethod + def _execute(self, designator: LookingMotion.Motion) -> None: + pass + + +class Pr2MoveHead(_Pr2MoveHead): + """ + This process module moves the head to look at a specific point in the world coordinate frame. + This point can either be a position or an object. + """ + + def _execute(self, desig: LookingMotion.Motion): + """ + Moves the head to look at the given position. + :param desig: The looking motion designator + """ + pan_goal, tilt_goal = self.get_pan_and_tilt_goals(desig) + self.robot.set_joint_position("head_pan_joint", pan_goal) + self.robot.set_joint_position("head_tilt_joint", tilt_goal) class Pr2MoveGripper(ProcessModule): @@ -129,7 +162,7 @@ class Pr2MoveGripper(ProcessModule): """ def _execute(self, desig: MoveGripperMotion.Motion): - robot = BulletWorld.robot + robot = World.robot gripper = desig.gripper motion = desig.motion for joint, state in robot_description.get_static_gripper_chain(gripper, motion).items(): @@ -143,14 +176,14 @@ class Pr2Detecting(ProcessModule): """ def _execute(self, desig: DetectingMotion.Motion): - robot = BulletWorld.robot + robot = World.robot object_type = desig.object_type # Should be "wide_stereo_optical_frame" cam_frame_name = robot_description.get_camera_frame() # should be [0, 0, 1] front_facing_axis = robot_description.front_facing_axis - objects = BulletWorld.current_world.get_objects_by_type(object_type) + objects = World.current_world.get_objects_by_type(object_type) for obj in objects: if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis): return obj @@ -163,7 +196,7 @@ class Pr2MoveTCP(ProcessModule): def _execute(self, desig: MoveTCPMotion.Motion): target = desig.target - robot = BulletWorld.robot + robot = World.robot _move_arm_tcp(target, robot, desig.arm) @@ -176,7 +209,7 @@ class Pr2MoveArmJoints(ProcessModule): def _execute(self, desig: MoveArmJointsMotion.Motion): - robot = BulletWorld.robot + robot = World.robot if desig.right_arm_poses: robot.set_positions_of_all_joints(desig.right_arm_poses) if desig.left_arm_poses: @@ -188,7 +221,7 @@ class PR2MoveJoints(ProcessModule): Process Module for generic joint movements, is not confined to the arms but can move any joint of the robot """ def _execute(self, desig: MoveJointsMotion.Motion): - robot = BulletWorld.robot + robot = World.robot robot.set_positions_of_all_joints(dict(zip(desig.names, desig.positions))) @@ -199,7 +232,7 @@ class Pr2WorldStateDetecting(ProcessModule): def _execute(self, desig: WorldStateDetectingMotion.Motion): obj_type = desig.object_type - return list(filter(lambda obj: obj.obj_type == obj_type, BulletWorld.current_world.objects))[0] + return list(filter(lambda obj: obj.obj_type == obj_type, World.current_world.objects))[0] class Pr2Open(ProcessModule): @@ -215,7 +248,7 @@ def _execute(self, desig: OpeningMotion.Motion): goal_pose = btr.link_pose_for_joint_config(part_of_object, { container_joint: part_of_object.get_joint_limits(container_joint)[1] - 0.05}, desig.object_part.name) - _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) + _move_arm_tcp(goal_pose, World.robot, desig.arm) desig.object_part.world_object.set_joint_position(container_joint, part_of_object.get_joint_limits( @@ -235,7 +268,7 @@ def _execute(self, desig: ClosingMotion.Motion): goal_pose = btr.link_pose_for_joint_config(part_of_object, { container_joint: part_of_object.get_joint_limits(container_joint)[0]}, desig.object_part.name) - _move_arm_tcp(goal_pose, BulletWorld.robot, desig.arm) + _move_arm_tcp(goal_pose, World.robot, desig.arm) desig.object_part.world_object.set_joint_position(container_joint, part_of_object.get_joint_limits( @@ -278,29 +311,22 @@ def _execute(self, designator: MotionDesignatorDescription.Motion) -> Any: pass -class Pr2MoveHeadReal(ProcessModule): +class Pr2MoveHeadReal(_Pr2MoveHead): """ Process module for the real robot to move that such that it looks at the given position. Uses the same calculation as the simulated one """ def _execute(self, desig: LookingMotion.Motion): - target = desig.target - robot = BulletWorld.robot - - local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose(target, robot.links["head_pan_link"].tf_frame) - pose_in_tilt = local_transformer.transform_pose(target, robot.links["head_tilt_link"].tf_frame) - - new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) - new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 - - current_pan = robot.get_joint_position("head_pan_joint") - current_tilt = robot.get_joint_position("head_tilt_joint") + """ + Moves the head to look at the given position. + :param desig: The looking motion designator + """ + pan_goal, tilt_goal = self.get_pan_and_tilt_goals(desig) giskard.avoid_all_collisions() - giskard.achieve_joint_goal({"head_pan_joint": new_pan + current_pan, - "head_tilt_joint": new_tilt + current_tilt}) + giskard.achieve_joint_goal({"head_pan_joint": pan_goal, + "head_tilt_joint": tilt_goal}) class Pr2DetectingReal(ProcessModule): @@ -315,14 +341,14 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: obj_pose = query_result["ClusterPoseBBAnnotator"] lt = LocalTransformer() - obj_pose = lt.transform_pose(obj_pose, BulletWorld.robot.links["torso_lift_link"].tf_frame) + obj_pose = lt.transform_pose(obj_pose, World.robot.links["torso_lift_link"].tf_frame) obj_pose.orientation = [0, 0, 0, 1] obj_pose.position.x += 0.05 - bullet_obj = BulletWorld.current_world.get_objects_by_type(designator.object_type) - if bullet_obj: - bullet_obj[0].set_pose(obj_pose) - return bullet_obj[0] + world_obj = World.current_world.get_objects_by_type(designator.object_type) + if world_obj: + world_obj[0].set_pose(obj_pose) + return world_obj[0] elif designator.object_type == ObjectType.JEROEN_CUP: cup = Object("cup", ObjectType.JEROEN_CUP, "jeroen_cup.stl", pose=obj_pose) return cup @@ -330,11 +356,7 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=obj_pose) return bowl - - return bullet_obj[0] - - - + return world_obj[0] class Pr2MoveTCPReal(ProcessModule): @@ -396,7 +418,10 @@ def feedback_callback(msg): goal = Pr2GripperCommandGoal() goal.command.position = 0.0 if designator.motion == "close" else 0.1 goal.command.max_effort = 50.0 - controller_topic = "r_gripper_controller/gripper_action" if designator.gripper == "right" else "l_gripper_controller/gripper_action" + if designator.gripper == "right": + controller_topic = "r_gripper_controller/gripper_action" + else: + controller_topic = "l_gripper_controller/gripper_action" client = actionlib.SimpleActionClient(controller_topic, Pr2GripperCommandAction) rospy.loginfo("Waiting for action server") client.wait_for_server() diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index 9d6122aa2..7285f3107 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -1,14 +1,12 @@ from pycram.external_interfaces.giskard import achieve_cartesian_goal from pycram.designators.location_designator import CostmapLocation -from pycram.world import UseProspectionWorld, BulletWorld -from pycram.helper import _apply_ik +from ...world import UseProspectionWorld, World from pycram.pose import Pose from pycram.robot_descriptions import robot_description from pycram.pose_generator_and_validator import reachability_validator -from typing import Tuple, Dict +from typing_extensions import Tuple, Dict import tf -import numpy as np class GiskardLocation(CostmapLocation): @@ -28,7 +26,7 @@ def __iter__(self) -> CostmapLocation.Location: pose_left, end_config_left = self._get_reachable_pose_for_arm(self.target, robot_description.get_tool_frame("left")) - test_robot = BulletWorld.current_world.get_prospection_object_for_object(BulletWorld.robot) + test_robot = World.current_world.get_prospection_object_for_object(World.robot) with UseProspectionWorld(): valid, arms = reachability_validator(pose_right, test_robot, self.target, {}) if valid: diff --git a/src/pycram/resolver/location/jpt_location.py b/src/pycram/resolver/location/jpt_location.py index b9de0ccb2..5161ee014 100644 --- a/src/pycram/resolver/location/jpt_location.py +++ b/src/pycram/resolver/location/jpt_location.py @@ -1,22 +1,18 @@ import dataclasses -import time -from typing import Optional, List, Tuple +from typing_extensions import Optional, List import jpt import numpy as np -import pybullet import tf -import pycram.designators.location_designator -import pycram.task -from ...costmaps import OccupancyCostmap, plot_grid -from ...plan_failures import PlanFailure +from ...designators import location_designator +from ...costmaps import OccupancyCostmap from ...pose import Pose -from pycram.bullet_world import BulletWorld -from pycram.world import Object +from ...world import Object, World +from ...world_dataclasses import BoxVisualShape, Color -class JPTCostmapLocation(pycram.designators.location_designator.CostmapLocation): +class JPTCostmapLocation(location_designator.CostmapLocation): """Costmap Locations using Joint Probability Trees (JPTs). JPT costmaps are trained to model the dependency with a robot position relative to the object, the robots type, the objects type, the robot torso height, and the grasp parameters. @@ -24,7 +20,7 @@ class JPTCostmapLocation(pycram.designators.location_designator.CostmapLocation) """ @dataclasses.dataclass - class Location(pycram.designators.location_designator.LocationDesignatorDescription.Location): + class Location(location_designator.LocationDesignatorDescription.Location): pose: Pose reachable_arm: str torso_height: float @@ -187,7 +183,7 @@ def __iter__(self): def visualize(self): """ - Plot the possible areas to stand in the BulletWorld. The opacity is the probability of success. + Plot the possible areas to stand in the World. The opacity is the probability of success. """ @@ -210,23 +206,14 @@ def visualize(self): center = np.array([sum(x_range) / 2, sum(y_range) / 2]) - visual = pybullet.createVisualShape(pybullet.GEOM_BOX, - halfExtents=[(x_range[1] - x_range[0]) / 2, - (y_range[1] - y_range[0]) / 2, 0.001], - rgbaColor=[1, 0, 0, success], - visualFramePosition=[*center, 0]) + box_visual_shape = BoxVisualShape(Color(1, 0, 0, success), [*center, 0], + half_extents=[(x_range[1] - x_range[0]) / 2, + (y_range[1] - y_range[0]) / 2, 0.001]) + visual = World.current_world.create_visual_shape(box_visual_shape) self.visual_ids.append(visual) for id_list in np.array_split(np.array(self.visual_ids), np.ceil(len(self.visual_ids) / 127)): - # Dummy paramater since these are needed to spawn visual shapes as a multibody. - link_poses = [[0, 0, 0] for c in id_list] - link_orientations = [[0, 0, 0, 1] for c in id_list] - link_masses = [1.0 for c in id_list] - link_parent = [0 for c in id_list] - link_joints = [pybullet.JOINT_FIXED for c in id_list] - link_collision = [-1 for c in id_list] - link_joint_axis = [[1, 0, 0] for c in id_list] # The position at which the multibody will be spawned. Offset such that # the origin referes to the centre of the costmap. @@ -234,15 +221,8 @@ def visualize(self): base_position = list(origin_pose[0]) base_position[2] = 0 - map_obj = pybullet.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=id_list, - basePosition=base_position, baseOrientation=origin_pose[1], - linkPositions=link_poses, - linkMasses=link_masses, linkOrientations=link_orientations, - linkInertialFramePositions=link_poses, - linkInertialFrameOrientations=link_orientations, - linkParentIndices=link_parent, - linkJointTypes=link_joints, linkJointAxis=link_joint_axis, - linkCollisionShapeIndices=link_collision) + map_obj = World.current_world.create_multi_body_from_visual_shapes(id_list.tolist(), Pose(base_position, + origin_pose[1])) self.visual_ids.append(map_obj) def close_visualization(self) -> None: @@ -250,6 +230,6 @@ def close_visualization(self) -> None: Close all plotted objects. """ - for id in self.visual_ids: - pybullet.removeBody(id) + for _id in self.visual_ids: + World.current_world.remove_object(World.current_world.get_object_by_id(_id)) self.visual_ids = [] diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index ab896d1e1..795330b90 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -3,7 +3,7 @@ import rospkg from copy import deepcopy from numbers import Number -from typing import List, Optional, Dict, Union, Type +from typing_extensions import List, Optional, Dict, Union, Type from urdf_parser_py.urdf import URDF from . import utils diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros/force_torque_sensor.py index 3b9b21af4..ed68a1a86 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros/force_torque_sensor.py @@ -3,11 +3,10 @@ import threading import rospy -import pybullet as pb from geometry_msgs.msg import WrenchStamped from std_msgs.msg import Header -from..bullet_world import BulletWorld +from ..world import World class ForceTorqueSensor: @@ -18,21 +17,22 @@ class ForceTorqueSensor: """ def __init__(self, joint_name, fts_topic="/pycram/fts", interval=0.1): """ - The given joint_name has to be part of :py:attr:`~pycram.world.BulletWorld.robot` otherwise a + The given joint_name has to be part of :py:attr:`~pycram.world.World.robot` otherwise a RuntimeError will be raised. :param joint_name: Name of the joint for which force-torque should be simulated :param fts_topic: Name of the ROS topic to which should be published :param interval: Interval at which the messages should be published, in seconds """ - self.world = BulletWorld.current_world + self.world = World.current_world self.fts_joint_idx = None self.joint_name = joint_name if joint_name in self.world.robot.joint_name_to_id.keys(): self.fts_joint_idx = self.world.robot.joint_name_to_id[joint_name] else: - raise RuntimeError(f"Could not register ForceTorqueSensor: Joint {joint_name} does not exist in robot object") - pb.enableJointForceTorqueSensor(self.world.robot.id, self.fts_joint_idx, enableSensor=1) + raise RuntimeError(f"Could not register ForceTorqueSensor: Joint {joint_name}" + f" does not exist in robot object") + self.world.enable_joint_force_torque_sensor(self.world.robot, self.fts_joint_idx) self.fts_pub = rospy.Publisher(fts_topic, WrenchStamped, queue_size=10) self.interval = interval @@ -50,8 +50,7 @@ def _publish(self) -> None: """ seq = 0 while not self.kill_event.is_set(): - current_joint_state = pb.getJointState(self.world.robot.id, self.fts_joint_idx) - joint_ft = current_joint_state[2] + joint_ft = self.world.get_joint_force_torque(self.world.robot, self.fts_joint_idx) h = Header() h.seq = seq h.stamp = rospy.Time.now() diff --git a/src/pycram/ros/joint_state_publisher.py b/src/pycram/ros/joint_state_publisher.py index fb7682e19..75e4c084c 100644 --- a/src/pycram/ros/joint_state_publisher.py +++ b/src/pycram/ros/joint_state_publisher.py @@ -6,22 +6,22 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header -from ..bullet_world import BulletWorld +from ..world import World class JointStatePublisher: """ - Joint state publisher for the robot currently loaded in the BulletWorld + Joint state publisher for the robot currently loaded in the World """ def __init__(self, joint_state_topic="/pycram/joint_state", interval=0.1): """ - Robot object is from :py:attr:`~pycram.world.BulletWorld.robot` and current joint states are published to + Robot object is from :py:attr:`~pycram.world.World.robot` and current joint states are published to the given joint_state_topic as a JointState message. :param joint_state_topic: Topic name to which the joint states should be published :param interval: Interval at which the joint states should be published, in seconds """ - self.world = BulletWorld.current_world + self.world = World.current_world self.joint_state_pub = rospy.Publisher(joint_state_topic, JointState, queue_size=10) self.interval = interval @@ -33,10 +33,11 @@ def __init__(self, joint_state_topic="/pycram/joint_state", interval=0.1): def _publish(self) -> None: """ - Publishes the current joint states of the :py:attr:`~pycram.world.BulletWorld.robot` in an infinite loop. - The joint states are published as long as the kill_event is not set by :py:meth:`~JointStatePublisher._stop_publishing` + Publishes the current joint states of the :py:attr:`~pycram.world.World.robot` in an infinite loop. + The joint states are published as long as the kill_event is not set + by :py:meth:`~JointStatePublisher._stop_publishing` """ - robot = BulletWorld.robot + robot = World.robot joint_names = [joint_name for joint_name in robot.joint_name_to_id.keys()] seq = 0 diff --git a/src/pycram/ros/robot_state_updater.py b/src/pycram/ros/robot_state_updater.py index f8f8bab82..93ab7ecce 100644 --- a/src/pycram/ros/robot_state_updater.py +++ b/src/pycram/ros/robot_state_updater.py @@ -1,19 +1,18 @@ import rospy -import threading import atexit import tf import time from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState -from pycram.bullet_world import BulletWorld -from pycram.robot_descriptions import robot_description -from pycram.pose import Transform, Pose +from ..world import World +from ..robot_descriptions import robot_description +from ..pose import Pose class RobotStateUpdater: """ - Updates the robot in the Bullet World with information of the real robot published to ROS topics. + Updates the robot in the World with information of the real robot published to ROS topics. Infos used to update the robot are: * The current pose of the robot * The current joint state of the robot @@ -31,10 +30,8 @@ def __init__(self, tf_topic: str, joint_state_topic: str): self.tf_topic = tf_topic self.joint_state_topic = joint_state_topic - self.tf_timer = rospy.Timer(rospy.Duration(0.1), self._subscribe_tf) - self.joint_state_timer = rospy.Timer(rospy.Duration(0.1), self._subscribe_joint_state) - - + self.tf_timer = rospy.Timer(rospy.Duration.from_sec(0.1), self._subscribe_tf) + self.joint_state_timer = rospy.Timer(rospy.Duration.from_sec(0.1), self._subscribe_joint_state) atexit.register(self._stop_subscription) @@ -45,27 +42,26 @@ def _subscribe_tf(self, msg: TransformStamped) -> None: :param msg: TransformStamped message published to the topic """ trans, rot = self.tf_listener.lookupTransform("/map", robot_description.base_frame, rospy.Time(0)) - BulletWorld.robot.set_pose(Pose(trans, rot)) + World.robot.set_pose(Pose(trans, rot)) def _subscribe_joint_state(self, msg: JointState) -> None: """ - Sets the current joint configuration of the robot in the bullet world to the configuration published on the topic. - Since this uses rospy.wait_for_message which can have errors when used with threads there might be an attribute error - in the rospy implementation. + Sets the current joint configuration of the robot in the world to the configuration published on the + topic. Since this uses rospy.wait_for_message which can have errors when used with threads there might be an + attribute error in the rospy implementation. :param msg: JointState message published to the topic. """ try: msg = rospy.wait_for_message(self.joint_state_topic, JointState) for name, position in zip(msg.name, msg.position): - BulletWorld.robot.set_joint_position(name, position) + World.robot.set_joint_position(name, position) except AttributeError: pass - def _stop_subscription(self) -> None: """ - Stops the Timer for TF and joint states and therefore the updating of the robot in the bullet world. + Stops the Timer for TF and joint states and therefore the updating of the robot in the world. """ self.tf_timer.shutdown() self.joint_state_timer.shutdown() diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index 5d6930522..e3be3bc6d 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -4,13 +4,13 @@ import atexit from ..pose import Pose -from ..bullet_world import BulletWorld +from ..world import World from tf2_msgs.msg import TFMessage class TFBroadcaster: """ - Broadcaster that publishes TF frames for every object in the BulletWorld. + Broadcaster that publishes TF frames for every object in the World. """ def __init__(self, projection_namespace="simulated", odom_frame="odom", interval=0.1): """ @@ -21,7 +21,7 @@ def __init__(self, projection_namespace="simulated", odom_frame="odom", interval :param odom_frame: Name of the statically published odom frame :param interval: Interval at which the TFs should be published, in seconds """ - self.world = BulletWorld.current_world + self.world = World.current_world self.tf_static_publisher = rospy.Publisher("/tf_static", TFMessage, queue_size=10) self.tf_publisher = rospy.Publisher("/tf", TFMessage, queue_size=10) @@ -39,7 +39,7 @@ def __init__(self, projection_namespace="simulated", odom_frame="odom", interval def update(self): """ - Updates the TFs for the static odom frame and all objects currently in the BulletWorld. + Updates the TFs for the static odom frame and all objects currently in the World. """ # Update static odom self._update_static_odom() @@ -48,7 +48,7 @@ def update(self): def _update_objects(self) -> None: """ - Publishes the current pose of all objects in the BulletWorld. As well as the poses of all links of these objects. + Publishes the current pose of all objects in the World. As well as the poses of all links of these objects. """ for obj in self.world.objects: pose = obj.get_pose() @@ -89,7 +89,7 @@ def _publish_pose(self, child_frame_id: str, pose: Pose, static=False) -> None: def _publish(self) -> None: """ - Constantly publishes the positions of all objects in the BulletWorld. + Constantly publishes the positions of all objects in the World. """ while not self.kill_event.is_set(): self.update() diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 0eb2cb5b2..d47279a90 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -5,22 +5,21 @@ from geometry_msgs.msg import Vector3 from std_msgs.msg import ColorRGBA -from pycram.bullet_world import BulletWorld +from ..world import World from visualization_msgs.msg import MarkerArray, Marker import rospy import urdf_parser_py -from tf.transformations import quaternion_from_euler from pycram.pose import Transform class VizMarkerPublisher: """ - Publishes an Array of visualization marker which represent the situation in the Bullet World + Publishes an Array of visualization marker which represent the situation in the World """ def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): """ - The Publisher creates an Array of Visualization marker with a Marker for each link of each Object in the Bullet + The Publisher creates an Array of Visualization marker with a Marker for each link of each Object in the World. This Array is published with a rate of interval. :param topic_name: The name of the topic to which the Visualization Marker should be published. @@ -56,7 +55,7 @@ def _make_marker_array(self) -> MarkerArray: :return: An Array of Visualization Marker """ marker_array = MarkerArray() - for obj in BulletWorld.current_world.objects: + for obj in World.current_world.objects: if obj.name == "floor": continue for link in obj.link_name_to_id.keys(): diff --git a/src/pycram/task.py b/src/pycram/task.py index 8bc63d246..b8baacbe0 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -7,19 +7,19 @@ import inspect import json import logging -from typing import List, Dict, Optional, Callable, Any +from typing_extensions import List, Dict, Optional, Callable import anytree -import pybullet import sqlalchemy.orm.session import tqdm -from .bullet_world import BulletWorld +from .world import World from .orm.task import (Code as ORMCode, TaskTreeNode as ORMTaskTreeNode) from .orm.base import ProcessMetaData from .plan_failures import PlanFailure from .language import Code from .enums import TaskStatus +from .world_dataclasses import Color class TaskCode(Code): @@ -215,18 +215,18 @@ class SimulatedTaskTree: """TaskTree for execution in a 'new' simulation.""" def __enter__(self): - """At the beginning of a with statement the current task tree and bullet world will be suspended and remembered. + """At the beginning of a with statement the current task tree and world will be suspended and remembered. Fresh structures are then available inside the with statement.""" global task_tree def simulation(): return None self.suspended_tree = task_tree - self.world_state = BulletWorld.current_world.save_state() + self.world_state = World.current_world.save_state() self.simulated_root = TaskTreeNode(code=TaskCode(simulation)) task_tree = self.simulated_root - pybullet.addUserDebugText("Simulating...", [0, 0, 1.75], textColorRGB=[0, 0, 0], - parentObjectUniqueId=1, lifeTime=0) + World.current_world.add_text("Simulating...", [0, 0, 1.75], color=Color.from_rgb([0, 0, 0]), + parent_object_id=1) return self def __exit__(self, exc_type, exc_val, exc_tb): @@ -235,8 +235,8 @@ def __exit__(self, exc_type, exc_val, exc_tb): """ global task_tree task_tree = self.suspended_tree - BulletWorld.current_world.restore_state(self.world_state) - pybullet.removeAllUserDebugItems() + World.current_world.restore_state(self.world_state) + World.current_world.remove_text() task_tree: Optional[TaskTreeNode] = None diff --git a/src/pycram/world.py b/src/pycram/world.py index 0a859b2a7..b520779b9 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -58,8 +58,8 @@ class World(ABC): robot: Optional[Object] = None """ - Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the - URDF with the name of the URDF on the parameter server. + Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in + the URDF with the name of the URDF on the parameter server. """ data_directory: List[str] = [os.path.join(os.path.dirname(__file__), '..', '..', 'resources')] @@ -170,6 +170,7 @@ def load_urdf_and_get_object_id(self, path: str, pose: Pose) -> int: """ pass + # TODO: This is not used anywhere, should it be removed? def get_objects_by_name(self, name: str) -> List[Object]: """ Returns a list of all Objects in this World with the same name as the given one. @@ -179,7 +180,7 @@ def get_objects_by_name(self, name: str) -> List[Object]: """ return list(filter(lambda obj: obj.name == name, self.objects)) - def get_objects_by_type(self, obj_type: str) -> List[Object]: + def get_objects_by_type(self, obj_type: ObjectType) -> List[Object]: """ Returns a list of all Objects which have the type 'obj_type'. @@ -357,6 +358,7 @@ def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: :param real_time: If the simulation should happen in real time or faster. """ for i in range(0, int(seconds * self.simulation_frequency)): + curr_time = rospy.Time.now() self.step() for objects, callbacks in self.coll_callbacks.items(): contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1]) @@ -365,7 +367,8 @@ def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: elif callbacks.no_collision_cb is not None: callbacks.no_collision_cb() if real_time: - time.sleep(self.simulation_time_step) + loop_time = rospy.Time.now() - curr_time + time.sleep(max(0, self.simulation_time_step - loop_time.to_sec())) @abstractmethod def perform_collision_detection(self) -> None: @@ -433,7 +436,6 @@ def get_link_names(self, obj: Object) -> List[str]: """ pass - @abstractmethod def get_number_of_joints(self, obj: Object) -> int: """ Get the number of joints of an articulated object @@ -899,6 +901,36 @@ def create_visual_shape(self, visual_shape: VisualShape) -> int: """ raise NotImplementedError + def create_multi_body_from_visual_shapes(self, visual_shape_ids: List[int], pose: Pose) -> int: + """ + Creates a multi body from visual shapes in the physics simulator and returns the unique id of the created + multi body. + :param visual_shape_ids: The ids of the visual shapes that should be used to create the multi body. + :param pose: The pose of the origin of the multi body relative to the world frame. + :return: The unique id of the created multi body. + """ + # Dummy paramater since these are needed to spawn visual shapes as a + # multibody. + num_of_shapes = len(visual_shape_ids) + link_poses = [[0, 0, 0] for _ in range(num_of_shapes)] + link_orientations = [[0, 0, 0, 1] for _ in range(num_of_shapes)] + link_masses = [1.0 for _ in range(num_of_shapes)] + link_parent = [0 for _ in range(num_of_shapes)] + link_joints = [JointType.FIXED for _ in range(num_of_shapes)] + link_collision = [-1 for _ in range(num_of_shapes)] + link_joint_axis = [[1, 0, 0] for _ in range(num_of_shapes)] + + multi_body = MultiBody(base_visual_shape_index=-1, base_position=pose.position_as_list(), + base_orientation=pose.orientation_as_list(), + link_visual_shape_indices=visual_shape_ids, link_positions=link_poses, + link_orientations=link_orientations, link_masses=link_masses, + link_inertial_frame_positions=link_poses, + link_inertial_frame_orientations=link_orientations, + link_parent_indices=link_parent, link_joint_types=link_joints, + link_joint_axis=link_joint_axis, + link_collision_shape_indices=link_collision) + return self.create_multi_body(multi_body) + def create_multi_body(self, multi_body: MultiBody) -> int: """ Creates a multi body in the physics simulator and returns the unique id of the created multi body. The multibody @@ -962,6 +994,69 @@ def create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: """ raise NotImplementedError + def add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, size: float = 0.1, + color: Optional[Color] = Color(), life_time: Optional[float] = 0, + parent_object_id: Optional[int] = None) -> None: + """ + Adds text to the world. + :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 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 parent_object_id: The id of the object to which the text should be attached. + """ + raise NotImplementedError + + def remove_text(self, text_id: Optional[int] = None) -> None: + """ + Removes text from the world using the given id. if no id is given all text will be removed. + :param text_id: The id of the text to be removed. + """ + raise NotImplementedError + + def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: + """ + You can enable a joint force/torque sensor in each joint. Once enabled, if you perform + a simulation step, the get_joint_force_torque will report the joint reaction forces in the fixed degrees of + freedom: a fixed joint will measure all 6DOF joint forces/torques. A revolute/hinge joint + force/torque sensor will measure 5DOF reaction forces along all axis except the hinge axis. The + applied force by a joint motor is available through get_applied_joint_motor_torque. + :param obj: The object in which the joint is located. + :param fts_joint_idx: The index of the joint for which the force torque sensor should be enabled. + """ + raise NotImplementedError + + def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: + """ + Disables the force torque sensor of a joint. + :param obj: The object in which the joint is located. + :param joint_id: The id of the joint for which the force torque sensor should be disabled. + """ + raise NotImplementedError + + def get_joint_force_torque(self, obj: Object, joint_id: int) -> List[float]: + """ + Returns the joint reaction forces and torques of the specified joint. + :param obj: The object in which the joint is located. + :param joint_id: The id of the joint for which the force torque should be returned. + :return: The joint reaction forces and torques of the specified joint. + """ + raise NotImplementedError + + def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: + """ + Returns the applied torque by a joint motor. + :param obj: The object in which the joint is located. + :param joint_id: The id of the joint for which the applied motor torque should be returned. + :return: The applied torque by a joint motor. + """ + raise NotImplementedError + class UseProspectionWorld: """ @@ -1030,8 +1125,8 @@ def run(self): terminate flag is set. While this loop runs it continuously checks the cartesian and joint position of every object in the World and updates the corresponding object in the - prospection world. When there are entries in the adding or removing queue the corresponding objects will be added - or removed in the same iteration. + prospection world. When there are entries in the adding or removing queue the corresponding objects will + be added or removed in the same iteration. """ while not self.terminate: self.check_for_pause() @@ -1382,7 +1477,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.original_pose = self.local_transformer.transform_pose(pose, "map") self._current_pose = self.original_pose - self._load_object_and_set_id_and_path(path, ignore_cached_files) + self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) @@ -1409,8 +1504,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) - - def _load_object_and_set_id_and_path(self, path, ignore_cached_files: bool) -> None: + def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, str]: """ Loads an object to the given World with the given position and orientation. The rgba_color will only be used when an .obj or .stl file is given. @@ -1421,6 +1515,7 @@ def _load_object_and_set_id_and_path(self, path, ignore_cached_files: bool) -> N to ROS packges instead there will be absolute file paths. :param ignore_cached_files: Whether to ignore files in the cache directory. + :return: The unique id of the object and the path of the file that was loaded. """ pa = pathlib.Path(path) extension = pa.suffix @@ -1469,8 +1564,7 @@ def _load_object_and_set_id_and_path(self, path, ignore_cached_files: bool) -> N try: obj_id = self.world.load_urdf_and_get_object_id(path, Pose(self.get_position_as_list(), self.get_orientation_as_list())) - self.id = obj_id - self.path = path + return obj_id, path except Exception as e: logging.error( "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" @@ -1983,7 +2077,8 @@ def contact_points_simulated(self) -> List: def update_joints_from_topic(self, topic_name: str) -> None: """ Updates the joints of this object with positions obtained from a topic with the message type JointState. - Joint names on the topic have to correspond to the joints of this object otherwise an error message will be logged. + Joint names on the topic have to correspond to the joints of this object otherwise an error message will be + logged. :param topic_name: Name of the topic with the joint states """ diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 6eff03e2a..40d4d8db2 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -183,83 +183,47 @@ class MultiBody: link_collision_shape_indices: List[int] -@dataclass -class ShapeData: - pass - - -@dataclass -class BoxShapeData(ShapeData): - half_extents: List[float] - - -@dataclass -class SphereShapeData(ShapeData): - radius: float - - -@dataclass -class CapsuleShapeData(SphereShapeData): - length: float - - -@dataclass -class CylinderShapeData(CapsuleShapeData): - pass - - -@dataclass -class MeshShapeData(ShapeData): - mesh_scale: List[float] - mesh_file_name: str - - -@dataclass -class PlaneShapeData(ShapeData): - normal: List[float] - - @dataclass class VisualShape: rgba_color: Color visual_frame_position: List[float] - shape_data: ShapeData - visual_geometry_type: Shape @dataclass class BoxVisualShape(VisualShape): - shape_data: BoxShapeData - visual_geometry_type = Shape.BOX + half_extents: List[float] + visual_geometry_type: Optional[Shape] = Shape.BOX @dataclass class SphereVisualShape(VisualShape): - shape_data: SphereShapeData - visual_geometry_type = Shape.SPHERE + radius: float + visual_geometry_type: Optional[Shape] = Shape.SPHERE @dataclass -class CapsuleVisualShape(SphereVisualShape): - shape_data: CapsuleShapeData - visual_geometry_type = Shape.CAPSULE +class CapsuleVisualShape(VisualShape): + radius: float + length: float + visual_geometry_type: Optional[Shape] = Shape.CAPSULE @dataclass class CylinderVisualShape(CapsuleVisualShape): - visual_geometry_type = Shape.CYLINDER + visual_geometry_type: Optional[Shape] = Shape.CYLINDER @dataclass class MeshVisualShape(VisualShape): - visual_geometry_type = Shape.MESH - shape_data: MeshShapeData + mesh_scale: List[float] + mesh_file_name: str + visual_geometry_type: Optional[Shape] = Shape.MESH @dataclass class PlaneVisualShape(VisualShape): - shape_data: PlaneShapeData - visual_geometry_type = Shape.PLANE + normal: List[float] + visual_geometry_type: Optional[Shape] = Shape.PLANE @dataclass diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index f9fc06319..01d9709a8 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -1,5 +1,5 @@ import itertools -from typing import List, Tuple, Optional, Union, Dict +from typing_extensions import List, Tuple, Optional, Union, Dict import numpy as np @@ -23,7 +23,7 @@ def __init__(*args, **kwargs): def stable(obj: Object) -> bool: """ Checks if an object is stable in the world. Stable meaning that it's position will not change after simulating - physics in the BulletWorld. This will be done by simulating the world for 10 seconds and compare + physics in the World. This will be done by simulating the world for 10 seconds and compare the previous coordinates with the coordinates after the simulation. :param obj: The object which should be checked @@ -78,6 +78,13 @@ def contact( def get_visible_objects( camera_pose: Pose, front_facing_axis: Optional[List[float]] = None) -> Tuple[np.ndarray, Pose]: + """ + Returns a segmentation mask of the objects that are visible from the given camera pose and the front facing axis. + :param camera_pose: The pose of the camera in world coordinate frame. + :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :return: A segmentation mask of the objects that are visible and the pose of the point at exactly 2 meters in front + of the camera in the direction of the front facing axis with respect to the world coordinate frame. + """ front_facing_axis = robot_description.front_facing_axis if not front_facing_axis else front_facing_axis world_to_cam = camera_pose.to_transform("camera") From 50adda9e548cdb478f4871546abd7190b0e216b8 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 1 Feb 2024 21:11:33 +0100 Subject: [PATCH 44/69] [Abstract World] all change requests completed Update positions of objects when simulating Increasing code coverage by tests (reached 55% for bullet_world.py) (77% world.py) --- examples/bullet_world.ipynb | 57 +--- src/pycram/bullet_world.py | 312 +++++------------- src/pycram/costmaps.py | 2 +- src/pycram/helper.py | 2 +- src/pycram/plan_failures.py | 10 + src/pycram/pose.py | 7 +- src/pycram/pose_generator_and_validator.py | 4 +- .../process_modules/boxy_process_modules.py | 10 +- .../process_modules/donbot_process_modules.py | 12 +- .../process_modules/hsr_process_modules.py | 2 +- .../process_modules/pr2_process_modules.py | 6 +- .../resolver/location/giskard_location.py | 10 +- src/pycram/ros/force_torque_sensor.py | 2 +- src/pycram/world.py | 297 +++++++---------- src/pycram/world_dataclasses.py | 115 ++++--- src/pycram/world_reasoning.py | 10 - test/test_attachment.py | 4 +- test/test_bullet_world.py | 45 ++- test/test_language.py | 4 +- test/test_links.py | 4 +- test/test_object.py | 4 +- 21 files changed, 385 insertions(+), 534 deletions(-) diff --git a/examples/bullet_world.ipynb b/examples/bullet_world.ipynb index c487ec212..6a7423953 100644 --- a/examples/bullet_world.ipynb +++ b/examples/bullet_world.ipynb @@ -17,8 +17,8 @@ "id": "23bbba35", "metadata": { "ExecuteTime": { - "end_time": "2024-01-25T18:29:09.773814091Z", - "start_time": "2024-01-25T18:29:08.901441651Z" + "end_time": "2024-02-01T16:28:16.307401805Z", + "start_time": "2024-02-01T16:28:15.499235438Z" } }, "outputs": [ @@ -26,61 +26,22 @@ "name": "stderr", "output_type": "stream", "text": [ - "pybullet build time: Nov 28 2023 23:51:11\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1706207349.376331]: Failed to import Giskard messages\n", - "[WARN] [1706207349.395439]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "Exception in thread Thread-11:\n", - "Traceback (most recent call last):\n", - " File \"/usr/lib/python3.8/threading.py\", line 932, in _bootstrap_inner\n", - " self.run()\n", - " File \"/home/bassioun/cram_ws/src/pycram/src/pycram/bullet_world.py\", line 527, in run\n", - " keys = p.getKeyboardEvents(physicalClientId=self.world.client_id)\n", - "TypeError: 'physicalClientId' is an invalid keyword argument for this function\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "startThreads creating 1 threads.\n", - "starting thread 0\n", - "started thread 0 \n", - "argc=2\n", - "argv[0] = --unused\n", - "argv[1] = --start_demo_name=Physics Server\n", - "ExampleBrowserThreadFunc started\n", - "X11 functions dynamically loaded using dlopen/dlsym OK!\n", - "X11 functions dynamically loaded using dlopen/dlsym OK!\n", - "Creating context\n", - "Created GL 3.3 context\n", - "Direct GLX rendering context obtained\n", - "Making context current\n", - "GL_VENDOR=NVIDIA Corporation\n", - "GL_RENDERER=NVIDIA GeForce RTX 4070/PCIe/SSE2\n", - "GL_VERSION=3.3.0 NVIDIA 535.154.05\n", - "GL_SHADING_LANGUAGE_VERSION=3.30 NVIDIA via Cg compiler\n", - "pthread_getconcurrency()=0\n", - "Version = 3.3.0 NVIDIA 535.154.05\n", - "Vendor = NVIDIA Corporation\n", - "Renderer = NVIDIA GeForce RTX 4070/PCIe/SSE2\n", - "b3Printf: Selected demo: Physics Server\n", - "startThreads creating 1 threads.\n", - "starting thread 0\n", - "started thread 0 \n", - "MotionThreadFunc thread started\n" + "Failed to import Giskard messages\n", + "Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "pybullet build time: Nov 28 2023 23:51:11\n" ] } ], "source": [ "from pycram.bullet_world import BulletWorld\n", "from pycram.pose import Pose\n", - "from pycram.enums import ObjectType\n", + "from pycram.enums import ObjectType, WorldMode\n", "\n", - "world = BulletWorld()" + "world = BulletWorld(mode=WorldMode.GUI)" ] }, { @@ -130,8 +91,8 @@ "id": "48a3aed2", "metadata": { "ExecuteTime": { - "end_time": "2024-01-25T18:29:17.841474190Z", - "start_time": "2024-01-25T18:29:17.782682388Z" + "end_time": "2024-02-01T16:14:50.629562309Z", + "start_time": "2024-02-01T16:14:50.621645448Z" } }, "outputs": [], diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index d268d37f4..68878541f 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -13,7 +13,7 @@ from .enums import JointType, ObjectType, WorldMode from .pose import Pose from .world import World, Object, Link -from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox, MultiBody, VisualShape +from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape from dataclasses import asdict @@ -65,212 +65,100 @@ def remove_object_from_simulator(self, obj: Object) -> None: p.removeBody(obj.id, self.client_id) def add_constraint(self, constraint: Constraint) -> int: - """ - Add a constraint between two objects so that attachment they become attached - """ - - parent_obj = self.get_object_by_id(constraint.parent_obj_id) - parent_link_id = parent_obj.link_name_to_id[constraint.parent_link_name] - child_obj = self.get_object_by_id(constraint.child_obj_id) - child_link_id = child_obj.link_name_to_id[constraint.child_link_name] - - constraint_id = p.createConstraint(constraint.parent_obj_id, - parent_link_id, - constraint.child_obj_id, - child_link_id, + joint_axis_in_child_link_frame = [constraint.joint_axis_in_child_link_frame.x, + constraint.joint_axis_in_child_link_frame.y, + constraint.joint_axis_in_child_link_frame.z] + + constraint_id = p.createConstraint(constraint.parent_link.get_object_id(), + constraint.parent_link.id, + constraint.child_link.get_object_id(), + constraint.child_link.id, constraint.joint_type.value, - constraint.joint_axis_in_child_link_frame, - constraint.joint_frame_position_wrt_parent_origin, - constraint.joint_frame_position_wrt_child_origin, - constraint.joint_frame_orientation_wrt_parent_origin, - constraint.joint_frame_orientation_wrt_child_origin, + joint_axis_in_child_link_frame, + constraint.joint_frame_pose_wrt_parent_origin.position_as_list(), + constraint.joint_frame_pose_wrt_child_origin.position_as_list(), + constraint.joint_frame_pose_wrt_parent_origin.orientation_as_list(), + constraint.joint_frame_pose_wrt_child_origin.orientation_as_list(), physicsClientId=self.client_id) return constraint_id def remove_constraint(self, constraint_id): - print("Removing constraint with id: ", constraint_id) p.removeConstraint(constraint_id, physicsClientId=self.client_id) - def get_joint_rest_pose(self, obj: Object, joint_name: str) -> float: - """ - Get the joint rest pose of an articulated object - - :param obj: The object - :param joint_name: The name of the joint - """ + def get_joint_rest_position(self, obj: Object, joint_name: str) -> float: return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[0] def get_joint_damping(self, obj: Object, joint_name: str) -> float: - """ - Get the joint damping of an articulated object - - :param obj: The object - :param joint_name: The name of the joint - """ return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[6] - def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: - """ - Get the joint upper limit of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: The joint upper limit as a float. - """ + def get_joint_upper_limit(self, obj: Object, joint_name: str) -> float: return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[8] - def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: - """ - Get the joint lower limit of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: The joint lower limit as a float. - """ + def get_joint_lower_limit(self, obj: Object, joint_name: str) -> float: return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[9] - def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: - """ - Returns the axis along which a joint is moving. The given joint_name has to be part of this object. - - :param obj: The object - :param joint_name: Name of the joint for which the axis should be returned. - :return: The axis a vector of xyz - """ + def get_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[13] - def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: - """ - Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - - :param obj: The object - :param joint_name: Joint name for which the type should be returned - :return: The type of the joint - """ + def get_joint_type(self, obj: Object, joint_name: str) -> JointType: joint_type = p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[2] return JointType(joint_type) - def get_object_joint_position(self, obj: Object, joint_name: str) -> float: - """ - Get the state of a joint of an articulated object - - :param obj: The object - :param joint_name: The name of the joint - """ + def get_joint_position(self, obj: Object, joint_name: str) -> float: return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[0] def get_link_pose(self, link: Link) -> Pose: - """ - Get the pose of a link of an articulated object with respect to the world frame. - """ return Pose(*p.getLinkState(link.get_object_id(), link.id, physicsClientId=self.client_id)[4:6]) def perform_collision_detection(self) -> None: - """ - Performs collision detection and updates the contact points. - """ p.performCollisionDetection(physicsClientId=self.client_id) def get_object_contact_points(self, obj: Object) -> List: """ - Returns a list of contact points of this Object with other Objects. For a more detailed explanation of the + For a more detailed explanation of the returned list please look at: `PyBullet Doc `_ - - :param obj: The object. - :return: A list of all contact points with other objects """ self.perform_collision_detection() return p.getContactPoints(obj.id, physicsClientId=self.client_id) def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: - """ - Returns a list of contact points between obj1 and obj2. - - :param obj1: The first object. - :param obj2: The second object. - :return: A list of all contact points between the two objects. - """ self.perform_collision_detection() return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.client_id) def get_joint_names(self, obj: Object) -> List[str]: - """ - Get the names of all joints of an articulated object. - """ num_joints = self.get_number_of_joints(obj) return [p.getJointInfo(obj.id, i, physicsClientId=self.client_id)[1].decode('utf-8') for i in range(num_joints)] def get_link_names(self, obj: Object) -> List[str]: - """ - Get the names of all joints of an articulated object. - """ num_links = self.get_number_of_links(obj) return [p.getJointInfo(obj.id, i, physicsClientId=self.client_id)[12].decode('utf-8') for i in range(num_links)] def get_number_of_links(self, obj: Object) -> int: - """ - Get the number of links of an articulated object - - :param obj: The object - """ return self.get_number_of_joints(obj) def get_number_of_joints(self, obj: Object) -> int: - """ - Get the number of joints of an articulated object - - :param obj: The object - """ return p.getNumJoints(obj.id, physicsClientId=self.client_id) def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: - """ - Reset the joint position instantly without physics simulation - - :param obj: The object - :param joint_name: The name of the joint - :param joint_pose: The new joint pose - """ p.resetJointState(obj.id, obj.joint_name_to_id[joint_name], joint_pose, physicsClientId=self.client_id) def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): - """ - 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 position: The new position of the object as a vector of x,y,z - :param orientation: The new orientation of the object as a quaternion of x,y,z,w - """ p.resetBasePositionAndOrientation(obj.id, position, orientation, physicsClientId=self.client_id) def step(self): - """ - Step the world simulation using forward dynamics - """ p.stepSimulation(physicsClientId=self.client_id) - def set_link_color(self, link: Link, rgba_color: Color): - """ - Changes the rgba_color of a link of this object, the rgba_color has to be given as a 4 element list - of RGBA values. + def update_obj_pose(self, obj: Object) -> None: + obj.set_pose(Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.client_id))) - :param link: The link which should be colored. - :param rgba_color: The rgba_color as RGBA values between 0 and 1 - """ + def set_link_color(self, link: Link, rgba_color: Color): p.changeVisualShape(link.get_object_id(), link.id, rgbaColor=rgba_color, physicsClientId=self.client_id) def get_link_color(self, link: Link) -> Color: return self.get_colors_of_object_links(link.object)[link.name] def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: - """ - Get the RGBA colors of each link in the object as a dictionary from link name to rgba_color. - - :param obj: The object - :return: A dictionary with link names as keys and 4 element list with the RGBA values for each link as value. - """ visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) link_id_to_name = {v: k for k, v in obj.link_name_to_id.items()} links = list(map(lambda x: link_id_to_name[x[1]] if x[1] != -1 else "base", visual_data)) @@ -279,56 +167,23 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: return link_to_color def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: - """ - Returns the axis aligned bounding box of this object. The return of this method are two points in - world coordinate frame which define a bounding box. - - :param obj: The object for which the bounding box should be returned. - :return: AxisAlignedBoundingBox object with min and max box points. - """ return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj.id, physicsClientId=self.client_id)) def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: - """ - Returns the axis aligned bounding box of the link. The return of this method are two points in - world coordinate frame which define a bounding box. - """ return AxisAlignedBoundingBox.from_min_max(*p.getAABB(link.get_object_id(), link.id, physicsClientId=self.client_id)) def set_realtime(self, real_time: bool) -> None: - """ - Enables the real time simulation of Physic in the BulletWorld. By default, this is disabled and Physics is only - simulated to reason about it. - - :param real_time: Whether the World should simulate Physics in real time. - """ p.setRealTimeSimulation(1 if real_time else 0, physicsClientId=self.client_id) def set_gravity(self, gravity_vector: List[float]) -> None: - """ - Sets the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). - Gravity is given as a vector in x,y,z. Gravity is only applied while simulating Physic. - - :param gravity_vector: The gravity vector that should be used in the World. - """ p.setGravity(gravity_vector[0], gravity_vector[1], gravity_vector[2], physicsClientId=self.client_id) - def exit(self, wait_time_before_exit_in_secs: Optional[float] = 0.1) -> None: - """ - Closes the BulletWorld as well as the shadow world, also collects any other thread that is running. This is the - preferred method to close the BulletWorld. - """ - super().exit(wait_time_before_exit_in_secs) - def disconnect_from_physics_server(self): - """ - Disconnects the world from the physics server. - """ p.disconnect(physicsClientId=self.client_id) def join_threads(self): """ - Join any running threads. Useful for example when exiting the world. + Joins the GUI thread if it exists. """ self.join_gui_thread_if_exists() @@ -337,22 +192,12 @@ def join_gui_thread_if_exists(self): self._gui_thread.join() def save_physics_simulator_state(self) -> int: - """ - Saves the state of the physics simulator and returns the unique id of the state. - """ return p.saveState(physicsClientId=self.client_id) def restore_physics_simulator_state(self, state_id): - """ - Restores the objects and environment state in the physics simulator according to - the given state using the unique state id. - """ p.restoreState(state_id, physicsClientId=self.client_id) def remove_physics_simulator_state(self, state_id: int): - """ - Removes the physics simulator state with the given unique state id. - """ p.removeState(state_id, physicsClientId=self.client_id) def add_vis_axis(self, pose: Pose, @@ -369,15 +214,14 @@ def add_vis_axis(self, pose: Pose, position, orientation = pose_in_map.to_list() - vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01], - rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01], - physicsClientId=self.client_id) - vis_y = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, length, 0.01], - rgbaColor=[0, 1, 0, 0.8], visualFramePosition=[0.01, length, 0.01], - physicsClientId=self.client_id) - vis_z = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.01, 0.01, length], - rgbaColor=[0, 0, 1, 0.8], visualFramePosition=[0.01, 0.01, length], - physicsClientId=self.client_id) + box_vis_shape = BoxVisualShape(Color(1, 0, 0, 0.8), [length, 0.01, 0.01], [length, 0.01, 0.01]) + vis_x = self.create_visual_shape(box_vis_shape) + + box_vis_shape = BoxVisualShape(Color(0, 1, 0, 0.8), [0.01, length, 0.01], [0.01, length, 0.01]) + vis_y = self.create_visual_shape(box_vis_shape) + + box_vis_shape = BoxVisualShape(Color(0, 0, 1, 0.8), [0.01, 0.01, length], [0.01, 0.01, length]) + vis_z = self.create_visual_shape(box_vis_shape) obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], basePosition=position, baseOrientation=orientation, @@ -411,41 +255,31 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L physicsClientId=self.client_id) def create_visual_shape(self, visual_shape: VisualShape) -> int: - return p.createVisualShape(visual_shape.visual_geometry_type, rgbaColor=visual_shape.rgba_color, + return p.createVisualShape(visual_shape.visual_geometry_type.value, rgbaColor=visual_shape.rgba_color, visualFramePosition=visual_shape.visual_frame_position, - physicsClientId=self.client_id, **asdict(visual_shape.shape_data)) + physicsClientId=self.client_id, **visual_shape.shape_data()) def create_multi_body(self, multi_body: MultiBody) -> int: return p.createMultiBody(baseVisualShapeIndex=-MultiBody.base_visual_shape_index, linkVisualShapeIndices=MultiBody.link_visual_shape_indices, - basePosition=MultiBody.base_position, baseOrientation=MultiBody.base_orientation, - linkPositions=MultiBody.link_positions, linkMasses=MultiBody.link_masses, - linkOrientations=MultiBody.link_orientations, - linkInertialFramePositions=MultiBody.link_inertial_frame_positions, - linkInertialFrameOrientations=MultiBody.link_inertial_frame_orientations, + basePosition=MultiBody.base_pose.position_as_list(), + baseOrientation=MultiBody.base_pose.orientation_as_list(), + linkPositions=[pose.position_as_list() for pose in MultiBody.link_poses], + linkMasses=MultiBody.link_masses, + linkOrientations=[pose.orientation_as_list() for pose in MultiBody.link_poses], + linkInertialFramePositions=[pose.position_as_list() + for pose in MultiBody.link_inertial_frame_poses], + linkInertialFrameOrientations=[pose.orientation_as_list() + for pose in MultiBody.link_inertial_frame_poses], linkParentIndices=MultiBody.link_parent_indices, linkJointTypes=MultiBody.link_joint_types, - linkJointAxis=MultiBody.link_joint_axis, + linkJointAxis=[[point.x, point.y, point.z] for point in MultiBody.link_joint_axis], linkCollisionShapeIndices=MultiBody.link_collision_shape_indices) def get_images_for_target(self, target_pose: Pose, cam_pose: Pose, size: Optional[int] = 256) -> List[np.ndarray]: - """ - Calculates the view and projection Matrix and returns 3 images: - - 1. An RGB image - 2. A depth image - 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object. - - From the given target_pose and cam_pose only the position is used. - - :param cam_pose: The pose of the camera - :param target_pose: The pose to which the camera should point to - :param size: The height and width of the images in pixel - :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. - """ # TODO: Might depend on robot cameras, if so please add these camera parameters to RobotDescription object # TODO: of your robot with a CameraDescription object. fov = 90 @@ -459,6 +293,40 @@ def get_images_for_target(self, return list(p.getCameraImage(size, size, view_matrix, projection_matrix, physicsClientId=self.client_id))[2:5] + def add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, + size: Optional[float] = None, color: Optional[Color] = Color(), life_time: Optional[float] = 0, + parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: + args = {} + if orientation: + args["textOrientation"] = orientation + if size: + args["textSize"] = size + if life_time: + args["lifeTime"] = life_time + if parent_object_id: + args["parentObjectUniqueId"] = parent_object_id + if parent_link_id: + args["parentLinkIndex"] = parent_link_id + return p.addUserDebugText(text, position, color.get_rgb(), physicsClientId=self.client_id, **args) + + def remove_text(self, text_id: Optional[int] = None) -> None: + if text_id: + p.removeUserDebugItem(text_id, physicsClientId=self.client_id) + else: + p.removeAllUserDebugItems(physicsClientId=self.client_id) + + def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: + p.enableJointForceTorqueSensor(obj.id, fts_joint_idx, enableSensor=1, physicsClientId=self.client_id) + + def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: + p.enableJointForceTorqueSensor(obj.id, joint_id, enableSensor=0, physicsClientId=self.client_id) + + def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[float]: + return p.getJointState(obj.id, joint_id, physicsClientId=self.client_id)[2] + + def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: + return p.getJointState(obj.id, joint_id, physicsClientId=self.client_id)[3] + class Gui(threading.Thread): """ @@ -519,18 +387,18 @@ def run(self): # Loop to update the camera position based on keyboard events while p.isConnected(self.world.client_id): # Monitor user input - keys = p.getKeyboardEvents(physicalClientId=self.world.client_id) - mouse = p.getMouseEvents(physicalClientId=self.world.client_id) + keys = p.getKeyboardEvents() + mouse = p.getMouseEvents() # Get infos about the camera - width, height, dist = (p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[0], - p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[1], - p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[10]) - cameraTargetPosition = p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[11] + width, height, dist = (p.getDebugVisualizerCamera()[0], + p.getDebugVisualizerCamera()[1], + p.getDebugVisualizerCamera()[10]) + cameraTargetPosition = p.getDebugVisualizerCamera()[11] # Get vectors used for movement on x,y,z Vector - xVec = [p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[2][i] for i in [0, 4, 8]] - yVec = [p.getDebugVisualizerCamera(physicalClientId=self.world.client_id)[2][i] for i in [2, 6, 10]] + xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] + yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] # Check the mouse state @@ -670,10 +538,8 @@ def run(self): dist += dist * 0.02 * speedMult p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, - cameraTargetPosition=cameraTargetPosition, - physicsClientId=self.world.client_id) + cameraTargetPosition=cameraTargetPosition) if visible == 0: cameraTargetPosition = (0.0, -50, 50) - p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1], - physicsClientId=self.world.client_id) + p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) time.sleep(1. / 80.) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index aac666802..db5a83a3d 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -10,7 +10,7 @@ from matplotlib import colors from nav_msgs.msg import OccupancyGrid, MapMetaData -from pycram.world import UseProspectionWorld, Object, Link +from .world import UseProspectionWorld, Object, Link from .local_transformer import LocalTransformer from .pose import Pose, Transform from .world import World diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 61ab85e59..4503e7a4d 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -49,7 +49,7 @@ def _apply_ik(robot: WorldObject, joint_poses: List[float], joints: List[str]) - # arm ="left" if gripper == robot_description.get_tool_frame("left") else "right" # ik_joints = [robot_description.torso_joint] + robot_description._safely_access_chains(arm).joints # ik_joints = robot_description._safely_access_chains(arm).joints - robot.set_positions_of_all_joints(dict(zip(joints, joint_poses))) + robot.set_joint_positions(dict(zip(joints, joint_poses))) # for i in range(0, len(joints)): # robot.set_joint_state(joints[i], joint_poses[i]) diff --git a/src/pycram/plan_failures.py b/src/pycram/plan_failures.py index 584a13e7b..dceb68013 100644 --- a/src/pycram/plan_failures.py +++ b/src/pycram/plan_failures.py @@ -385,3 +385,13 @@ class SustainedFailure(PlanFailure): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) + + +class ReasoningError(PlanFailure): + def __init__(*args, **kwargs): + super().__init__(*args, **kwargs) + + +class CollisionError(PlanFailure): + def __init__(*args, **kwargs): + super().__init__(*args, **kwargs) diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 08987ae90..0873883aa 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -100,7 +100,6 @@ def position(self, value) -> None: """ if (not type(value) == list and not type(value) == tuple and not type(value) == GeoPose and not type(value) == Point): - print(type(value)) rospy.logwarn("Position can only be a list or geometry_msgs/Pose") return if type(value) == list or type(value) == tuple and len(value) == 3: @@ -159,7 +158,8 @@ def to_transform(self, child_frame: str) -> Transform: :param child_frame: Child frame id to which the Transform points :return: A new Transform """ - return Transform(self.position_as_list(), self.orientation_as_list(), self.frame, child_frame, self.header.stamp) + return Transform(self.position_as_list(), self.orientation_as_list(), self.frame, child_frame, + self.header.stamp) def copy(self) -> Pose: """ @@ -167,11 +167,8 @@ def copy(self) -> Pose: :return: A copy of this pose """ - if isinstance(self.position, list): - print("Position is list") p = Pose(self.position_as_list(), self.orientation_as_list(), self.frame, self.header.stamp) p.header.frame_id = self.header.frame_id - # p.header.stamp = self.header.stamp return p def position_as_list(self) -> List[float]: diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 6f14c7a46..6e28cce94 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -188,7 +188,7 @@ def reachability_validator(pose: Pose, except IKError: pass finally: - robot.set_positions_of_all_joints(joint_state_before_ik) + robot.set_joint_positions(joint_state_before_ik) try: # resp = request_ik(base_link, end_effector, target_diff, robot, right_joints) @@ -206,6 +206,6 @@ def reachability_validator(pose: Pose, except IKError: pass finally: - robot.set_positions_of_all_joints(joint_state_before_ik) + robot.set_joint_positions(joint_state_before_ik) return res, arms diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index eafdd360c..e5930b694 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -67,13 +67,13 @@ def _execute(self, desig): pose_in_shoulder = local_transformer.transform_pose(target, robot.links["neck_shoulder_link"].tf_frame) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_positions_of_all_joints(robot_description.get_static_joint_chain("neck", "front")) + robot.set_joint_positions(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_positions_of_all_joints(robot_description.get_static_joint_chain("neck", "neck_right")) + robot.set_joint_positions(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_positions_of_all_joints(robot_description.get_static_joint_chain("neck", "back")) + robot.set_joint_positions(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_positions_of_all_joints(robot_description.get_static_joint_chain("neck", "neck_left")) + robot.set_joint_positions(robot_description.get_static_joint_chain("neck", "neck_left")) pose_in_shoulder = local_transformer.transform_pose(target, robot.links["neck_shoulder_link"].tf_frame) @@ -93,7 +93,7 @@ def _execute(self, desig): robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_positions_of_all_joints(robot_description.get_static_gripper_chain(gripper, motion)) + robot.set_joint_positions(robot_description.get_static_gripper_chain(gripper, motion)) class BoxyManager(ProcessModuleManager): diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 936c5c57e..4b9979cd5 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -78,13 +78,13 @@ def _execute(self, desig): pose_in_shoulder = local_transformer.transform_pose(target, robot.links["ur5_shoulder_link"].tf_frame) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_positions_of_all_joints(robot_description.get_static_joint_chain("left", "front")) + robot.set_joint_positions(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_positions_of_all_joints(robot_description.get_static_joint_chain("left", "arm_right")) + robot.set_joint_positions(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_positions_of_all_joints(robot_description.get_static_joint_chain("left", "back")) + robot.set_joint_positions(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_positions_of_all_joints(robot_description.get_static_joint_chain("left", "arm_left")) + robot.set_joint_positions(robot_description.get_static_joint_chain("left", "arm_left")) pose_in_shoulder = local_transformer.transform_pose(target, robot.links["ur5_shoulder_link"].tf_frame) @@ -103,7 +103,7 @@ def _execute(self, desig): robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_positions_of_all_joints(robot_description.get_static_gripper_chain(gripper, motion)) + robot.set_joint_positions(robot_description.get_static_gripper_chain(gripper, motion)) class DonbotMoveTCP(ProcessModule): @@ -127,7 +127,7 @@ class DonbotMoveJoints(ProcessModule): def _execute(self, desig: MoveArmJointsMotion.Motion): robot = World.robot if desig.left_arm_poses: - robot.set_positions_of_all_joints(desig.left_arm_poses) + robot.set_joint_positions(desig.left_arm_poses) class DonbotWorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 2467f522e..f0f4d7d1b 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -7,7 +7,7 @@ from ..pose import Pose, Point from ..helper import _apply_ik from ..external_interfaces.ik import request_ik -import pycram.world_reasoning as btr +from .. import world_reasoning as btr import logging import time diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index b8db116cc..e4d02d306 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -211,9 +211,9 @@ def _execute(self, desig: MoveArmJointsMotion.Motion): robot = World.robot if desig.right_arm_poses: - robot.set_positions_of_all_joints(desig.right_arm_poses) + robot.set_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_positions_of_all_joints(desig.left_arm_poses) + robot.set_joint_positions(desig.left_arm_poses) class PR2MoveJoints(ProcessModule): @@ -222,7 +222,7 @@ class PR2MoveJoints(ProcessModule): """ def _execute(self, desig: MoveJointsMotion.Motion): robot = World.robot - robot.set_positions_of_all_joints(dict(zip(desig.names, desig.positions))) + robot.set_joint_positions(dict(zip(desig.names, desig.positions))) class Pr2WorldStateDetecting(ProcessModule): diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index 7285f3107..add24a41c 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -1,9 +1,9 @@ -from pycram.external_interfaces.giskard import achieve_cartesian_goal -from pycram.designators.location_designator import CostmapLocation +from ...external_interfaces.giskard import achieve_cartesian_goal +from ...designators.location_designator import CostmapLocation from ...world import UseProspectionWorld, World -from pycram.pose import Pose -from pycram.robot_descriptions import robot_description -from pycram.pose_generator_and_validator import reachability_validator +from ...pose import Pose +from ...robot_descriptions import robot_description +from ...pose_generator_and_validator import reachability_validator from typing_extensions import Tuple, Dict import tf diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros/force_torque_sensor.py index ed68a1a86..b2d07b2c6 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros/force_torque_sensor.py @@ -50,7 +50,7 @@ def _publish(self) -> None: """ seq = 0 while not self.kill_event.is_set(): - joint_ft = self.world.get_joint_force_torque(self.world.robot, self.fts_joint_idx) + joint_ft = self.world.get_joint_reaction_force_torque(self.world.robot, self.fts_joint_idx) h = Header() h.seq = seq h.stamp = rospy.Time.now() diff --git a/src/pycram/world.py b/src/pycram/world.py index b520779b9..02032afe1 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -26,7 +26,6 @@ from .robot_descriptions import robot_description from .enums import JointType, ObjectType, WorldMode from .local_transformer import LocalTransformer -from sensor_msgs.msg import JointState from .pose import Pose, Transform @@ -222,7 +221,7 @@ def remove_object(self, obj: Object) -> None: # This means the current world of the object is not the prospection world, since it # has a reference to the prospection world if self.prospection_world is not None: - self.world_sync.remove_obj_queue.put(self) + self.world_sync.remove_obj_queue.put(obj) self.world_sync.remove_obj_queue.join() self.remove_object_from_simulator(obj) @@ -242,15 +241,12 @@ def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_par :return: The unique id of the created constraint. """ - constraint = Constraint(parent_obj_id=parent_link.get_object_id(), - parent_link_name=parent_link.name, - child_obj_id=child_link.get_object_id(), - child_link_name=child_link.name, + constraint = Constraint(parent_link=parent_link, + child_link=child_link, joint_type=JointType.FIXED, - joint_axis_in_child_link_frame=[0, 0, 0], - joint_frame_position_wrt_parent_origin=child_to_parent_transform.translation_as_list(), - joint_frame_position_wrt_child_origin=[0, 0, 0], - joint_frame_orientation_wrt_parent_origin=child_to_parent_transform.rotation_as_list(), + joint_axis_in_child_link_frame=Point(0, 0, 0), + joint_frame_pose_wrt_parent_origin=child_to_parent_transform.to_pose(), + joint_frame_pose_wrt_child_origin=Pose() ) constraint_id = self.add_constraint(constraint) return constraint_id @@ -273,7 +269,7 @@ def remove_constraint(self, constraint_id) -> None: """ pass - def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: + def get_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: """ Get the joint limits of an articulated object @@ -281,10 +277,10 @@ def get_object_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, :param joint_name: The name of the joint. :return: A tuple containing the upper and the lower limits of the joint respectively. """ - return self.get_object_joint_upper_limit(obj, joint_name), self.get_object_joint_lower_limit(obj, joint_name) + return self.get_joint_upper_limit(obj, joint_name), self.get_joint_lower_limit(obj, joint_name) @abstractmethod - def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: + def get_joint_upper_limit(self, obj: Object, joint_name: str) -> float: """ Get the joint upper limit of an articulated object @@ -295,7 +291,7 @@ def get_object_joint_upper_limit(self, obj: Object, joint_name: str) -> float: pass @abstractmethod - def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: + def get_joint_lower_limit(self, obj: Object, joint_name: str) -> float: """ Get the joint lower limit of an articulated object @@ -306,7 +302,7 @@ def get_object_joint_lower_limit(self, obj: Object, joint_name: str) -> float: pass @abstractmethod - def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: + def get_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: """ Returns the axis along/around which a joint is moving. The given joint_name has to be part of this object. @@ -317,7 +313,7 @@ def get_object_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: pass @abstractmethod - def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: + def get_joint_type(self, obj: Object, joint_name: str) -> JointType: """ Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. @@ -328,7 +324,7 @@ def get_object_joint_type(self, obj: Object, joint_name: str) -> JointType: pass @abstractmethod - def get_object_joint_position(self, obj: Object, joint_name: str) -> float: + def get_joint_position(self, obj: Object, joint_name: str) -> float: """ Get the position of a joint of an articulated object @@ -369,6 +365,22 @@ def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: if real_time: loop_time = rospy.Time.now() - curr_time time.sleep(max(0, self.simulation_time_step - loop_time.to_sec())) + self.update_all_objects_poses() + + def update_all_objects_poses(self) -> None: + """ + Updates the positions of all objects in the world. + """ + for obj in self.objects: + self.update_obj_pose(obj) + + @abstractmethod + def update_obj_pose(self, obj: Object) -> None: + """ + Updates the position of the given object in the world, by retrieving the position from the physics simulator. + :param obj: The object for which the position should be updated. + """ + pass @abstractmethod def perform_collision_detection(self) -> None: @@ -398,7 +410,7 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ pass - def get_joint_rest_pose(self, obj: Object, joint_name: str) -> float: + def get_joint_rest_position(self, obj: Object, joint_name: str) -> float: """ Get the rest pose of a joint of an articulated object @@ -638,13 +650,10 @@ def robot_is_set() -> bool: """ return World.robot is not None - def exit(self, wait_time_before_exit_in_secs: Optional[float] = None) -> None: + def exit(self) -> None: """ Closes the World as well as the prospection world, also collects any other thread that is running. - :param wait_time_before_exit_in_secs: The time to wait before exiting the world in seconds. """ - if wait_time_before_exit_in_secs is not None: - time.sleep(wait_time_before_exit_in_secs) self.exit_prospection_world_if_exists() self.disconnect_from_physics_server() self.reset_current_world() @@ -909,23 +918,20 @@ def create_multi_body_from_visual_shapes(self, visual_shape_ids: List[int], pose :param pose: The pose of the origin of the multi body relative to the world frame. :return: The unique id of the created multi body. """ - # Dummy paramater since these are needed to spawn visual shapes as a + # Dummy parameter since these are needed to spawn visual shapes as a # multibody. num_of_shapes = len(visual_shape_ids) - link_poses = [[0, 0, 0] for _ in range(num_of_shapes)] - link_orientations = [[0, 0, 0, 1] for _ in range(num_of_shapes)] + link_poses = [Pose() for _ in range(num_of_shapes)] link_masses = [1.0 for _ in range(num_of_shapes)] link_parent = [0 for _ in range(num_of_shapes)] link_joints = [JointType.FIXED for _ in range(num_of_shapes)] link_collision = [-1 for _ in range(num_of_shapes)] - link_joint_axis = [[1, 0, 0] for _ in range(num_of_shapes)] - - multi_body = MultiBody(base_visual_shape_index=-1, base_position=pose.position_as_list(), - base_orientation=pose.orientation_as_list(), - link_visual_shape_indices=visual_shape_ids, link_positions=link_poses, - link_orientations=link_orientations, link_masses=link_masses, - link_inertial_frame_positions=link_poses, - link_inertial_frame_orientations=link_orientations, + link_joint_axis = [Point(1, 0, 0) for _ in range(num_of_shapes)] + + multi_body = MultiBody(base_visual_shape_index=-1, base_pose=pose, + link_visual_shape_indices=visual_shape_ids, link_poses=link_poses, + link_masses=link_masses, + link_inertial_frame_poses=link_poses, link_parent_indices=link_parent, link_joint_types=link_joints, link_joint_axis=link_joint_axis, link_collision_shape_indices=link_collision) @@ -996,7 +1002,7 @@ def create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: def add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, size: float = 0.1, color: Optional[Color] = Color(), life_time: Optional[float] = 0, - parent_object_id: Optional[int] = None) -> None: + parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: """ Adds text to the world. :param text: The text to be added. @@ -1009,6 +1015,8 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ :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. """ raise NotImplementedError @@ -1022,10 +1030,10 @@ def remove_text(self, text_id: Optional[int] = None) -> None: def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: """ You can enable a joint force/torque sensor in each joint. Once enabled, if you perform - a simulation step, the get_joint_force_torque will report the joint reaction forces in the fixed degrees of - freedom: a fixed joint will measure all 6DOF joint forces/torques. A revolute/hinge joint - force/torque sensor will measure 5DOF reaction forces along all axis except the hinge axis. The - applied force by a joint motor is available through get_applied_joint_motor_torque. + a simulation step, the get_joint_reaction_force_torque will report the joint reaction forces in + the fixed degrees of freedom: a fixed joint will measure all 6DOF joint forces/torques. + A revolute/hinge joint force/torque sensor will measure 5DOF reaction forces along all axis except + the hinge axis. The applied force by a joint motor is available through get_applied_joint_motor_torque. :param obj: The object in which the joint is located. :param fts_joint_idx: The index of the joint for which the force torque sensor should be enabled. """ @@ -1039,7 +1047,7 @@ def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: """ raise NotImplementedError - def get_joint_force_torque(self, obj: Object, joint_id: int) -> List[float]: + def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[float]: """ Returns the joint reaction forces and torques of the specified joint. :param obj: The object in which the joint is located. @@ -1119,7 +1127,7 @@ def __init__(self, world: World, prospection_world: World): self.object_mapping: Dict[Object, Object] = {} self.equal_states = False - def run(self): + def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): """ Main method of the synchronization, this thread runs in a loop until the terminate flag is set. @@ -1127,6 +1135,8 @@ def run(self): every object in the World and updates the corresponding object in the prospection world. When there are entries in the adding or removing queue the corresponding objects will be added or removed in the same iteration. + :param wait_time_as_n_simulation_steps: The time in simulation steps to wait between each iteration of the + syncing loop. """ while not self.terminate: self.check_for_pause() @@ -1156,12 +1166,12 @@ def run(self): if len(world_obj.joint_name_to_id) > 2: for joint_name in world_obj.joint_name_to_id.keys(): if prospection_obj.get_joint_position(joint_name) != world_obj.get_joint_position(joint_name): - prospection_obj.set_positions_of_all_joints(world_obj.get_positions_of_all_joints()) + prospection_obj.set_joint_positions(world_obj.get_positions_of_all_joints()) break self.check_for_pause() # self.check_for_equal() - time.sleep(1 / 240) + time.sleep(wait_time_as_n_simulation_steps * self.world.simulation_time_step) self.add_obj_queue.join() self.remove_obj_queue.join() @@ -1498,7 +1508,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, # takes the state id as key and returns the attachments of the object at that state if not self.world.is_prospection_world: - self._add_to_world_sync_obj_queue(path) + self._add_to_world_sync_obj_queue(self.path) self._init_current_positions_of_joint() @@ -1664,7 +1674,7 @@ def _init_current_positions_of_joint(self) -> None: """ self._current_joints_positions = {} for joint_name in self.joint_name_to_id.keys(): - self._current_joints_positions[joint_name] = self.world.get_object_joint_position(self, joint_name) + self._current_joints_positions[joint_name] = self.world.get_joint_position(self, joint_name) @property def base_origin_shift(self) -> np.ndarray: @@ -2008,9 +2018,9 @@ def reset_all_joints_positions(self) -> None: """ joint_names = list(self.joint_name_to_id.keys()) joint_positions = [0] * len(joint_names) - self.set_positions_of_all_joints(dict(zip(joint_names, joint_positions))) + self.set_joint_positions(dict(zip(joint_names, joint_positions))) - def set_positions_of_all_joints(self, joint_poses: dict) -> None: + def set_joint_positions(self, joint_poses: dict) -> 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. @@ -2031,7 +2041,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: :param joint_position: The target pose for this joint """ # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly - up_lim, low_lim = self.world.get_object_joint_limits(self, joint_name) + up_lim, low_lim = self.world.get_joint_limits(self, joint_name) if low_lim > up_lim: low_lim, up_lim = up_lim, low_lim if not low_lim <= joint_position <= up_lim: @@ -2054,89 +2064,23 @@ def get_joint_position(self, joint_name: str) -> float: """ return self._current_joints_positions[joint_name] - def contact_points(self) -> List: - """ - Returns a list of contact points of this Object with other Objects. - - :return: A list of all contact points with other objects - """ - return self.world.get_object_contact_points(self) - - def contact_points_simulated(self) -> List: - """ - Returns a list of all contact points between this Object and other Objects after stepping the simulation once. - - :return: A list of contact points between this Object and other Objects - """ - state_id = self.world.save_state() - self.world.step() - contact_points = self.contact_points() - self.world.restore_state(state_id) - return contact_points - - def update_joints_from_topic(self, topic_name: str) -> None: - """ - Updates the joints of this object with positions obtained from a topic with the message type JointState. - Joint names on the topic have to correspond to the joints of this object otherwise an error message will be - logged. - - :param topic_name: Name of the topic with the joint states - """ - msg = rospy.wait_for_message(topic_name, JointState) - joint_names = msg.name - joint_positions = msg.position - if set(joint_names).issubset(self.joint_name_to_id.keys()): - for i in range(len(joint_names)): - self.set_joint_position(joint_names[i], joint_positions[i]) - else: - add_joints = set(joint_names) - set(self.joint_name_to_id.keys()) - rospy.logerr(f"There are joints in the published joint state which are not in this model: /n \ - The following joint{'s' if len(add_joints) != 1 else ''}: {add_joints}") - - def update_pose_from_tf(self, frame: str) -> None: - """ - Updates the pose of this object from a TF message. + def get_joint_rest_position(self, joint_name: str) -> float: + return self.world.get_joint_rest_position(self, joint_name) - :param frame: Name of the TF frame from which the position should be taken - """ - tf_listener = tf.TransformListener() - time.sleep(0.5) - position, orientation = tf_listener.lookupTransform(frame, "map", rospy.Time(0)) - position = [position[0][0] * -1, - position[0][1] * -1, - position[0][2]] - self.set_position(Pose(position, orientation)) + def get_joint_damping(self, joint_name: str) -> float: + return self.world.get_joint_damping(self, joint_name) - def set_color(self, color: Color) -> None: - """ - Changes the rgba_color of this object, the rgba_color has to be given as a list - of RGBA values. All links of this object will be colored. - - :param color: The rgba_color as RGBA values between 0 and 1 - """ - self.world.set_object_color(self, color) + def get_joint_upper_limit(self, joint_name: str) -> float: + return self.world.get_joint_upper_limit(self, joint_name) - def get_color(self) -> Union[Color, Dict[str, Color]]: - """ - :return: The rgba_color of this object or a dictionary of link names and their colors. - """ - return self.world.get_object_color(self) + def get_joint_lower_limit(self, joint_name: str) -> float: + return self.world.get_joint_lower_limit(self, joint_name) - def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: - """ - :return: The axis aligned bounding box of this object. - """ - return self.world.get_object_axis_aligned_bounding_box(self) + def get_joint_axis(self, joint_name: str) -> Tuple[float]: + return self.world.get_joint_axis(self, joint_name) - def get_base_origin(self) -> Pose: - """ - :return: the origin of the base/bottom of this object. - """ - aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() - base_width = np.absolute(aabb.min_x - aabb.max_x) - base_length = np.absolute(aabb.min_y - aabb.max_y) - return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], - self.get_pose().orientation_as_list()) + def get_joint_type(self, joint_name: str) -> JointType: + return self.world.get_joint_type(self, joint_name) def get_joint_limits(self, joint: str) -> Tuple[float, float]: """ @@ -2148,29 +2092,11 @@ def get_joint_limits(self, joint: str) -> Tuple[float, float]: """ if joint not in self.joint_name_to_id.keys(): raise KeyError(f"The given Joint: {joint} is not part of this object") - lower, upper = self.world.get_object_joint_limits(self, joint) + lower, upper = self.world.get_joint_limits(self, joint) if lower > upper: lower, upper = upper, lower return lower, upper - def get_joint_axis(self, joint_name: str) -> Tuple[float]: - """ - Returns the axis along which a joint is moving. The given joint_name has to be part of this object. - - :param joint_name: Name of the joint for which the axis should be returned. - :return: The axis a vector of xyz - """ - return self.world.get_object_joint_axis(self, joint_name) - - def get_joint_type(self, joint_name: str) -> JointType: - """ - Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - - :param joint_name: Joint name for which the type should be returned - :return: The type of the joint - """ - return self.world.get_object_joint_type(self, joint_name) - def find_joint_above(self, link_name: str, joint_type: JointType) -> str: """ Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. @@ -2205,6 +2131,57 @@ def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> for link in self.links.values(): link.update_transform(transform_time) + def contact_points(self) -> List: + """ + Returns a list of contact points of this Object with other Objects. + + :return: A list of all contact points with other objects + """ + return self.world.get_object_contact_points(self) + + def contact_points_simulated(self) -> List: + """ + Returns a list of all contact points between this Object and other Objects after stepping the simulation once. + + :return: A list of contact points between this Object and other Objects + """ + state_id = self.world.save_state() + self.world.step() + contact_points = self.contact_points() + self.world.restore_state(state_id) + return contact_points + + def set_color(self, color: Color) -> None: + """ + Changes the rgba_color of this object, the rgba_color has to be given as a list + of RGBA values. All links of this object will be colored. + + :param color: The rgba_color as RGBA values between 0 and 1 + """ + self.world.set_object_color(self, color) + + def get_color(self) -> Union[Color, Dict[str, Color]]: + """ + :return: The rgba_color of this object or a dictionary of link names and their colors. + """ + return self.world.get_object_color(self) + + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis aligned bounding box of this object. + """ + return self.world.get_object_axis_aligned_bounding_box(self) + + def get_base_origin(self) -> Pose: + """ + :return: the origin of the base/bottom of this object. + """ + aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() + base_width = np.absolute(aabb.min_x - aabb.max_x) + base_length = np.absolute(aabb.min_y - aabb.max_y) + return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], + self.get_pose().orientation_as_list()) + def filter_contact_points(contact_points, exclude_ids) -> List: """ @@ -2231,22 +2208,6 @@ def get_path_from_data_dir(file_name: str, data_directory: str) -> str: return data_directory + f"/{file_name}" -def _get_robot_name_from_urdf(urdf_string: str) -> str: - """ - Extracts the robot name from the 'robot_name' tag of a URDF. - - :param urdf_string: The URDF as string. - :return: The name of the robot described by the URDF. - """ - res = re.findall(r"robot\ *name\ *=\ *\"\ *[a-zA-Z_0-9]*\ *\"", urdf_string) - if len(res) == 1: - begin = res[0].find("\"") - end = res[0][begin + 1:].find("\"") - robot = res[0][begin + 1:begin + 1 + end].lower() - return robot - raise Exception("Robot Name not Found") - - class Attachment: def __init__(self, parent_link: Link, @@ -2266,7 +2227,7 @@ def __init__(self, self.parent_link: Link = parent_link self.child_link: Link = child_link self.bidirectional: bool = bidirectional - self._loose: bool = False and not bidirectional + self._loose: bool = False self.parent_to_child_transform: Transform = parent_to_child_transform if self.parent_to_child_transform is None: @@ -2322,7 +2283,7 @@ def get_inverse(self) -> Attachment: """ attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, constraint_id=self.constraint_id) - attachment.loose = False if self.loose else True + attachment.loose = not self._loose return attachment @property @@ -2457,15 +2418,3 @@ def _is_cached(path) -> bool: if full_path.exists(): return True return False - - -def _world_and_id(world: World) -> Tuple[World, int]: - """ - Selects the world to be used. If the given world is None the 'current_world' is used. - - :param world: The world which should be used or None if 'current_world' should be used - :return: The World object and the id of this World - """ - world = world if world is not None else World.current_world - client_id = world.client_id if world is not None else World.current_world.client_id - return world, client_id diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 40d4d8db2..1c0ee3ea0 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -1,6 +1,8 @@ from dataclasses import dataclass -from typing_extensions import List, Optional, Tuple, Callable, Dict +from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union from .enums import JointType, Shape +from .pose import Pose, Point +from abc import ABC, abstractmethod @dataclass @@ -55,38 +57,26 @@ def get_rgba(self) -> List[float]: """ return [self.R, self.G, self.B, self.A] + def get_rgb(self) -> List[float]: + """ + Returns the rgba_color as a list of RGB values. + + :return: The rgba_color as a list of RGB values + """ + return [self.R, self.G, self.B] + @dataclass class Constraint: """ Dataclass for storing a constraint between two objects. """ - parent_obj_id: int - parent_link_name: str - child_obj_id: int - child_link_name: str + parent_link: 'Link' + child_link: 'Link' joint_type: JointType - joint_axis_in_child_link_frame: List[int] - joint_frame_position_wrt_parent_origin: List[float] - joint_frame_position_wrt_child_origin: List[float] - joint_frame_orientation_wrt_parent_origin: Optional[List[float]] = None - joint_frame_orientation_wrt_child_origin: Optional[List[float]] = None - - -@dataclass -class Point: - x: float - y: float - z: float - - @classmethod - def from_list(cls, point: List[float]): - """ - Sets the point from a list of x, y, z values. - - :param point: The list of x, y, z values - """ - return cls(point[0], point[1], point[2]) + joint_axis_in_child_link_frame: Point + joint_frame_pose_wrt_parent_origin: Pose + joint_frame_pose_wrt_child_origin: Pose @dataclass @@ -169,61 +159,106 @@ class CollisionCallbacks: @dataclass class MultiBody: base_visual_shape_index: int - base_position: List[float] - base_orientation: List[float] + base_pose: Pose link_visual_shape_indices: List[int] - link_positions: List[List[float]] - link_orientations: List[List[float]] + link_poses: List[Pose] link_masses: List[float] - link_inertial_frame_positions: List[List[float]] - link_inertial_frame_orientations: List[List[float]] + link_inertial_frame_poses: List[Pose] link_parent_indices: List[int] link_joint_types: List[JointType] - link_joint_axis: List[List[float]] + link_joint_axis: List[Point] link_collision_shape_indices: List[int] @dataclass -class VisualShape: +class VisualShape(ABC): rgba_color: Color visual_frame_position: List[float] + @abstractmethod + def shape_data(self) -> Dict[str, Any]: + """ + Returns the shape data of the visual shape (e.g. half extents for a box, radius for a sphere). + """ + pass + + @property + @abstractmethod + def visual_geometry_type(self) -> Shape: + """ + Returns the visual geometry type of the visual shape (e.g. box, sphere). + """ + pass + @dataclass class BoxVisualShape(VisualShape): half_extents: List[float] - visual_geometry_type: Optional[Shape] = Shape.BOX + + def shape_data(self) -> Dict[str, List[float]]: + return {"halfExtents": self.half_extents} + + @property + def visual_geometry_type(self) -> Shape: + return Shape.BOX @dataclass class SphereVisualShape(VisualShape): radius: float - visual_geometry_type: Optional[Shape] = Shape.SPHERE + + def shape_data(self) -> Dict[str, float]: + return {"radius": self.radius} + + @property + def visual_geometry_type(self) -> Shape: + return Shape.SPHERE @dataclass class CapsuleVisualShape(VisualShape): radius: float length: float - visual_geometry_type: Optional[Shape] = Shape.CAPSULE + + def shape_data(self) -> Dict[str, float]: + return {"radius": self.radius, "length": self.length} + + @property + def visual_geometry_type(self) -> Shape: + return Shape.CAPSULE @dataclass class CylinderVisualShape(CapsuleVisualShape): - visual_geometry_type: Optional[Shape] = Shape.CYLINDER + + @property + def visual_geometry_type(self) -> Shape: + return Shape.CYLINDER @dataclass class MeshVisualShape(VisualShape): mesh_scale: List[float] mesh_file_name: str - visual_geometry_type: Optional[Shape] = Shape.MESH + + def shape_data(self) -> Dict[str, Union[List[float], str]]: + return {"meshScale": self.mesh_scale, "meshFileName": self.mesh_file_name} + + @property + def visual_geometry_type(self) -> Shape: + return Shape.MESH @dataclass class PlaneVisualShape(VisualShape): normal: List[float] - visual_geometry_type: Optional[Shape] = Shape.PLANE + + def shape_data(self) -> Dict[str, List[float]]: + return {"normal": self.normal} + + @property + def visual_geometry_type(self) -> Shape: + return Shape.PLANE @dataclass diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 01d9709a8..80cfa0ae9 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -10,16 +10,6 @@ from .world import World -class ReasoningError(Exception): - def __init__(*args, **kwargs): - Exception.__init__(*args, **kwargs) - - -class CollisionError(Exception): - def __init__(*args, **kwargs): - Exception.__init__(*args, **kwargs) - - def stable(obj: Object) -> bool: """ Checks if an object is stable in the world. Stable meaning that it's position will not change after simulating diff --git a/test/test_attachment.py b/test/test_attachment.py index d99cb05fc..dc1ba8869 100644 --- a/test/test_attachment.py +++ b/test/test_attachment.py @@ -1,7 +1,7 @@ -import test_bullet_world +from bullet_world_testcase import BulletWorldTestCase -class TestAttachment(test_bullet_world.BulletWorldTest): +class TestAttachment(BulletWorldTestCase): def test_attach(self): self.milk.attach(self.robot) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 91639d8ca..2387a234b 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -1,11 +1,13 @@ +import time import unittest import rospkg from bullet_world_testcase import BulletWorldTestCase from pycram.pose import Pose -from pycram.world import fix_missing_inertial +from pycram.world import fix_missing_inertial, Object, ObjectType import xml.etree.ElementTree as ET +from pycram.enums import JointType class BulletWorldTest(BulletWorldTestCase): @@ -34,6 +36,47 @@ def test_save_and_restore_state(self): self.assertTrue(self.milk in self.robot.attachments) self.assertTrue(cid == self.robot.attachments[self.milk].constraint_id) + def test_remove_object(self): + time.sleep(2) + milk_id = self.milk.id + self.assertTrue(milk_id in [obj.id for obj in self.world.objects]) + self.world.remove_object(self.milk) + self.assertTrue(milk_id not in [obj.id for obj in self.world.objects]) + BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + + def test_get_joint_rest_pose(self): + self.assertEqual(self.robot.get_joint_rest_position("head_pan_joint"), 0.0) + + def test_get_joint_damping(self): + self.assertEqual(self.robot.get_joint_damping("head_pan_joint"), 0.5) + + def test_get_joint_upper_limit(self): + self.assertEqual(self.robot.get_joint_upper_limit("head_pan_joint"), -3.007) + + def test_get_joint_lower_limit(self): + self.assertEqual(self.robot.get_joint_lower_limit("head_pan_joint"), 3.007) + + def test_get_joint_axis(self): + self.assertEqual(self.robot.get_joint_axis("head_pan_joint"), (0.0, 0.0, 1.0)) + + def test_get_joint_type(self): + self.assertEqual(self.robot.get_joint_type("head_pan_joint"), JointType.REVOLUTE) + + def test_get_joint_position(self): + self.assertEqual(self.robot.get_joint_position("head_pan_joint"), 0.0) + + def test_get_object_contact_points(self): + self.assertEqual(len(self.robot.contact_points()), 0) + self.milk.set_position(self.robot.get_position()) + self.assertTrue(len(self.robot.contact_points()) > 0) + + def test_step_simulation(self): + # TODO: kitchen explodes when stepping simulation, fix this + self.world.remove_object(self.kitchen) + self.milk.set_position(Pose([0, 0, 2])) + self.world.simulate(1) + self.assertTrue(self.milk.get_position().z < 2) + class XMLTester(unittest.TestCase): diff --git a/test/test_language.py b/test/test_language.py index 7470ed751..e4739802a 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -7,10 +7,10 @@ from pycram.pose import Pose from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Repeat, Code, RenderTree from pycram.process_module import simulated_robot -import test_bullet_world +from bullet_world_testcase import BulletWorldTestCase -class LanguageTestCase(test_bullet_world.BulletWorldTest): +class LanguageTestCase(BulletWorldTestCase): def test_inheritance(self): act = NavigateAction([Pose()]) diff --git a/test/test_links.py b/test/test_links.py index 0115c8c48..a2f7f1c4f 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -1,7 +1,7 @@ -import test_bullet_world +from bullet_world_testcase import BulletWorldTestCase -class TestLinks(test_bullet_world.BulletWorldTest): +class TestLinks(BulletWorldTestCase): def test_add_constraint(self): milk_link = self.milk.get_root_link() diff --git a/test/test_object.py b/test/test_object.py index ece06cc46..409b1db9e 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -1,9 +1,9 @@ -import test_bullet_world +from bullet_world_testcase import BulletWorldTestCase from pycram.pose import Pose from geometry_msgs.msg import Point -class TestObject(test_bullet_world.BulletWorldTest): +class TestObject(BulletWorldTestCase): def test_set_position_as_point(self): self.milk.set_position(Point(1, 2, 3)) From 99c675f7108b022946e162903a95faafda6519bd Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sat, 3 Feb 2024 08:59:16 +0100 Subject: [PATCH 45/69] [AbstractWorld] Added joint class to handle joint related things. Object, Link, and Joint all have cached poses. Cached poses update when World.simulate() is called. Or when the poses are set manually. --- src/pycram/bullet_world.py | 9 +- src/pycram/enums.py | 4 +- src/pycram/pose_generator_and_validator.py | 2 +- src/pycram/world.py | 443 +++++++++++++++------ src/pycram/world_dataclasses.py | 11 +- test/test_bullet_world.py | 7 +- 6 files changed, 346 insertions(+), 130 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 68878541f..0f58af09f 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -143,14 +143,15 @@ def get_number_of_joints(self, obj: Object) -> int: def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: p.resetJointState(obj.id, obj.joint_name_to_id[joint_name], joint_pose, physicsClientId=self.client_id) - def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): - p.resetBasePositionAndOrientation(obj.id, position, orientation, physicsClientId=self.client_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.client_id) def step(self): p.stepSimulation(physicsClientId=self.client_id) - def update_obj_pose(self, obj: Object) -> None: - obj.set_pose(Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.client_id))) + def get_object_pose(self, obj: Object) -> Pose: + return Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.client_id)) def set_link_color(self, link: Link, rgba_color: Color): p.changeVisualShape(link.get_object_id(), link.id, rgbaColor=rgba_color, physicsClientId=self.client_id) diff --git a/src/pycram/enums.py b/src/pycram/enums.py index 3b7280ad6..b1c1cc011 100644 --- a/src/pycram/enums.py +++ b/src/pycram/enums.py @@ -19,7 +19,6 @@ class TaskStatus(Enum): SUCCEEDED = 2 FAILED = 3 - class JointType(Enum): """ Enum for readable joint types. @@ -29,6 +28,9 @@ class JointType(Enum): SPHERICAL = 2 PLANAR = 3 FIXED = 4 + UNKNOWN = 5 + CONTINUOUS = 6 + FLOATING = 7 class Grasp(Enum): diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 6e28cce94..dfcc50176 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -171,7 +171,7 @@ def reachability_validator(pose: Pose, if robot in allowed_collision.keys(): allowed_robot_links = allowed_collision[robot] - joint_state_before_ik = robot._current_joints_positions + joint_state_before_ik = robot.get_positions_of_all_joints() try: # resp = request_ik(base_link, end_effector, target_diff, robot, left_joints) resp = request_ik(target, robot, left_joints, left_gripper) diff --git a/src/pycram/world.py b/src/pycram/world.py index 02032afe1..736c6a3eb 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -8,31 +8,28 @@ import threading import time import xml.etree.ElementTree +from abc import ABC, abstractmethod from queue import Queue -import tf -from tf.transformations import quaternion_from_euler -from typing_extensions import List, Optional, Dict, Tuple, Callable -from typing_extensions import Union import numpy as np import rospkg import rospy - import urdf_parser_py.urdf from geometry_msgs.msg import Quaternion, Point +from tf.transformations import quaternion_from_euler +from typing_extensions import List, Optional, Dict, Tuple, Callable +from typing_extensions import Union from urdf_parser_py.urdf import URDF, Collision, GeometricType -from .event import Event -from .robot_descriptions import robot_description from .enums import JointType, ObjectType, WorldMode +from .event import Event from .local_transformer import LocalTransformer - from .pose import Pose, Transform - -from abc import ABC, abstractmethod +from .robot_descriptions import robot_description from .world_dataclasses import (Color, Constraint, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, - CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, LinkState, ObjectState) + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, + LinkState, ObjectState, JointState) class World(ABC): @@ -156,7 +153,7 @@ def simulation_time_step(self): """ The time step of the simulation in seconds. """ - return 1/World.simulation_frequency + return 1 / World.simulation_frequency @abstractmethod def load_urdf_and_get_object_id(self, path: str, pose: Pose) -> int: @@ -279,7 +276,6 @@ def get_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: """ return self.get_joint_upper_limit(obj, joint_name), self.get_joint_lower_limit(obj, joint_name) - @abstractmethod def get_joint_upper_limit(self, obj: Object, joint_name: str) -> float: """ Get the joint upper limit of an articulated object @@ -288,9 +284,8 @@ def get_joint_upper_limit(self, obj: Object, joint_name: str) -> float: :param joint_name: The name of the joint. :return: The joint upper limit as a float. """ - pass + raise NotImplementedError - @abstractmethod def get_joint_lower_limit(self, obj: Object, joint_name: str) -> float: """ Get the joint lower limit of an articulated object @@ -299,9 +294,8 @@ def get_joint_lower_limit(self, obj: Object, joint_name: str) -> float: :param joint_name: The name of the joint. :return: The joint lower limit as a float. """ - pass + raise NotImplementedError - @abstractmethod def get_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: """ Returns the axis along/around which a joint is moving. The given joint_name has to be part of this object. @@ -310,9 +304,8 @@ def get_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: :param joint_name: Name of the joint for which the axis should be returned. :return: The axis which is a 3D vector of xyz values. """ - pass + raise NotImplementedError - @abstractmethod def get_joint_type(self, obj: Object, joint_name: str) -> JointType: """ Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. @@ -321,7 +314,7 @@ def get_joint_type(self, obj: Object, joint_name: str) -> JointType: :param joint_name: Joint name for which the type should be returned. :return: The type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. """ - pass + raise NotImplementedError @abstractmethod def get_joint_position(self, obj: Object, joint_name: str) -> float: @@ -372,13 +365,12 @@ def update_all_objects_poses(self) -> None: Updates the positions of all objects in the world. """ for obj in self.objects: - self.update_obj_pose(obj) + obj.update_pose() @abstractmethod - def update_obj_pose(self, obj: Object) -> None: + def get_object_pose(self, obj: Object) -> Pose: """ - Updates the position of the given object in the world, by retrieving the position from the physics simulator. - :param obj: The object for which the position should be updated. + Get the pose of an object in the world frame from the current object pose in the simulator. """ pass @@ -430,23 +422,21 @@ def get_joint_damping(self, obj: Object, joint_name: str) -> float: """ pass - @abstractmethod def get_joint_names(self, obj: Object) -> List[str]: """ Get the names of all joints of an articulated object. :param obj: The object. :return: A list of all joint names of the object. """ - pass + raise NotImplementedError - @abstractmethod def get_link_names(self, obj: Object) -> List[str]: """ Get the names of all links of an articulated object. :param obj: The object. :return: A list of all link names of the object. """ - pass + raise NotImplementedError def get_number_of_joints(self, obj: Object) -> int: """ @@ -454,7 +444,7 @@ def get_number_of_joints(self, obj: Object) -> int: :param obj: The object. :return: The number of joints of the object. """ - pass + raise NotImplementedError def get_number_of_links(self, obj: Object) -> int: """ @@ -462,7 +452,7 @@ def get_number_of_links(self, obj: Object) -> int: :param obj: The object. :return: The number of links of the object. """ - pass + raise NotImplementedError @abstractmethod def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: @@ -476,14 +466,13 @@ def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) pass @abstractmethod - def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]): + def reset_object_base_pose(self, obj: Object, pose: Pose): """ 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 position: The new position of the object as a vector of x,y,z - :param orientation: The new orientation of the object as a quaternion of x,y,z,w + :param obj: The object. + :param pose: The new pose as a Pose object. """ pass @@ -1194,10 +1183,192 @@ def check_for_equal(self) -> None: self.equal_states = eql +class Joint: + """ + Represents a joint of an Object in the World. + """ + urdf_joint_types_mapping = {'unknown': JointType.UNKNOWN, + 'revolute': JointType.REVOLUTE, + 'continuous': JointType.CONTINUOUS, + 'prismatic': JointType.PRISMATIC, + 'floating': JointType.FLOATING, + 'planar': JointType.PLANAR, + 'fixed': JointType.FIXED} + + def __init__(self, _id: int, + urdf_joint: urdf_parser_py.urdf.Joint, + obj: Object): + self.id: int = _id + self.urdf_joint: urdf_parser_py.urdf.Joint = urdf_joint + self.object: Object = obj + self.world: World = obj.world + self.saved_states: Dict[int, JointState] = {} + self._update_position() + + def _update_position(self) -> None: + """ + Updates the current position of the joint from the physics simulator. + """ + self._current_position = self.world.get_joint_position(self.object, self.name) + + @property + def parent_link(self) -> Link: + """ + Returns the parent link of this joint. + :return: The parent link as a Link object. + """ + return self.object.links[self.urdf_joint.parent.name] + + @property + def child_link(self) -> Link: + """ + Returns the child link of this joint. + :return: The child link as a Link object. + """ + return self.object.links[self.urdf_joint.child.name] + + @property + def has_limits(self) -> bool: + """ + Checks if this joint has limits. + :return: True if the joint has limits, False otherwise. + """ + return bool(self.urdf_joint.limit) + + @property + def limits(self) -> Tuple[float, float]: + """ + Returns the lower and upper limit of a joint, if the lower limit is higher + than the upper they are swapped to ensure the lower limit is always the smaller one. + :return: A tuple with the lower and upper joint limits. + """ + lower, upper = self.lower_limit, self.upper_limit + if lower > upper: + lower, upper = upper, lower + return lower, upper + + @property + def upper_limit(self) -> float: + """ + Returns the upper joint limit of this joint. + :return: The upper joint limit as a float. + """ + return self.urdf_joint.limit.upper + + @property + def lower_limit(self) -> float: + """ + Returns the lower joint limit of this joint. + :return: The lower joint limit as a float. + """ + return self.urdf_joint.limit.lower + + @property + def axis(self) -> Point: + """ + Returns the joint axis of this joint. + :return: The joint axis as a Point object. + """ + return Point(*self.urdf_joint.axis) + + @property + def type(self) -> JointType: + """ + Returns the joint type of this joint. + :return: The joint type as a JointType object. + """ + return self.urdf_joint_types_mapping[self.urdf_joint.type] + + @property + def position(self) -> float: + return self._current_position + + @property + def rest_position(self) -> float: + return self.world.get_joint_rest_position(self.object, self.name) + + @property + def damping(self) -> float: + """ + Returns the damping of this joint. + :return: The damping as a float. + """ + return self.urdf_joint.dynamics.damping + + @property + def name(self) -> str: + """ + Returns the name of this joint. + :return: The name of this joint as a string. + """ + return self.urdf_joint.name + + def reset_position(self, position: float) -> None: + self.world.reset_joint_position(self.object, self.name, position) + self._update_position() + + def get_object_id(self) -> int: + """ + Returns the id of the object to which this joint belongs. + :return: The integer id of the object to which this joint belongs. + """ + return self.object.id + + @position.setter + def position(self, joint_position: float) -> None: + """ + Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, as stated + in the URDF, an error will be printed. However, the joint will be set either way. + + :param joint_position: The target pose for this joint + """ + # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly + if self.has_limits: + low_lim, up_lim = self.limits + if not low_lim <= joint_position <= up_lim: + logging.error( + f"The joint position has to be within the limits of the joint. The joint limits for {self.name}" + f" are {low_lim} and {up_lim}") + logging.error(f"The given joint position was: {joint_position}") + # Temporarily disabled because kdl outputs values exciting joint limits + # return + self.reset_position(joint_position) + + def enable_force_torque_sensor(self) -> None: + self.world.enable_joint_force_torque_sensor(self.object, self.id) + + def disable_force_torque_sensor(self) -> None: + self.world.disable_joint_force_torque_sensor(self.object, self.id) + + def get_reaction_force_torque(self) -> List[float]: + return self.world.get_joint_reaction_force_torque(self.object, self.id) + + def get_applied_motor_torque(self) -> float: + return self.world.get_applied_joint_motor_torque(self.object, self.id) + + def save_state(self, state_id: int) -> None: + self.saved_states[state_id] = self.state + + def restore_state(self, state_id: int) -> None: + self.state = self.saved_states[state_id] + + def remove_saved_states(self) -> None: + self.saved_states = {} + + @property + def state(self) -> JointState: + return JointState(self.position) + + @state.setter + def state(self, joint_state: JointState) -> None: + self.position = joint_state.position + + class Link: """ Represents a link of an Object in the World. """ + def __init__(self, _id: int, urdf_link: urdf_parser_py.urdf.Link, @@ -1209,6 +1380,7 @@ def __init__(self, self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} self.saved_states: Dict[int, LinkState] = {} + self._update_pose() def save_state(self, state_id: int) -> None: """ @@ -1224,6 +1396,12 @@ def restore_state(self, state_id: int) -> None: """ self.constraint_ids = self.saved_states[state_id].constraint_ids + def remove_saved_states(self) -> None: + """ + Removes all saved states of this link. + """ + self.saved_states = {} + def get_current_state(self) -> LinkState: """ :return: The current state of this link as a LinkState object. @@ -1353,13 +1531,19 @@ def orientation_as_list(self) -> List[float]: """ return self.pose.orientation_as_list() + def _update_pose(self) -> None: + """ + Updates the current pose of this link from the world. + """ + self._current_pose = self.world.get_link_pose(self) + @property def pose(self) -> Pose: """ The pose of the link relative to the world frame. :return: A Pose object containing the pose of the link relative to the world frame. """ - return self.world.get_link_pose(self) + return self._current_pose @property def pose_as_list(self) -> List[List[float]]: @@ -1427,6 +1611,7 @@ class RootLink(Link): Represents the root link of an Object in the World. It differs from the normal Link class in that the pose ande the tf_frame is the same as that of the object. """ + def __init__(self, obj: Object): super().__init__(obj.get_root_link_id(), obj.get_root_urdf_link(), obj) @@ -1438,13 +1623,8 @@ def tf_frame(self) -> str: """ return self.object.tf_frame - @property - def pose(self) -> Pose: - """ - Returns the pose of the root link, which is the same as the pose of the object. - :return: A Pose object containing the pose of the root link. - """ - return self.object.get_pose() + def _update_pose(self) -> None: + self._current_pose = self.object.get_pose() class Object: @@ -1497,21 +1677,17 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.set_robot_if_not_set(self) self._init_joint_name_and_id_map() - self._init_link_name_and_id_map() - self._init_links() - self.update_link_transforms() + self._init_links_and_update_transforms() + self._init_joints() self.attachments: Dict[Object, Attachment] = {} self.saved_states: Dict[int, ObjectState] = {} - # takes the state id as key and returns the attachments of the object at that state if not self.world.is_prospection_world: self._add_to_world_sync_obj_queue(self.path) - self._init_current_positions_of_joint() - self.world.objects.append(self) def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, str]: @@ -1640,24 +1816,36 @@ def _init_link_name_and_id_map(self) -> None: link_names = self.world.get_link_names(self) n_links = len(link_names) self.link_name_to_id: Dict[str, int] = dict(zip(link_names, range(n_links))) - self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) self.link_name_to_id[self.urdf_object.get_root()] = -1 - self.link_id_to_name[-1] = self.urdf_object.get_root() + self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) - def _init_links(self) -> None: + def _init_links_and_update_transforms(self) -> None: """ Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the corresponding link objects. """ - links = {} + self.links = {} for urdf_link in self.urdf_object.links: link_name = urdf_link.name link_id = self.link_name_to_id[link_name] if link_name == self.urdf_object.get_root(): - links[link_name] = RootLink(self) + self.links[link_name] = RootLink(self) else: - links[link_name] = Link(link_id, urdf_link, self) - self.links = links + self.links[link_name] = Link(link_id, urdf_link, self) + + + self.update_link_transforms() + + def _init_joints(self): + """ + Initialize the joint objects from the URDF file and creates a dictionary which mas the joint names to the + corresponding joint objects + """ + self.joints = {} + for urdf_joint in self.urdf_object.joints: + joint_name = urdf_joint.name + joint_id = self.joint_name_to_id[joint_name] + self.joints[joint_name] = Joint(joint_id, urdf_joint, self) def _add_to_world_sync_obj_queue(self, path: str) -> None: """ @@ -1668,14 +1856,6 @@ def _add_to_world_sync_obj_queue(self, path: str) -> None: [self.name, self.obj_type, path, self.get_position_as_list(), self.get_orientation_as_list(), self.world.prospection_world, self.color, self]) - def _init_current_positions_of_joint(self) -> None: - """ - Initialize the cached joint position for each joint. - """ - self._current_joints_positions = {} - for joint_name in self.joint_name_to_id.keys(): - self._current_joints_positions[joint_name] = self.world.get_joint_position(self, joint_name) - @property def base_origin_shift(self) -> np.ndarray: """ @@ -1816,17 +1996,41 @@ def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Op :param set_attachments: Whether to set the poses of the attached objects to this object or not. """ pose_in_map = self.local_transformer.transform_pose(pose, "map") - - position, orientation = pose_in_map.to_list() if base: - position = np.array(position) + self.base_origin_shift + pose_in_map.position = np.array(pose_in_map.position) + self.base_origin_shift - self.world.reset_object_base_pose(self, position, orientation) - self._current_pose = pose_in_map + self.reset_base_pose(pose_in_map) if set_attachments: self._set_attached_objects_poses() + def reset_base_pose(self, pose: Pose): + self.world.reset_object_base_pose(self, pose) + self.update_pose() + + def update_pose(self): + """ + Updates the current pose of this object from the world. + """ + self._current_pose = self.world.get_object_pose(self) + self._update_all_joints_positions() + self._update_all_links_poses() + self.update_link_transforms() + + def _update_all_joints_positions(self): + """ + Updates the posisitons of all joints by getting them from the simulator. + """ + for joint in self.joints.values(): + joint._update_position() + + def _update_all_links_poses(self): + """ + Updates the poses of all links by getting them from the simulator. + """ + for link in self.links.values(): + link._update_pose() + def move_base_to_origin_pos(self) -> None: """ Move the object such that its base will be at the current origin position. @@ -1840,6 +2044,7 @@ def save_state(self, state_id) -> None: :param state_id: The unique id of the state. """ self.save_links_states(state_id) + self.save_joints_states(state_id) self.saved_states[state_id] = ObjectState(state_id, self.attachments.copy()) def save_links_states(self, state_id: int) -> None: @@ -1850,22 +2055,31 @@ def save_links_states(self, state_id: int) -> None: for link in self.links.values(): link.save_state(state_id) + def save_joints_states(self, state_id: int) -> None: + """ + Saves the state of all joints of this object. + :param state_id: The unique id of the state. + """ + for joint in self.joints.values(): + joint.save_state(state_id) + def restore_state(self, state_id: int) -> None: """ Restores the state of this object by restoring the state of all links and attachments. :param state_id: The unique id of the state. """ - self.restore_links_states(state_id) self.restore_attachments(state_id) + self.restore_links_states(state_id) + self.restore_joints_states(state_id) - def restore_attachments(self, state_id) -> None: + def restore_attachments(self, state_id: int) -> None: """ Restores the attachments of this object from a saved state using the given state id. :param state_id: The unique id of the state. """ self.attachments = self.saved_states[state_id].attachments - def restore_links_states(self, state_id) -> None: + def restore_links_states(self, state_id: int) -> None: """ Restores the states of all links of this object from a saved state using the given state id. :param state_id: The unique id of the state. @@ -1873,13 +2087,35 @@ def restore_links_states(self, state_id) -> None: for link in self.links.values(): link.restore_state(state_id) + def restore_joints_states(self, state_id: int) -> None: + """ + Restores the states of all joints of this object from a saved state using the given state id. + :param state_id: The unique id of the state. + """ + for joint in self.joints.values(): + joint.restore_state(state_id) + def remove_saved_states(self) -> None: """ Removes all saved states of this object. """ self.saved_states = {} + self.remove_links_saved_states() + self.remove_joints_saved_states() + + def remove_links_saved_states(self) -> None: + """ + Removes all saved states of the links of this object. + """ for link in self.links.values(): - link.saved_states = {} + link.remove_saved_states() + + def remove_joints_saved_states(self) -> None: + """ + Removes all saved states of the joints of this object. + """ + for joint in self.joints.values(): + joint.remove_saved_states() def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: """ @@ -2028,74 +2264,45 @@ def set_joint_positions(self, joint_poses: dict) -> None: :param joint_poses: """ for joint_name, joint_position in joint_poses.items(): - self.world.reset_joint_position(self, joint_name, joint_position) - self._current_joints_positions[joint_name] = joint_position + self.joints[joint_name].position = joint_position + self.update_pose() 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. If the pose is outside the joint limits, as stated - in the URDF, an error will be printed. However, the joint will be set either way. + 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 """ - # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly - up_lim, low_lim = self.world.get_joint_limits(self, joint_name) - if low_lim > up_lim: - low_lim, up_lim = up_lim, low_lim - if not low_lim <= joint_position <= up_lim: - logging.error( - f"The joint position has to be within the limits of the joint. The joint limits for {joint_name}" - f" are {low_lim} and {up_lim}") - logging.error(f"The given joint position was: {joint_position}") - # Temporarily disabled because kdl outputs values exciting joint limits - # return - self.world.reset_joint_position(self, joint_name, joint_position) - self._current_joints_positions[joint_name] = joint_position + 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: - """ - Returns the joint position for the given joint name. - - :param joint_name: The name of the joint - :return: The current pose of the joint - """ - return self._current_joints_positions[joint_name] + return self.joints[joint_name].position def get_joint_rest_position(self, joint_name: str) -> float: - return self.world.get_joint_rest_position(self, joint_name) + return self.joints[joint_name].rest_position def get_joint_damping(self, joint_name: str) -> float: - return self.world.get_joint_damping(self, joint_name) + return self.joints[joint_name].damping def get_joint_upper_limit(self, joint_name: str) -> float: - return self.world.get_joint_upper_limit(self, joint_name) + return self.joints[joint_name].upper_limit def get_joint_lower_limit(self, joint_name: str) -> float: - return self.world.get_joint_lower_limit(self, joint_name) + return self.joints[joint_name].lower_limit - def get_joint_axis(self, joint_name: str) -> Tuple[float]: - return self.world.get_joint_axis(self, joint_name) + def get_joint_axis(self, joint_name: str) -> Point: + return self.joints[joint_name].axis def get_joint_type(self, joint_name: str) -> JointType: - return self.world.get_joint_type(self, joint_name) - - def get_joint_limits(self, joint: str) -> Tuple[float, float]: - """ - Returns the lower and upper limit of a joint, if the lower limit is higher - than the upper they are swapped to ensure the lower limit is always the smaller one. + return self.joints[joint_name].type - :param joint: The name of the joint for which the limits should be found. - :return: The lower and upper limit of the joint. - """ - if joint not in self.joint_name_to_id.keys(): - raise KeyError(f"The given Joint: {joint} is not part of this object") - lower, upper = self.world.get_joint_limits(self, joint) - if lower > upper: - lower, upper = upper, lower - return lower, upper + def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: + return self.joints[joint_name].limits def find_joint_above(self, link_name: str, joint_type: JointType) -> str: """ @@ -2122,7 +2329,7 @@ def get_positions_of_all_joints(self) -> Dict[str: float]: :return: A dictionary with all joints positions'. """ - return self._current_joints_positions + return {j.name: j.position for j in self.joints.values()} def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: """ diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 1c0ee3ea0..fc96cd60e 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -261,12 +261,17 @@ def visual_geometry_type(self) -> Shape: return Shape.PLANE +@dataclass +class ObjectState: + state_id: int + attachments: Dict['Object', 'Attachment'] + + @dataclass class LinkState: constraint_ids: Dict['Link', int] @dataclass -class ObjectState: - state_id: int - attachments: Dict['Object', 'Attachment'] +class JointState: + position: float diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 2387a234b..15e46e637 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -2,6 +2,7 @@ import unittest import rospkg +from geometry_msgs.msg import Point from bullet_world_testcase import BulletWorldTestCase from pycram.pose import Pose @@ -51,13 +52,13 @@ def test_get_joint_damping(self): self.assertEqual(self.robot.get_joint_damping("head_pan_joint"), 0.5) def test_get_joint_upper_limit(self): - self.assertEqual(self.robot.get_joint_upper_limit("head_pan_joint"), -3.007) + self.assertEqual(self.robot.get_joint_upper_limit("head_pan_joint"), 3.007) def test_get_joint_lower_limit(self): - self.assertEqual(self.robot.get_joint_lower_limit("head_pan_joint"), 3.007) + self.assertEqual(self.robot.get_joint_lower_limit("head_pan_joint"), -3.007) def test_get_joint_axis(self): - self.assertEqual(self.robot.get_joint_axis("head_pan_joint"), (0.0, 0.0, 1.0)) + self.assertEqual(self.robot.get_joint_axis("head_pan_joint"), Point(0.0, 0.0, 1.0)) def test_get_joint_type(self): self.assertEqual(self.robot.get_joint_type("head_pan_joint"), JointType.REVOLUTE) From a8405b2b7fc6f99f3d6c7573533c22c5d3a43013 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Fri, 9 Feb 2024 15:53:06 +0100 Subject: [PATCH 46/69] [AbstractWorld] Abstracting URDF dependency to be any type of description. Cleaning. In progress, tests are not passing. --- src/pycram/bullet_world.py | 331 ++- src/pycram/enums.py | 3 + src/pycram/external_interfaces/giskard.py | 6 +- src/pycram/urdf_interface.py | 348 +++ src/pycram/world.py | 2461 +++++++++++---------- src/pycram/world_dataclasses.py | 74 +- test/test_bullet_world.py | 22 +- test/test_links.py | 12 +- test/test_object.py | 16 + 9 files changed, 1902 insertions(+), 1371 deletions(-) create mode 100644 src/pycram/urdf_interface.py diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 0f58af09f..a4317cce4 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -3,7 +3,7 @@ import threading import time -from typing_extensions import List, Optional, Dict, Tuple +from typing_extensions import List, Optional, Dict import numpy as np import pybullet as p @@ -12,9 +12,8 @@ from .enums import JointType, ObjectType, WorldMode from .pose import Pose -from .world import World, Object, Link -from .world_dataclasses import Color, Constraint, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape -from dataclasses import asdict +from .world import World, Object, Link, Constraint +from .world_dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape class BulletWorld(World): @@ -56,62 +55,40 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo if not is_prospection_world: plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def load_urdf_and_get_object_id(self, path: str, pose: Pose) -> int: + def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: return p.loadURDF(path, basePosition=pose.position_as_list(), - baseOrientation=pose.orientation_as_list(), physicsClientId=self.client_id) + baseOrientation=pose.orientation_as_list(), physicsClientId=self.id) def remove_object_from_simulator(self, obj: Object) -> None: - p.removeBody(obj.id, self.client_id) + p.removeBody(obj.id, self.id) def add_constraint(self, constraint: Constraint) -> int: - joint_axis_in_child_link_frame = [constraint.joint_axis_in_child_link_frame.x, - constraint.joint_axis_in_child_link_frame.y, - constraint.joint_axis_in_child_link_frame.z] - - constraint_id = p.createConstraint(constraint.parent_link.get_object_id(), - constraint.parent_link.id, - constraint.child_link.get_object_id(), - constraint.child_link.id, - constraint.joint_type.value, - joint_axis_in_child_link_frame, - constraint.joint_frame_pose_wrt_parent_origin.position_as_list(), - constraint.joint_frame_pose_wrt_child_origin.position_as_list(), - constraint.joint_frame_pose_wrt_parent_origin.orientation_as_list(), - constraint.joint_frame_pose_wrt_child_origin.orientation_as_list(), - physicsClientId=self.client_id) + + constraint_id = p.createConstraint(constraint.parent_object_id, + constraint.parent_link_id, + constraint.child_object_id, + constraint.child_link_id, + constraint.type.value, + constraint.axis_as_list, + constraint.position_wrt_parent_as_list(), + constraint.position_wrt_child_as_list(), + constraint.orientation_wrt_parent_as_list(), + constraint.orientation_wrt_child_as_list(), + physicsClientId=self.id) return constraint_id def remove_constraint(self, constraint_id): - p.removeConstraint(constraint_id, physicsClientId=self.client_id) - - def get_joint_rest_position(self, obj: Object, joint_name: str) -> float: - return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[0] - - def get_joint_damping(self, obj: Object, joint_name: str) -> float: - return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[6] - - def get_joint_upper_limit(self, obj: Object, joint_name: str) -> float: - return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[8] - - def get_joint_lower_limit(self, obj: Object, joint_name: str) -> float: - return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[9] - - def get_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: - return p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[13] - - def get_joint_type(self, obj: Object, joint_name: str) -> JointType: - joint_type = p.getJointInfo(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[2] - return JointType(joint_type) + p.removeConstraint(constraint_id, physicsClientId=self.id) def get_joint_position(self, obj: Object, joint_name: str) -> float: - return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.client_id)[0] + return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.id)[0] def get_link_pose(self, link: Link) -> Pose: - return Pose(*p.getLinkState(link.get_object_id(), link.id, physicsClientId=self.client_id)[4:6]) + return Pose(*p.getLinkState(link.object_id, link.id, physicsClientId=self.id)[4:6]) def perform_collision_detection(self) -> None: - p.performCollisionDetection(physicsClientId=self.client_id) + p.performCollisionDetection(physicsClientId=self.id) def get_object_contact_points(self, obj: Object) -> List: """ @@ -120,47 +97,33 @@ def get_object_contact_points(self, obj: Object) -> List: `PyBullet Doc `_ """ self.perform_collision_detection() - return p.getContactPoints(obj.id, physicsClientId=self.client_id) + return p.getContactPoints(obj.id, physicsClientId=self.id) def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: self.perform_collision_detection() - return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.client_id) - - def get_joint_names(self, obj: Object) -> List[str]: - num_joints = self.get_number_of_joints(obj) - return [p.getJointInfo(obj.id, i, physicsClientId=self.client_id)[1].decode('utf-8') for i in range(num_joints)] - - def get_link_names(self, obj: Object) -> List[str]: - num_links = self.get_number_of_links(obj) - return [p.getJointInfo(obj.id, i, physicsClientId=self.client_id)[12].decode('utf-8') for i in range(num_links)] - - def get_number_of_links(self, obj: Object) -> int: - return self.get_number_of_joints(obj) - - def get_number_of_joints(self, obj: Object) -> int: - return p.getNumJoints(obj.id, physicsClientId=self.client_id) + return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.id) def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: - p.resetJointState(obj.id, obj.joint_name_to_id[joint_name], joint_pose, physicsClientId=self.client_id) + p.resetJointState(obj.id, obj.joint_name_to_id[joint_name], joint_pose, 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.client_id) + physicsClientId=self.id) def step(self): - p.stepSimulation(physicsClientId=self.client_id) + p.stepSimulation(physicsClientId=self.id) def get_object_pose(self, obj: Object) -> Pose: - return Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.client_id)) + return Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.id)) def set_link_color(self, link: Link, rgba_color: Color): - p.changeVisualShape(link.get_object_id(), link.id, rgbaColor=rgba_color, physicsClientId=self.client_id) + p.changeVisualShape(link.object_id, link.id, rgbaColor=rgba_color, physicsClientId=self.id) def get_link_color(self, link: Link) -> Color: return self.get_colors_of_object_links(link.object)[link.name] def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: - visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.client_id) + visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.id) link_id_to_name = {v: k for k, v in obj.link_name_to_id.items()} links = list(map(lambda x: link_id_to_name[x[1]] if x[1] != -1 else "base", visual_data)) colors = list(map(lambda x: Color.from_rgba(x[7]), visual_data)) @@ -168,19 +131,19 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: return link_to_color def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: - return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj.id, physicsClientId=self.client_id)) + return AxisAlignedBoundingBox.from_min_max(*p.getAABB(obj.id, physicsClientId=self.id)) def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: - return AxisAlignedBoundingBox.from_min_max(*p.getAABB(link.get_object_id(), link.id, physicsClientId=self.client_id)) + return AxisAlignedBoundingBox.from_min_max(*p.getAABB(link.object_id, link.id, physicsClientId=self.id)) def set_realtime(self, real_time: bool) -> None: - p.setRealTimeSimulation(1 if real_time else 0, physicsClientId=self.client_id) + p.setRealTimeSimulation(1 if real_time else 0, physicsClientId=self.id) def set_gravity(self, gravity_vector: List[float]) -> None: - p.setGravity(gravity_vector[0], gravity_vector[1], gravity_vector[2], physicsClientId=self.client_id) + p.setGravity(gravity_vector[0], gravity_vector[1], gravity_vector[2], physicsClientId=self.id) def disconnect_from_physics_server(self): - p.disconnect(physicsClientId=self.client_id) + p.disconnect(physicsClientId=self.id) def join_threads(self): """ @@ -193,13 +156,13 @@ def join_gui_thread_if_exists(self): self._gui_thread.join() def save_physics_simulator_state(self) -> int: - return p.saveState(physicsClientId=self.client_id) + return p.saveState(physicsClientId=self.id) def restore_physics_simulator_state(self, state_id): - p.restoreState(state_id, physicsClientId=self.client_id) + p.restoreState(state_id, physicsClientId=self.id) def remove_physics_simulator_state(self, state_id: int): - p.removeState(state_id, physicsClientId=self.client_id) + p.removeState(state_id, physicsClientId=self.id) def add_vis_axis(self, pose: Pose, length: Optional[float] = 0.2) -> None: @@ -234,7 +197,7 @@ def add_vis_axis(self, pose: Pose, linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], linkCollisionShapeIndices=[-1, -1, -1], - physicsClientId=self.client_id) + physicsClientId=self.id) self.vis_axis.append(obj) @@ -243,22 +206,22 @@ def remove_vis_axis(self) -> None: Removes all spawned vis axis objects that are currently in this BulletWorld. """ for vis_id in self.vis_axis: - p.removeBody(vis_id, physicsClientId=self.client_id) + p.removeBody(vis_id, physicsClientId=self.id) self.vis_axis = [] def ray_test(self, from_position: List[float], to_position: List[float]) -> int: - res = p.rayTest(from_position, to_position, physicsClientId=self.client_id) + res = p.rayTest(from_position, to_position, physicsClientId=self.id) return res[0][0] def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], num_threads: int = 1) -> List[int]: return p.rayTestBatch(from_positions, to_positions, numThreads=num_threads, - physicsClientId=self.client_id) + physicsClientId=self.id) def create_visual_shape(self, visual_shape: VisualShape) -> int: return p.createVisualShape(visual_shape.visual_geometry_type.value, rgbaColor=visual_shape.rgba_color, visualFramePosition=visual_shape.visual_frame_position, - physicsClientId=self.client_id, **visual_shape.shape_data()) + physicsClientId=self.id, **visual_shape.shape_data()) def create_multi_body(self, multi_body: MultiBody) -> int: return p.createMultiBody(baseVisualShapeIndex=-MultiBody.base_visual_shape_index, @@ -289,10 +252,10 @@ def get_images_for_target(self, far = 100 view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1], - physicsClientId=self.client_id) - projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far, physicsClientId=self.client_id) + physicsClientId=self.id) + projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far, physicsClientId=self.id) return list(p.getCameraImage(size, size, view_matrix, projection_matrix, - physicsClientId=self.client_id))[2:5] + physicsClientId=self.id))[2:5] def add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, size: Optional[float] = None, color: Optional[Color] = Color(), life_time: Optional[float] = 0, @@ -308,25 +271,25 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ args["parentObjectUniqueId"] = parent_object_id if parent_link_id: args["parentLinkIndex"] = parent_link_id - return p.addUserDebugText(text, position, color.get_rgb(), physicsClientId=self.client_id, **args) + return p.addUserDebugText(text, position, color.get_rgb(), physicsClientId=self.id, **args) def remove_text(self, text_id: Optional[int] = None) -> None: if text_id: - p.removeUserDebugItem(text_id, physicsClientId=self.client_id) + p.removeUserDebugItem(text_id, physicsClientId=self.id) else: - p.removeAllUserDebugItems(physicsClientId=self.client_id) + p.removeAllUserDebugItems(physicsClientId=self.id) def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: - p.enableJointForceTorqueSensor(obj.id, fts_joint_idx, enableSensor=1, physicsClientId=self.client_id) + p.enableJointForceTorqueSensor(obj.id, fts_joint_idx, enableSensor=1, physicsClientId=self.id) def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: - p.enableJointForceTorqueSensor(obj.id, joint_id, enableSensor=0, physicsClientId=self.client_id) + p.enableJointForceTorqueSensor(obj.id, joint_id, enableSensor=0, physicsClientId=self.id) def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[float]: - return p.getJointState(obj.id, joint_id, physicsClientId=self.client_id)[2] + return p.getJointState(obj.id, joint_id, physicsClientId=self.id)[2] def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: - return p.getJointState(obj.id, joint_id, physicsClientId=self.client_id)[3] + return p.getJointState(obj.id, joint_id, physicsClientId=self.id)[3] class Gui(threading.Thread): @@ -348,45 +311,45 @@ def run(self): thus the thread terminates. The loop also checks for mouse and keyboard inputs to control the camera. """ if self.mode == WorldMode.DIRECT: - self.world.client_id = p.connect(p.DIRECT) + self.world.id = p.connect(p.DIRECT) else: - self.world.client_id = p.connect(p.GUI) + self.world.id = p.connect(p.GUI) # Disable the side windows of the GUI - p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0, physicsClientId=self.world.client_id) + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0, physicsClientId=self.world.id) # Change the init camera pose p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, - cameraTargetPosition=[-2, 0, 1], physicsClientId=self.world.client_id) + cameraTargetPosition=[-2, 0, 1], physicsClientId=self.world.id) # Get the initial camera target location - cameraTargetPosition = p.getDebugVisualizerCamera(physicsClientId=self.world.client_id)[11] + camera_target_position = p.getDebugVisualizerCamera(physicsClientId=self.world.id)[11] - sphereVisualId = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1], - physicsClientId=self.world.client_id) + sphere_visual_id = p.createVisualShape(p.GEOM_SPHERE, radius=0.05, rgbaColor=[1, 0, 0, 1], + physicsClientId=self.world.id) # Create a sphere with a radius of 0.05 and a mass of 0 - sphereUid = p.createMultiBody(baseMass=0.0, - baseInertialFramePosition=[0, 0, 0], - baseVisualShapeIndex=sphereVisualId, - basePosition=cameraTargetPosition, - physicsClientId=self.world.client_id) + sphere_uid = p.createMultiBody(baseMass=0.0, + baseInertialFramePosition=[0, 0, 0], + baseVisualShapeIndex=sphere_visual_id, + basePosition=camera_target_position, + physicsClientId=self.world.id) # Define the maxSpeed, used in calculations - maxSpeed = 16 + max_speed = 16 # Set initial Camera Rotation - cameraYaw = 50 - cameraPitch = -35 + camera_yaw = 50 + camera_pitch = -35 # Keep track of the mouse state - mouseState = [0, 0, 0] - oldMouseX, oldMouseY = 0, 0 + mouse_state = [0, 0, 0] + old_mouse_x, old_mouse_y = 0, 0 # Determines if the sphere at cameraTargetPosition is visible visible = 1 # Loop to update the camera position based on keyboard events - while p.isConnected(self.world.client_id): + while p.isConnected(self.world.id): # Monitor user input keys = p.getKeyboardEvents() mouse = p.getMouseEvents() @@ -395,30 +358,30 @@ def run(self): width, height, dist = (p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], p.getDebugVisualizerCamera()[10]) - cameraTargetPosition = p.getDebugVisualizerCamera()[11] + camera_target_position = p.getDebugVisualizerCamera()[11] # Get vectors used for movement on x,y,z Vector - xVec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] - yVec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] - zVec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] + x_vec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]] + y_vec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]] + z_vec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]] # Check the mouse state if mouse: for m in mouse: - mouseX = m[2] - mouseY = m[1] + mouse_x = m[2] + mouse_y = m[1] # update mouseState # Left Mouse button if m[0] == 2 and m[3] == 0: - mouseState[0] = m[4] - # Middle mouse butto (scroll wheel) + mouse_state[0] = m[4] + # Middle mouse button (scroll wheel) if m[0] == 2 and m[3] == 1: - mouseState[1] = m[4] + mouse_state[1] = m[4] # right mouse button if m[0] == 2 and m[3] == 2: - mouseState[2] = m[4] + mouse_state[2] = m[4] # change visibility by clicking the mousewheel if m[4] == 6 and m[3] == 1 and visible == 1: @@ -427,62 +390,64 @@ def run(self): visible = 1 # camera movement when the left mouse button is pressed - if mouseState[0] == 3: - speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < maxSpeed else maxSpeed - speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < maxSpeed else maxSpeed + if mouse_state[0] == 3: + speed_x = abs(old_mouse_x - mouse_x) if (abs(old_mouse_x - mouse_x)) < max_speed\ + else max_speed + speed_y = abs(old_mouse_y - mouse_y) if (abs(old_mouse_y - mouse_y)) < max_speed\ + else max_speed # max angle of 89.5 and -89.5 to make sure the camera does not flip (is annoying) - if mouseX < oldMouseX: - if (cameraPitch + speedX) < 89.5: - cameraPitch += (speedX / 4) + 1 - elif mouseX > oldMouseX: - if (cameraPitch - speedX) > -89.5: - cameraPitch -= (speedX / 4) + 1 - - if mouseY < oldMouseY: - cameraYaw += (speedY / 4) + 1 - elif mouseY > oldMouseY: - cameraYaw -= (speedY / 4) + 1 - - if mouseState[1] == 3: - speedX = abs(oldMouseX - mouseX) + if mouse_x < old_mouse_x: + if (camera_pitch + speed_x) < 89.5: + camera_pitch += (speed_x / 4) + 1 + elif mouse_x > old_mouse_x: + if (camera_pitch - speed_x) > -89.5: + camera_pitch -= (speed_x / 4) + 1 + + if mouse_y < old_mouse_y: + camera_yaw += (speed_y / 4) + 1 + elif mouse_y > old_mouse_y: + camera_yaw -= (speed_y / 4) + 1 + + if mouse_state[1] == 3: + speed_x = abs(old_mouse_x - mouse_x) factor = 0.05 - if mouseX < oldMouseX: - dist = dist - speedX * factor - elif mouseX > oldMouseX: - dist = dist + speedX * factor + if mouse_x < old_mouse_x: + dist = dist - speed_x * factor + elif mouse_x > old_mouse_x: + dist = dist + speed_x * factor dist = max(dist, 0.1) # camera movement when the right mouse button is pressed - if mouseState[2] == 3: - speedX = abs(oldMouseX - mouseX) if (abs(oldMouseX - mouseX)) < 5 else 5 - speedY = abs(oldMouseY - mouseY) if (abs(oldMouseY - mouseY)) < 5 else 5 + if mouse_state[2] == 3: + speed_x = abs(old_mouse_x - mouse_x) if (abs(old_mouse_x - mouse_x)) < 5 else 5 + speed_y = abs(old_mouse_y - mouse_y) if (abs(old_mouse_y - mouse_y)) < 5 else 5 factor = 0.05 - if mouseX < oldMouseX: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(zVec, factor), speedX)) - elif mouseX > oldMouseX: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(zVec, factor), speedX)) - - if mouseY < oldMouseY: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(xVec, factor), speedY)) - elif mouseY > oldMouseY: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(xVec, factor), speedY)) + if mouse_x < old_mouse_x: + camera_target_position = np.subtract(camera_target_position, + np.multiply(np.multiply(z_vec, factor), speed_x)) + elif mouse_x > old_mouse_x: + camera_target_position = np.add(camera_target_position, + np.multiply(np.multiply(z_vec, factor), speed_x)) + + if mouse_y < old_mouse_y: + camera_target_position = np.add(camera_target_position, + np.multiply(np.multiply(x_vec, factor), speed_y)) + elif mouse_y > old_mouse_y: + camera_target_position = np.subtract(camera_target_position, + np.multiply(np.multiply(x_vec, factor), speed_y)) # update oldMouse values - oldMouseY, oldMouseX = mouseY, mouseX + old_mouse_y, old_mouse_x = mouse_y, mouse_x # check the keyboard state if keys: # if shift is pressed, double the speed if p.B3G_SHIFT in keys: - speedMult = 5 + speed_mult = 5 else: - speedMult = 2.5 + speed_mult = 2.5 # if control is pressed, the movements caused by the arrowkeys, the '+' as well as the '-' key # change @@ -490,57 +455,57 @@ def run(self): # the up and down arrowkeys cause the targetPos to move along the z axis of the map if p.B3G_DOWN_ARROW in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(zVec, 0.03), speedMult)) + camera_target_position = np.subtract(camera_target_position, + np.multiply(np.multiply(z_vec, 0.03), speed_mult)) elif p.B3G_UP_ARROW in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(zVec, 0.03), speedMult)) + camera_target_position = np.add(camera_target_position, + np.multiply(np.multiply(z_vec, 0.03), speed_mult)) # left and right arrowkeys cause the targetPos to move horizontally relative to the camera if p.B3G_LEFT_ARROW in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(xVec, 0.03), speedMult)) + camera_target_position = np.subtract(camera_target_position, + np.multiply(np.multiply(x_vec, 0.03), speed_mult)) elif p.B3G_RIGHT_ARROW in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(xVec, 0.03), speedMult)) + camera_target_position = np.add(camera_target_position, + np.multiply(np.multiply(x_vec, 0.03), speed_mult)) # the '+' and '-' keys cause the targetpos to move forwards and backwards relative to the camera # while the camera stays at a constant distance. SHIFT + '=' is for US layout if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: - cameraTargetPosition = np.subtract(cameraTargetPosition, - np.multiply(np.multiply(yVec, 0.03), speedMult)) + camera_target_position = np.subtract(camera_target_position, + np.multiply(np.multiply(y_vec, 0.03), speed_mult)) elif ord("-") in keys: - cameraTargetPosition = np.add(cameraTargetPosition, - np.multiply(np.multiply(yVec, 0.03), speedMult)) + camera_target_position = np.add(camera_target_position, + np.multiply(np.multiply(y_vec, 0.03), speed_mult)) # standard bindings for thearrowkeys, the '+' as well as the '-' key else: # left and right arrowkeys cause the camera to rotate around the yaw axis if p.B3G_RIGHT_ARROW in keys: - cameraYaw += (360 / width) * speedMult + camera_yaw += (360 / width) * speed_mult elif p.B3G_LEFT_ARROW in keys: - cameraYaw -= (360 / width) * speedMult + camera_yaw -= (360 / width) * speed_mult # the up and down arrowkeys cause the camera to rotate around the pitch axis if p.B3G_DOWN_ARROW in keys: - if (cameraPitch + (360 / height) * speedMult) < 89.5: - cameraPitch += (360 / height) * speedMult + if (camera_pitch + (360 / height) * speed_mult) < 89.5: + camera_pitch += (360 / height) * speed_mult elif p.B3G_UP_ARROW in keys: - if (cameraPitch - (360 / height) * speedMult) > -89.5: - cameraPitch -= (360 / height) * speedMult + if (camera_pitch - (360 / height) * speed_mult) > -89.5: + camera_pitch -= (360 / height) * speed_mult # the '+' and '-' keys cause the camera to zoom towards and away from the targetPos without # moving it. SHIFT + '=' is for US layout since the events can't handle shift plus something if ord("+") in keys or p.B3G_SHIFT in keys and ord("=") in keys: - if (dist - (dist * 0.02) * speedMult) > 0.1: - dist -= dist * 0.02 * speedMult + if (dist - (dist * 0.02) * speed_mult) > 0.1: + dist -= dist * 0.02 * speed_mult elif ord("-") in keys: - dist += dist * 0.02 * speedMult + dist += dist * 0.02 * speed_mult - p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=cameraYaw, cameraPitch=cameraPitch, - cameraTargetPosition=cameraTargetPosition) + p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=camera_yaw, cameraPitch=camera_pitch, + cameraTargetPosition=camera_target_position) if visible == 0: - cameraTargetPosition = (0.0, -50, 50) - p.resetBasePositionAndOrientation(sphereUid, cameraTargetPosition, [0, 0, 0, 1]) + camera_target_position = (0.0, -50, 50) + p.resetBasePositionAndOrientation(sphere_uid, camera_target_position, [0, 0, 0, 1]) time.sleep(1. / 80.) diff --git a/src/pycram/enums.py b/src/pycram/enums.py index b1c1cc011..2f03e422b 100644 --- a/src/pycram/enums.py +++ b/src/pycram/enums.py @@ -32,6 +32,9 @@ class JointType(Enum): CONTINUOUS = 6 FLOATING = 7 + # override enum method to return an int + + class Grasp(Enum): """ diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 25e9bc833..fd4958f6c 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -159,7 +159,7 @@ def spawn_object(object: Object) -> None: :param object: World object that should be spawned """ if len(object.link_name_to_id) == 1: - geometry = object.urdf_object.link_map[object.urdf_object.get_root()].collision.geometry + geometry = object.object_description.link_map[object.object_description.get_root()].collision.geometry if isinstance(geometry, urdf_parser_py.urdf.Mesh): filename = geometry.filename spawn_mesh(object.name + "_" + str(object.id), filename, object.get_pose()) @@ -239,8 +239,8 @@ def _manage_par_motion_goals(goal_func, *args) -> Optional['MoveResult']: if "tip_link" in par_value_pair.keys() and "root_link" in par_value_pair.keys(): if par_value_pair["tip_link"] == robot_description.base_link: continue - chain = World.robot.urdf_object.get_chain(par_value_pair["root_link"], - par_value_pair["tip_link"]) + chain = World.robot.object_description.get_chain(par_value_pair["root_link"], + par_value_pair["tip_link"]) if set(chain).intersection(used_joints) != set(): giskard_wrapper.cmd_seq = tmp raise AttributeError(f"The joint(s) {set(chain).intersection(used_joints)} is used by multiple Designators") diff --git a/src/pycram/urdf_interface.py b/src/pycram/urdf_interface.py new file mode 100644 index 000000000..240e6b955 --- /dev/null +++ b/src/pycram/urdf_interface.py @@ -0,0 +1,348 @@ +import os +import pathlib +import re +from xml.etree import ElementTree + +import rospkg +import rospy +from geometry_msgs.msg import Point +from tf.transformations import quaternion_from_euler +from typing_extensions import Union, List +from urdf_parser_py import urdf +from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, + Sphere as URDF_Sphere, Mesh as URDF_Mesh) + +from pycram.enums import JointType, Shape +from pycram.pose import Pose +from pycram.world import JointDescription as AbstractJointDescription, Joint as AbstractJoint, \ + LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription, World, _is_cached + + +class LinkDescription(AbstractLinkDescription): + """ + A class that represents a link description of an object. + """ + urdf_shape_map = { + URDF_Box: Shape.BOX, + URDF_Cylinder: Shape.CYLINDER, + URDF_Sphere: Shape.SPHERE, + URDF_Mesh: Shape.MESH + } + + def __init__(self, urdf_description: urdf.Link): + super().__init__(urdf_description) + + @property + def geometry(self) -> Union[Shape, None]: + """ + Returns the geometry type of the URDF collision element of this link. + """ + return self.urdf_shape_map[type(self.collision.geometry)] + + @property + def origin(self) -> Pose: + return Pose(self.collision.origin.xyz, + quaternion_from_euler(*self.collision.origin.rpy)) + + @property + def name(self) -> str: + return self.parsed_description.name + + @property + def collision(self) -> Collision: + return self.parsed_description.collision + + +class JointDescription(AbstractJointDescription): + urdf_type_map = {'unknown': JointType.UNKNOWN, + 'revolute': JointType.REVOLUTE, + 'continuous': JointType.CONTINUOUS, + 'prismatic': JointType.PRISMATIC, + 'floating': JointType.FLOATING, + 'planar': JointType.PLANAR, + 'fixed': JointType.FIXED} + + def __init__(self, urdf_description: urdf.Joint): + super().__init__(urdf_description) + + @property + def origin(self) -> Pose: + return Pose(self.parsed_description.origin.xyz, + quaternion_from_euler(*self.parsed_description.origin.rpy)) + + @property + def name(self) -> str: + return self.parsed_description.name + + @property + def has_limits(self) -> bool: + return bool(self.parsed_description.limit) + + @property + def type(self) -> JointType: + """ + :return: The type of this joint. + """ + return self.urdf_type_map[self.parsed_description.type] + + @property + def axis(self) -> Point: + """ + :return: The axis of this joint, for example the rotation axis for a revolute joint. + """ + return Point(*self.parsed_description.axis) + + @property + def lower_limit(self) -> Union[float, None]: + """ + :return: The lower limit of this joint, or None if the joint has no limits. + """ + if self.has_limits: + return self.parsed_description.limit.lower + else: + return None + + @property + def upper_limit(self) -> Union[float, None]: + """ + :return: The upper limit of this joint, or None if the joint has no limits. + """ + if self.has_limits: + return self.parsed_description.limit.upper + else: + return None + + @property + def parent_link_name(self) -> str: + """ + :return: The name of the parent link of this joint. + """ + return self.parsed_description.parent + + @property + def child_link_name(self) -> str: + """ + :return: The name of the child link of this joint. + """ + return self.parsed_description.child + + @property + def damping(self) -> float: + """ + :return: The damping of this joint. + """ + return self.parsed_description.dynamics.damping + + @property + def friction(self) -> float: + """ + :return: The friction of this joint. + """ + return self.parsed_description.dynamics.friction + + +class Joint(AbstractJoint, JointDescription): + ... + + +class ObjectDescription(AbstractObjectDescription): + """ + A class that represents an object description of an object. + """ + def from_description_file(self, path) -> URDF: + """ + Create an object description from a description file. + + :param path: The path to the description file. + """ + with open(path, 'r') as file: + return URDF.from_xml_string(file.read()) + + def preprocess_file(self, path: str, ignore_cached_files: bool) -> str: + """ + Preprocesses the file, if it is a .obj or .stl file it will be converted to an URDF file. + """ + path_object = pathlib.Path(path) + extension = path_object.suffix + + path = _look_for_file_in_data_dir(path, path_object) + + _create_cache_dir_if_not_exists() + + # if file is not yet cached correct the urdf and save if in the cache directory + if not _is_cached(path) or ignore_cached_files: + if extension == ".obj" or extension == ".stl": + path = self._generate_urdf_file(path) + elif extension == ".urdf": + with open(path, mode="r") as f: + urdf_string = fix_missing_inertial(f.read()) + urdf_string = remove_error_tags(urdf_string) + urdf_string = fix_link_attributes(urdf_string) + try: + urdf_string = _correct_urdf_string(urdf_string) + except rospkg.ResourceNotFound as e: + rospy.logerr(f"Could not find resource package linked in this URDF") + raise e + path = World.cache_dir + path_object.name + with open(path, mode="w") as f: + f.write(urdf_string) + else: # Using the urdf from the parameter server + urdf_string = rospy.get_param(path) + path = World.cache_dir + self.name + ".urdf" + with open(path, mode="w") as f: + f.write(_correct_urdf_string(urdf_string)) + # save correct path in case the file is already in the cache directory + elif extension == ".obj" or extension == ".stl": + path = World.cache_dir + path_object.stem + ".urdf" + elif extension == ".urdf": + path = World.cache_dir + path_object.name + else: + path = World.cache_dir + self.name + ".urdf" + + return path + + def _generate_urdf_file(self, path) -> str: + """ + Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be + used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with + the name as filename. + + :return: The absolute path of the created file + """ + urdf_template = ' \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + ' + urdf_template = fix_missing_inertial(urdf_template) + rgb = " ".join(list(map(str, self.color.get_rgba()))) + pathlib_obj = pathlib.Path(path) + path = str(pathlib_obj.resolve()) + content = urdf_template.replace("~a", self.name).replace("~b", path).replace("~c", rgb) + with open(World.cache_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: + file.write(content) + return World.cache_dir + pathlib_obj.stem + ".urdf" + + @property + def links(self) -> List[LinkDescription]: + """ + :return: A list of links descriptions of this object. + """ + return [LinkDescription(link) for link in self.parsed_description.links] + + @property + def joints(self) -> List[JointDescription]: + """ + :return: A list of joints descriptions of this object. + """ + return [JointDescription(joint) for joint in self.parsed_description.joints] + + def get_root(self) -> str: + """ + :return: the name of the root link of this object. + """ + return self.parsed_description.get_root() + + def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: + """ + :return: the chain of links from 'start_link_name' to 'end_link_name'. + """ + return self.parsed_description.get_chain(start_link_name, end_link_name) + + +def _correct_urdf_string(urdf_string: str) -> str: + """ + Changes paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legac) + can't deal with ROS package paths. + + :param urdf_string: The name of the URDf on the parameter server + :return: The URDF string with paths in the filesystem instead of ROS packages + """ + r = rospkg.RosPack() + new_urdf_string = "" + for line in urdf_string.split('\n'): + if "package://" in line: + s = line.split('//') + s1 = s[1].split('/') + path = r.get_path(s1[0]) + line = line.replace("package://" + s1[0], path) + new_urdf_string += line + '\n' + + return fix_missing_inertial(new_urdf_string) + + +def fix_missing_inertial(urdf_string: str) -> str: + """ + Insert inertial tags for every URDF link that has no inertia. + This is used to prevent Legacy(PyBullet) from dumping warnings in the terminal + + :param urdf_string: The URDF description as string + :returns: The new, corrected URDF description as string. + """ + + inertia_tree = ElementTree.ElementTree(ElementTree.Element("inertial")) + inertia_tree.getroot().append(ElementTree.Element("mass", {"value": "0.1"})) + inertia_tree.getroot().append(ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) + inertia_tree.getroot().append(ElementTree.Element("inertia", {"ixx": "0.01", + "ixy": "0", + "ixz": "0", + "iyy": "0.01", + "iyz": "0", + "izz": "0.01"})) + + # create tree from string + tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + + for link_element in tree.iter("link"): + inertial = [*link_element.iter("inertial")] + if len(inertial) == 0: + link_element.append(inertia_tree.getroot()) + + return ElementTree.tostring(tree.getroot(), encoding='unicode') + + +def remove_error_tags(urdf_string: str) -> str: + """ + Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the + URDF_parser + + :param urdf_string: String of the URDF from which the tags should be removed + :return: The URDF string with the tags removed + """ + tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + removing_tags = ["gazebo", "transmission"] + for tag_name in removing_tags: + all_tags = tree.findall(tag_name) + for tag in all_tags: + tree.getroot().remove(tag) + + return ElementTree.tostring(tree.getroot(), encoding='unicode') + + +def fix_link_attributes(urdf_string: str) -> str: + """ + Removes the attribute 'type' from links since this is not parsable by the URDF parser. + + :param urdf_string: The string of the URDF from which the attributes should be removed + :return: The URDF string with the attributes removed + """ + tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + + for link in tree.iter("link"): + if "type" in link.attrib.keys(): + del link.attrib["type"] + + return ElementTree.tostring(tree.getroot(), encoding='unicode') diff --git a/src/pycram/world.py b/src/pycram/world.py index 736c6a3eb..08c80e300 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -7,32 +7,86 @@ import re import threading import time -import xml.etree.ElementTree from abc import ABC, abstractmethod from queue import Queue import numpy as np -import rospkg import rospy -import urdf_parser_py.urdf from geometry_msgs.msg import Quaternion, Point -from tf.transformations import quaternion_from_euler -from typing_extensions import List, Optional, Dict, Tuple, Callable +from typing_extensions import List, Optional, Dict, Tuple, Callable, Any from typing_extensions import Union -from urdf_parser_py.urdf import URDF, Collision, GeometricType -from .enums import JointType, ObjectType, WorldMode +from .enums import JointType, ObjectType, WorldMode, Shape from .event import Event from .local_transformer import LocalTransformer from .pose import Pose, Transform from .robot_descriptions import robot_description -from .world_dataclasses import (Color, Constraint, AxisAlignedBoundingBox, CollisionCallbacks, +from .world_dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - LinkState, ObjectState, JointState) + LinkState, ObjectState, JointState, State, WorldState) -class World(ABC): +class StateEntity: + """ + The StateEntity class is used to store the state of an object or the physics simulator. This is used to save and + restore the state of the World. + """ + + def __init__(self): + self._saved_states: Dict[int, State] = {} + + @property + def saved_states(self) -> Dict[int, State]: + """ + Returns the saved states of this entity. + """ + return self._saved_states + + def save_state(self, state_id: int) -> None: + """ + Saves the state of this entity with the given state id. + + :param state_id: The unique id of the state. + """ + self._saved_states[state_id] = self.current_state + + @property + @abstractmethod + def current_state(self) -> State: + """ + Returns the current state of this entity. + + :return: The current state of this entity. + """ + pass + + @current_state.setter + @abstractmethod + def current_state(self, state: State) -> None: + """ + Sets the current state of this entity. + + :param state: The new state of this entity. + """ + pass + + def restore_state(self, state_id: int) -> None: + """ + Restores the state of this entity from a saved state using the given state id. + + :param state_id: The unique id of the state. + """ + self.current_state = self.saved_states[state_id] + + def remove_saved_states(self) -> None: + """ + Removes all saved states of this entity. + """ + self._saved_states = {} + + +class World(StateEntity, ABC): """ The World Class represents the physics Simulation and belief state, it is the main interface for reasoning about the World. This is implemented as a singleton, the current World can be accessed via the static variable @@ -63,7 +117,7 @@ class World(ABC): Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. """ - cach_dir = data_directory[0] + '/cached/' + cache_dir = data_directory[0] + '/cached/' """ Global reference for the cache directory, this is used to cache the URDF files of the robot and the objects. """ @@ -78,6 +132,8 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. """ + StateEntity.__init__(self) + if World.current_world is None: World.current_world = self World.simulation_frequency = simulation_frequency @@ -91,7 +147,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.objects: List[Object] = [] # List of all Objects in the World - self.client_id: int = -1 + self.id: int = -1 # This is used to connect to the physics server (allows multiple clients) self.mode: WorldMode = mode @@ -101,8 +157,6 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._init_events() - self.saved_states: List[int] = [] - def _init_events(self): """ Initializes dynamic events that can be used to react to changes in the World. @@ -156,11 +210,11 @@ def simulation_time_step(self): return 1 / World.simulation_frequency @abstractmethod - def load_urdf_and_get_object_id(self, path: str, pose: Pose) -> int: + def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: """ - Loads a URDF file at the given pose and returns the id of the loaded object. + Loads a description file (e.g. URDF) at the given pose and returns the id of the loaded object. - :param path: The path to the URDF file. + :param path: The path to the description file. :param pose: The pose at which the object should be loaded. :return: The id of the loaded object. """ @@ -240,10 +294,10 @@ def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_par constraint = Constraint(parent_link=parent_link, child_link=child_link, - joint_type=JointType.FIXED, - joint_axis_in_child_link_frame=Point(0, 0, 0), - joint_frame_pose_wrt_parent_origin=child_to_parent_transform.to_pose(), - joint_frame_pose_wrt_child_origin=Pose() + _type=JointType.FIXED, + axis_in_child_frame=Point(0, 0, 0), + constraint_to_parent=child_to_parent_transform, + child_to_constraint=Transform(frame=child_link.tf_frame) ) constraint_id = self.add_constraint(constraint) return constraint_id @@ -266,56 +320,6 @@ def remove_constraint(self, constraint_id) -> None: """ pass - def get_joint_limits(self, obj: Object, joint_name: str) -> Tuple[float, float]: - """ - Get the joint limits of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: A tuple containing the upper and the lower limits of the joint respectively. - """ - return self.get_joint_upper_limit(obj, joint_name), self.get_joint_lower_limit(obj, joint_name) - - def get_joint_upper_limit(self, obj: Object, joint_name: str) -> float: - """ - Get the joint upper limit of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: The joint upper limit as a float. - """ - raise NotImplementedError - - def get_joint_lower_limit(self, obj: Object, joint_name: str) -> float: - """ - Get the joint lower limit of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: The joint lower limit as a float. - """ - raise NotImplementedError - - def get_joint_axis(self, obj: Object, joint_name: str) -> Tuple[float]: - """ - Returns the axis along/around which a joint is moving. The given joint_name has to be part of this object. - - :param obj: The object. - :param joint_name: Name of the joint for which the axis should be returned. - :return: The axis which is a 3D vector of xyz values. - """ - raise NotImplementedError - - def get_joint_type(self, obj: Object, joint_name: str) -> JointType: - """ - Returns the type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - - :param obj: The object. - :param joint_name: Joint name for which the type should be returned. - :return: The type of the joint as element of the Enum :mod:`~pycram.enums.JointType`. - """ - raise NotImplementedError - @abstractmethod def get_joint_position(self, obj: Object, joint_name: str) -> float: """ @@ -402,58 +406,6 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ pass - def get_joint_rest_position(self, obj: Object, joint_name: str) -> float: - """ - Get the rest pose of a joint of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: The rest pose of the joint. - """ - pass - - def get_joint_damping(self, obj: Object, joint_name: str) -> float: - """ - Get the damping of a joint of an articulated object - - :param obj: The object. - :param joint_name: The name of the joint. - :return: The damping of the joint. - """ - pass - - def get_joint_names(self, obj: Object) -> List[str]: - """ - Get the names of all joints of an articulated object. - :param obj: The object. - :return: A list of all joint names of the object. - """ - raise NotImplementedError - - def get_link_names(self, obj: Object) -> List[str]: - """ - Get the names of all links of an articulated object. - :param obj: The object. - :return: A list of all link names of the object. - """ - raise NotImplementedError - - def get_number_of_joints(self, obj: Object) -> int: - """ - Get the number of joints of an articulated object - :param obj: The object. - :return: The number of joints of the object. - """ - raise NotImplementedError - - def get_number_of_links(self, obj: Object) -> int: - """ - Get the number of links of an articulated object - :param obj: The object. - :return: The number of links of the object. - """ - raise NotImplementedError - @abstractmethod def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ @@ -483,22 +435,6 @@ def step(self): """ pass - def set_object_color(self, obj: Object, rgba_color: Color): - """ - Changes the color of this object, the color has to be given as a list - of RGBA values. - - :param obj: The object which should be colored - :param rgba_color: The color as Color object with RGBA values between 0 and 1 - """ - # Check if there is only one link, this is the case for primitive - # forms or if loaded from an .stl or .obj file - if obj.links != {}: - for link in obj.links.values(): - self.set_link_color(link, rgba_color) - else: - self.set_link_color(obj.get_root_link(), rgba_color) - @abstractmethod def set_link_color(self, link: Link, rgba_color: Color): """ @@ -518,27 +454,6 @@ def get_link_color(self, link: Link) -> Color: """ pass - def get_object_color(self, obj: Object) -> Union[Color, Dict[str, Color]]: - """ - This method returns the rgba_color of this object. The return is either: - - 1. A Color object with RGBA values, this is the case if the object only has one link (this - happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object. - Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which - the object is spawned. - - :param obj: The object for which the rgba_color should be returned. - :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and - the rgba_color as value. - """ - link_to_color_dict = self.get_colors_of_object_links(obj) - - if len(link_to_color_dict) == 1: - return list(link_to_color_dict.values())[0] - else: - return link_to_color_dict - @abstractmethod def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: """ @@ -691,17 +606,25 @@ def terminate_world_sync(self) -> None: self.world_sync.terminate = True self.world_sync.join() - def save_state(self) -> int: + def save_state(self, state_id: Optional[int] = None) -> int: """ Returns the id of the saved state of the World. The saved state contains the states of all the objects and the state of the physics simulator. :return: A unique id of the state """ + return self.current_state.state_id + + @property + def current_state(self) -> WorldState: state_id = self.save_physics_simulator_state() self.save_objects_state(state_id) self.saved_states.append(state_id) - return state_id + return WorldState(state_id) + + @current_state.setter + def current_state(self, state: WorldState) -> None: + self.restore_state(state.state_id) def save_objects_state(self, state_id: int) -> None: """ @@ -857,7 +780,7 @@ def remove_saved_states(self) -> None: """ for state_id in self.saved_states: self.remove_physics_simulator_state(state_id) - self.saved_states = [] + super().remove_saved_states() def update_transforms_for_objects_in_current_world(self) -> None: """ @@ -1183,1239 +1106,1510 @@ def check_for_equal(self) -> None: self.equal_states = eql -class Joint: +class WorldEntity(StateEntity, ABC): """ - Represents a joint of an Object in the World. + A data class that represents an entity of the world, such as an object or a link. """ - urdf_joint_types_mapping = {'unknown': JointType.UNKNOWN, - 'revolute': JointType.REVOLUTE, - 'continuous': JointType.CONTINUOUS, - 'prismatic': JointType.PRISMATIC, - 'floating': JointType.FLOATING, - 'planar': JointType.PLANAR, - 'fixed': JointType.FIXED} - def __init__(self, _id: int, - urdf_joint: urdf_parser_py.urdf.Joint, - obj: Object): - self.id: int = _id - self.urdf_joint: urdf_parser_py.urdf.Joint = urdf_joint - self.object: Object = obj - self.world: World = obj.world - self.saved_states: Dict[int, JointState] = {} - self._update_position() + def __init__(self, _id: int, world: Optional[World] = None): + StateEntity.__init__(self) + self.id = _id + self.world: World = world if world is not None else World.current_world - def _update_position(self) -> None: - """ - Updates the current position of the joint from the physics simulator. - """ - self._current_position = self.world.get_joint_position(self.object, self.name) - @property - def parent_link(self) -> Link: - """ - Returns the parent link of this joint. - :return: The parent link as a Link object. +class Object(WorldEntity): + """ + Represents a spawned Object in the World. + """ + object_description: ObjectDescription + prospection_world_prefix: str = "prospection/" + """ + The ObjectDescription of the object, this contains the name and type of the object as well as the path to the source + file. + """ + def __init__(self, name: str, obj_type: ObjectType, path: str, + pose: Optional[Pose] = None, + world: Optional[World] = None, + color: Optional[Color] = Color(), + ignore_cached_files: Optional[bool] = False): """ - return self.object.links[self.urdf_joint.parent.name] + The constructor loads the urdf file into the given World, if no World is specified the + :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. + The rgba_color parameter is only used when loading .stl or .obj files, + for URDFs :func:`~Object.set_color` can be used. - @property - def child_link(self) -> Link: - """ - Returns the child link of this joint. - :return: The child link as a Link object. + :param name: The name of the object + :param obj_type: The type of the object as an ObjectType enum. + :param path: The path to the source file, if only a filename is provided then the resourcer directories will be + searched + :param pose: The pose at which the Object should be spawned + :param world: The World in which the object should be spawned, + if no world is specified the :py:attr:`~World.current_world` will be used. + :param color: The rgba_color with which the object should be spawned. + :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ - return self.object.links[self.urdf_joint.child.name] - @property - def has_limits(self) -> bool: - """ - Checks if this joint has limits. - :return: True if the joint has limits, False otherwise. - """ - return bool(self.urdf_joint.limit) + if pose is None: + pose = Pose() - @property - def limits(self) -> Tuple[float, float]: - """ - Returns the lower and upper limit of a joint, if the lower limit is higher - than the upper they are swapped to ensure the lower limit is always the smaller one. - :return: A tuple with the lower and upper joint limits. - """ - lower, upper = self.lower_limit, self.upper_limit - if lower > upper: - lower, upper = upper, lower - return lower, upper + self.name: str = name + self.obj_type: ObjectType = obj_type + self.color: Color = color - @property - def upper_limit(self) -> float: - """ - Returns the upper joint limit of this joint. - :return: The upper joint limit as a float. - """ - return self.urdf_joint.limit.upper + self.local_transformer = LocalTransformer() + self.original_pose = self.local_transformer.transform_pose(pose, "map") + self._current_pose = self.original_pose - @property - def lower_limit(self) -> float: - """ - Returns the lower joint limit of this joint. - :return: The lower joint limit as a float. - """ - return self.urdf_joint.limit.lower + _id, self.path = self._load_object_and_get_id(path, ignore_cached_files) - @property - def axis(self) -> Point: - """ - Returns the joint axis of this joint. - :return: The joint axis as a Point object. - """ - return Point(*self.urdf_joint.axis) + super().__init__(_id, world) - @property - def type(self) -> JointType: - """ - Returns the joint type of this joint. - :return: The joint type as a JointType object. - """ - return self.urdf_joint_types_mapping[self.urdf_joint.type] + self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") + + f"{self.name}_{self.id}") - @property - def position(self) -> float: - return self._current_position + # self.object_description = ObjectDescription(self.path) - @property - def rest_position(self) -> float: - return self.world.get_joint_rest_position(self.object, self.name) + if self.object_description.name == robot_description.name: + self.world.set_robot_if_not_set(self) - @property - def damping(self) -> float: - """ - Returns the damping of this joint. - :return: The damping as a float. - """ - return self.urdf_joint.dynamics.damping + self._init_joint_name_and_id_map() + self._init_link_name_and_id_map() - @property - def name(self) -> str: - """ - Returns the name of this joint. - :return: The name of this joint as a string. - """ - return self.urdf_joint.name + self._init_links_and_update_transforms() + self._init_joints() - def reset_position(self, position: float) -> None: - self.world.reset_joint_position(self.object, self.name, position) - self._update_position() + self.attachments: Dict[Object, Attachment] = {} + self.saved_states: Dict[int, ObjectState] = {} - def get_object_id(self) -> int: - """ - Returns the id of the object to which this joint belongs. - :return: The integer id of the object to which this joint belongs. - """ - return self.object.id + if not self.world.is_prospection_world: + self._add_to_world_sync_obj_queue(self.path) - @position.setter - def position(self, joint_position: float) -> None: - """ - Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, as stated - in the URDF, an error will be printed. However, the joint will be set either way. + self.world.objects.append(self) - :param joint_position: The target pose for this joint + def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, str]: """ - # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly - if self.has_limits: - low_lim, up_lim = self.limits - if not low_lim <= joint_position <= up_lim: - logging.error( - f"The joint position has to be within the limits of the joint. The joint limits for {self.name}" - f" are {low_lim} and {up_lim}") - logging.error(f"The given joint position was: {joint_position}") - # Temporarily disabled because kdl outputs values exciting joint limits - # return - self.reset_position(joint_position) - - def enable_force_torque_sensor(self) -> None: - self.world.enable_joint_force_torque_sensor(self.object, self.id) - - def disable_force_torque_sensor(self) -> None: - self.world.disable_joint_force_torque_sensor(self.object, self.id) - - def get_reaction_force_torque(self) -> List[float]: - return self.world.get_joint_reaction_force_torque(self.object, self.id) - - def get_applied_motor_torque(self) -> float: - return self.world.get_applied_joint_motor_torque(self.object, self.id) + Loads an object to the given World with the given position and orientation. The rgba_color will only be + used when an .obj or .stl file is given. + If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created + and this URDf file will be loaded instead. + When spawning a URDf file a new file will be created in the cache directory, if there exists none. + This new file will have resolved mesh file paths, meaning there will be no references + to ROS packges instead there will be absolute file paths. - def save_state(self, state_id: int) -> None: - self.saved_states[state_id] = self.state + :param ignore_cached_files: Whether to ignore files in the cache directory. + :return: The unique id of the object and the path of the file that was loaded. + """ - def restore_state(self, state_id: int) -> None: - self.state = self.saved_states[state_id] + path = self.sync_cache_dir(path, ignore_cached_files) - def remove_saved_states(self) -> None: - self.saved_states = {} + try: + obj_id = self.world.load_description_and_get_object_id(path, Pose(self.get_position_as_list(), + self.get_orientation_as_list())) + return obj_id, path + except Exception as e: + logging.error( + "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" + " the name of an URDF string on the parameter server.") + os.remove(path) + raise (e) - @property - def state(self) -> JointState: - return JointState(self.position) + def sync_cache_dir(self, path: str, ignore_cached_files: bool) -> str: + """ + Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. + """ + path_object = pathlib.Path(path) + extension = path_object.suffix - @state.setter - def state(self, joint_state: JointState) -> None: - self.position = joint_state.position + path = _look_for_file_in_data_dir(path, path_object) + _create_cache_dir_if_not_exists() -class Link: - """ - Represents a link of an Object in the World. - """ + # if file is not yet cached preprocess the description file and save it in the cache directory. + if not _is_cached(path) or ignore_cached_files: + if extension == ".obj": + path = ObjectDescription.generate_from_obj_file(path) + elif extension == ".stl": + path = ObjectDescription.generate_from_stl_file(path) + elif extension == ObjectDescription.file_extension: + description_string = ObjectDescription.preprocess_file(path) + path = World.cache_dir + path_object.name + with open(path, mode="w") as f: + f.write(description_string) + else: # Using the description from the parameter server + description_string = ObjectDescription.preprocess_from_parameter_server(path) + path = f"{World.cache_dir}{self.name}{ObjectDescription.file_extension}" + with open(path, mode="w") as f: + f.write(description_string) + # save correct path in case the file is already in the cache directory + elif extension == ".obj" or extension == ".stl": + path = World.cache_dir + path_object.stem + ObjectDescription.file_extension + elif extension == ObjectDescription.file_extension: + path = World.cache_dir + path_object.name + else: + path = World.cache_dir + self.name + ObjectDescription.file_extension - def __init__(self, - _id: int, - urdf_link: urdf_parser_py.urdf.Link, - obj: Object): - self.id: int = _id - self.urdf_link: urdf_parser_py.urdf.Link = urdf_link - self.object: Object = obj - self.world: World = obj.world - self.local_transformer: LocalTransformer = LocalTransformer() - self.constraint_ids: Dict[Link, int] = {} - self.saved_states: Dict[int, LinkState] = {} - self._update_pose() + return path - def save_state(self, state_id: int) -> None: + def _init_joint_name_and_id_map(self) -> None: """ - Saves the state of this link. - :param state_id: The unique id of the state. + Creates a dictionary which maps the joint names to their unique ids and vice versa. """ - self.saved_states[state_id] = self.get_current_state() + n_joints = len(self.joint_names) + self.joint_name_to_id = dict(zip(self.joint_names, range(n_joints))) + self.joint_id_to_name = dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys())) - def restore_state(self, state_id: int) -> None: + def _init_link_name_and_id_map(self) -> None: """ - Restores the state of this link. - :param state_id: The unique id of the state. + Creates a dictionary which maps the link names to their unique ids and vice versa. """ - self.constraint_ids = self.saved_states[state_id].constraint_ids + n_links = len(self.link_names) + self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) + self.link_name_to_id[self.object_description.get_root()] = -1 + self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) - def remove_saved_states(self) -> None: + def _init_links_and_update_transforms(self) -> None: """ - Removes all saved states of this link. + Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the + corresponding link objects. """ - self.saved_states = {} + self.links = {} + for link_description in self.object_description.links: + link_name = link_description.name + link_id = self.link_name_to_id[link_name] + if link_name == self.object_description.get_root(): + self.links[link_name] = RootLink(self) + else: + self.links[link_name] = Link(link_id, link_description, self) - def get_current_state(self) -> LinkState: - """ - :return: The current state of this link as a LinkState object. - """ - return LinkState(self.constraint_ids.copy()) + self.update_link_transforms() - def add_fixed_constraint_with_link(self, child_link: Link) -> int: + def _init_joints(self): """ - 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. - :return: The unique id of the constraint. + Initialize the joint objects from the URDF file and creates a dictionary which mas the joint names to the + corresponding joint objects """ - constraint_id = self.world.add_fixed_constraint(self, - child_link, - child_link.get_transform_from_link(self)) - self.constraint_ids[child_link] = constraint_id - child_link.constraint_ids[self] = constraint_id - return constraint_id + self.joints = {} + for joint_description in self.object_description.joints: + joint_name = joint_description.name + joint_id = self.joint_name_to_id[joint_name] + self.joints[joint_name] = Joint(joint_id, joint_description, self) - def remove_constraint_with_link(self, child_link: Link) -> None: + def _add_to_world_sync_obj_queue(self, path: str) -> None: """ - Removes the constraint between this link and the given link. - :param child_link: The child link of the constraint that should be removed. + Adds this object to the objects queue of the WorldSync object of the World. + :param path: The path of the URDF file of this object. """ - self.world.remove_constraint(self.constraint_ids[child_link]) - del self.constraint_ids[child_link] - del child_link.constraint_ids[self] + self.world.world_sync.add_obj_queue.put( + [self.name, self.obj_type, path, self.get_position_as_list(), self.get_orientation_as_list(), + self.world.prospection_world, self.color, self]) - def get_object_id(self) -> int: + @property + def link_names(self) -> List[str]: """ - Returns the id of the object to which this link belongs. - :return: The integer id of the object to which this link belongs. + :return: The name of each link as a list. """ - return self.object.id + return [link.name for link in self.object_description.links] @property - def tf_frame(self) -> str: + def joint_names(self) -> List[str]: """ - Returns the tf frame of this link. - :return: The tf frame of this link as a string. + :return: The name of each joint as a list. """ - return f"{self.object.tf_frame}/{self.urdf_link.name}" + return [joint.name for joint in self.object_description.joints] @property - def is_root(self) -> bool: + def base_origin_shift(self) -> np.ndarray: """ - Returns whether this link is the root link of the object. - :return: True if this link is the root link, False otherwise. + The shift between the base of the object and the origin of the object. + :return: A numpy array with the shift between the base of the object and the origin of the object. """ - return self.object.get_root_link_id() == self.id + return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) - def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: + def __repr__(self): + skip_attr = ["links", "joints", "object_description", "attachments"] + return self.__class__.__qualname__ + f"(" + ', \n'.join( + [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" + + def remove(self) -> None: """ - Updates the transformation of this link at the given time. - :param transform_time: The time at which the transformation should be updated. + Removes this object from the World it currently resides in. + For the object to be removed it has to be detached from all objects it + is currently attached to. After this is done a call to world remove object is done + to remove this Object from the simulation/world. """ - self.local_transformer.update_transforms([self.transform], transform_time) + self.world.remove_object(self) - @property - def transform(self) -> Transform: + def reset(self, remove_saved_states=True) -> None: """ - The transformation from the world frame to this link frame. - :return: A Transform object with the transformation from the world frame to this link frame. + Resets the Object to the state it was first spawned in. + All attached objects will be detached, all joints will be set to the + default position of 0 and the object will be set to the position and + orientation in which it was spawned. + :param remove_saved_states: If True the saved states will be removed. """ - return self.pose.to_transform(self.tf_frame) + self.detach_all() + self.reset_all_joints_positions() + self.set_pose(self.original_pose) + if remove_saved_states: + self.saved_states = {} - def get_transform_to_link(self, link: Link) -> Transform: + def attach(self, + child_object: Object, + parent_link: Optional[str] = None, + child_link: Optional[str] = None, + bidirectional: Optional[bool] = True) -> None: """ - Returns the transformation from this link to the given link. - :param link: The link to which the transformation should be returned. - :return: A Transform object with the transformation from this link to the given link. + Attaches another object to this object. This is done by + saving the transformation between the given link, if there is one, and + the base pose of the other object. Additionally, the name of the link, to + which the object is attached, will be saved. + Furthermore, a simulator constraint will be created so the attachment + also works while simulation. + Loose attachments means that the attachment will only be one-directional. For example, if this object moves the + other, attached, object will also move but not the other way around. + + :param child_object: The other object that should be attached. + :param parent_link: The link name of this object. + :param child_link: The link name of the other object. + :param bidirectional: If the attachment should be a loose attachment. """ - return link.get_transform_from_link(self) + parent_link = self.links[parent_link] if parent_link else self.root_link + child_link = child_object.links[child_link] if child_link else child_object.root_link - def get_transform_from_link(self, link: Link) -> Transform: + attachment = Attachment(parent_link, child_link, bidirectional) + + self.attachments[child_object] = attachment + child_object.attachments[self] = attachment.get_inverse() + + self.world.attachment_event(self, [self, child_object]) + + def detach(self, child_object: Object) -> None: """ - Returns the transformation from the given link to this link. - :param link: The link from which the transformation should be returned. - :return: A Transform object with the transformation from the given link to this link. + Detaches another object from this object. This is done by + deleting the attachment from the attachments dictionary of both objects + and deleting the constraint of the simulator. + Afterward the detachment event of the corresponding World will be fired. + + :param child_object: The object which should be detached """ - return self.get_pose_wrt_link(link).to_transform(self.tf_frame) + del self.attachments[child_object] + del child_object.attachments[self] - def get_pose_wrt_link(self, link: Link) -> Pose: + self.world.detachment_event(self, [self, child_object]) + + def detach_all(self) -> None: """ - Returns the pose of this link with respect to the given link. - :param link: The link with respect to which the pose should be returned. - :return: A Pose object with the pose of this link with respect to the given link. + Detach all objects attached to this object. """ - return self.local_transformer.transform_pose(self.pose, link.tf_frame) + attachments = self.attachments.copy() + for att in attachments.keys(): + self.detach(att) - def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + def update_attachment_with_object(self, child_object: Object): + self.attachments[child_object].update_transform_and_constraint() + + def get_position(self) -> Point: """ - Returns the axis aligned bounding box of this link. - :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. + Returns the position of this Object as a list of xyz. + + :return: The current position of this object """ - return self.world.get_link_axis_aligned_bounding_box(self) + return self.get_pose().position - @property - def position(self) -> Point: + def get_orientation(self) -> Pose.orientation: """ - The getter for the position of the link relative to the world frame. - :return: A Point object containing the position of the link relative to the world frame. + Returns the orientation of this object as a list of xyzw, representing a quaternion. + + :return: A list of xyzw """ - return self.pose.position + return self.get_pose().orientation - @property - def position_as_list(self) -> List[float]: + def get_position_as_list(self) -> List[float]: """ - The getter for the position of the link relative to the world frame as a list. - :return: A list containing the position of the link relative to the world frame. + Returns the position of this Object as a list of xyz. + + :return: The current position of this object """ - return self.pose.position_as_list() + return self.get_pose().position_as_list() - @property - def orientation(self) -> Quaternion: + def get_orientation_as_list(self) -> List[float]: """ - The getter for the orientation of the link relative to the world frame. - :return: A Quaternion object containing the orientation of the link relative to the world frame. + Returns the orientation of this object as a list of xyzw, representing a quaternion. + + :return: A list of xyzw """ - return self.pose.orientation + return self.get_pose().orientation_as_list() - @property - def orientation_as_list(self) -> List[float]: + def get_pose(self) -> Pose: """ - The getter for the orientation of the link relative to the world frame as a list. - :return: A list containing the orientation of the link relative to the world frame. + Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. + + :return: The current pose of this object """ - return self.pose.orientation_as_list() + return self._current_pose - def _update_pose(self) -> None: + def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Optional[bool] = True) -> None: """ - Updates the current pose of this link from the world. + Sets the Pose of the object. + + :param pose: New Pose for the object + :param base: If True places the object base instead of origin at the specified position and orientation + :param set_attachments: Whether to set the poses of the attached objects to this object or not. """ - self._current_pose = self.world.get_link_pose(self) + pose_in_map = self.local_transformer.transform_pose(pose, "map") + if base: + pose_in_map.position = np.array(pose_in_map.position) + self.base_origin_shift - @property - def pose(self) -> Pose: + self.reset_base_pose(pose_in_map) + + if set_attachments: + self._set_attached_objects_poses() + + def reset_base_pose(self, pose: Pose): + self.world.reset_object_base_pose(self, pose) + self.update_pose() + + def update_pose(self): """ - The pose of the link relative to the world frame. - :return: A Pose object containing the pose of the link relative to the world frame. + Updates the current pose of this object from the world. """ - return self._current_pose + self._current_pose = self.world.get_object_pose(self) + self._update_all_joints_positions() + self._update_all_links_poses() + self.update_link_transforms() - @property - def pose_as_list(self) -> List[List[float]]: + def _update_all_joints_positions(self): """ - The pose of the link relative to the world frame as a list. - :return: A list containing the position and orientation of the link relative to the world frame. + Updates the posisitons of all joints by getting them from the simulator. """ - return self.pose.to_list() + for joint in self.joints.values(): + joint._update_position() - @property - def name(self) -> str: + def _update_all_links_poses(self): """ - The name of the link as defined in the URDF. - :return: The name of the link as a string. + Updates the poses of all links by getting them from the simulator. """ - return self.urdf_link.name + for link in self.links.values(): + link._update_pose() - def get_geometry(self) -> GeometricType: + def move_base_to_origin_pos(self) -> None: """ - Returns the geometry type of the URDF collision element of this link. + Move the object such that its base will be at the current origin position. + This is useful when placing objects on surfaces where you want the object base in contact with the surface. """ - return None if not self.collision else self.collision.geometry + self.set_pose(self.get_pose(), base=True) - def get_origin_transform(self) -> Transform: + def save_state(self, state_id) -> None: """ - Returns the transformation between the link frame and the origin frame of this link. + Saves the state of this object by saving the state of all links and attachments. + :param state_id: The unique id of the state. """ - return Transform(self.origin.xyz, list(quaternion_from_euler(*self.origin.rpy))) + self.save_links_states(state_id) + self.save_joints_states(state_id) + self.saved_states[state_id] = ObjectState(state_id, self.attachments.copy()) - @property - def origin(self) -> urdf_parser_py.urdf.Pose: + def save_links_states(self, state_id: int) -> None: """ - The URDF origin pose of this link. - :return: A '~urdf_parser_py.urdf.Pose' object containing the origin pose of this link. + Saves the state of all links of this object. + :param state_id: The unique id of the state. """ - return self.collision.origin + for link in self.links.values(): + link.save_state(state_id) - @property - def collision(self) -> Collision: + def save_joints_states(self, state_id: int) -> None: """ - The URDF collision element of this link which has a geometry, and origin. - :return: A '~urdf_parser_py.urdf.Collision' object containing the collision element of this link. + Saves the state of all joints of this object. + :param state_id: The unique id of the state. """ - return self.urdf_link.collision + for joint in self.joints.values(): + joint.save_state(state_id) - @property - def color(self) -> Color: + def restore_state(self, state_id: int) -> None: """ - The getter for the rgba_color of this link. - :return: A Color object containing the rgba_color of this link. + Restores the state of this object by restoring the state of all links and attachments. + :param state_id: The unique id of the state. """ - return self.world.get_link_color(self) + self.restore_attachments(state_id) + self.restore_links_states(state_id) + self.restore_joints_states(state_id) - @color.setter - def color(self, color: List[float]) -> None: + def restore_attachments(self, state_id: int) -> None: """ - The setter for the color of this link, could be rgb or rgba. - :param color: The color as a list of floats, either rgb or rgba. + Restores the attachments of this object from a saved state using the given state id. + :param state_id: The unique id of the state. """ - self.world.set_link_color(self, Color.from_list(color)) + self.attachments = self.saved_states[state_id].attachments + def restore_links_states(self, state_id: int) -> None: + """ + Restores the states of all links of this object from a saved state using the given state id. + :param state_id: The unique id of the state. + """ + for link in self.links.values(): + link.restore_state(state_id) -class RootLink(Link): - """ - Represents the root link of an Object in the World. - It differs from the normal Link class in that the pose ande the tf_frame is the same as that of the object. - """ + def restore_joints_states(self, state_id: int) -> None: + """ + Restores the states of all joints of this object from a saved state using the given state id. + :param state_id: The unique id of the state. + """ + for joint in self.joints.values(): + joint.restore_state(state_id) - def __init__(self, obj: Object): - super().__init__(obj.get_root_link_id(), obj.get_root_urdf_link(), obj) + def remove_saved_states(self) -> None: + """ + Removes all saved states of this object. + """ + self.saved_states = {} + self.remove_links_saved_states() + self.remove_joints_saved_states() + + def remove_links_saved_states(self) -> None: + """ + Removes all saved states of the links of this object. + """ + for link in self.links.values(): + link.remove_saved_states() + + def remove_joints_saved_states(self) -> None: + """ + Removes all saved states of the joints of this object. + """ + for joint in self.joints.values(): + joint.remove_saved_states() + + def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: + """ + Updates the positions of all attached objects. This is done + by calculating the new pose in world coordinate frame and setting the + base pose of the attached objects to this new pose. + After this the _set_attached_objects method of all attached objects + will be called. + + :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent + loops in the update. + """ + + if already_moved_objects is None: + already_moved_objects = [] + + for child in self.attachments: + + if child in already_moved_objects: + continue + + attachment = self.attachments[child] + if not attachment.bidirectional: + self.update_attachment_with_object(child) + 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_attached_objects_poses(already_moved_objects + [self]) + + def set_position(self, position: Union[Pose, Point], base=False) -> None: + """ + Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position + instead of the origin in the center of the Object. The given position can either be a Pose, + in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. + + :param position: Target position as xyz. + :param base: If the bottom of the Object should be placed or the origin in the center. + """ + pose = Pose() + if isinstance(position, Pose): + target_position = position.position + pose.frame = position.frame + elif isinstance(position, Point): + target_position = position + elif isinstance(position, list): + target_position = position + else: + raise TypeError("The given position has to be a Pose, Point or a list of xyz.") + + pose.position = target_position + pose.orientation = self.get_orientation() + self.set_pose(pose, base=base) + + def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: + """ + Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only + the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. + + :param orientation: Target orientation given as a list of xyzw. + """ + pose = Pose() + if isinstance(orientation, Pose): + target_orientation = orientation.orientation + pose.frame = orientation.frame + else: + target_orientation = orientation + + pose.pose.position = self.get_position() + pose.pose.orientation = target_orientation + self.set_pose(pose) + + def get_joint_id(self, name: str) -> int: + """ + Returns the unique id for a joint name. As used by the world/simulator. + + :param name: The joint name + :return: The unique id + """ + return self.joint_name_to_id[name] + + def get_root_link_description(self) -> LinkDescription: + """ + Returns the root link of the URDF of this object. + :return: The root link as defined in the URDF of this object. + """ + root_link_name = self.object_description.get_root() + for link_description in self.object_description.links: + if link_description.name == root_link_name: + return link_description @property - def tf_frame(self) -> str: + def root_link(self) -> Link: """ - Returns the tf frame of the root link, which is the same as the tf frame of the object. - :return: A string containing the tf frame of the root link. + Returns the root link of this object. + :return: The root link of this object. """ - return self.object.tf_frame + return self.links[self.object_description.get_root()] - def _update_pose(self) -> None: - self._current_pose = self.object.get_pose() + def get_root_link_id(self) -> int: + """ + Returns the unique id of the root link of this object. + :return: The unique id of the root link of this object. + """ + return self.get_link_id(self.object_description.get_root()) + def get_link_id(self, link_name: str) -> int: + """ + Returns a unique id for a link name. + :param link_name: The name of the link. + :return: The unique id of the link. + """ + assert link_name is not None + return self.link_name_to_id[link_name] -class Object: - """ - Represents a spawned Object in the World. - """ + def get_link_by_id(self, link_id: int) -> Link: + """ + Returns the link for a given unique link id + :param link_id: The unique id of the link. + :return: The link object. + """ + return self.links[self.link_id_to_name[link_id]] - def __init__(self, name: str, obj_type: ObjectType, path: str, - pose: Optional[Pose] = None, - world: Optional[World] = None, - color: Optional[Color] = Color(), - ignore_cached_files: Optional[bool] = False): + def get_joint_by_id(self, joint_id: int) -> str: """ - The constructor loads the urdf file into the given World, if no World is specified the - :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. - The rgba_color parameter is only used when loading .stl or .obj files, - for URDFs :func:`~Object.set_color` can be used. + Returns the joint name for a unique world id - :param name: The name of the object - :param obj_type: The type of the object as an ObjectType enum. - :param path: The path to the source file, if only a filename is provided then the resourcer directories will be - searched - :param pose: The pose at which the Object should be spawned - :param world: The World in which the object should be spawned, - if no world is specified the :py:attr:`~World.current_world` will be used. - :param color: The rgba_color with which the object should be spawned. - :param ignore_cached_files: If true the file will be spawned while ignoring cached files. + :param joint_id: The world id of for joint + :return: The joint name """ + return dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys()))[joint_id] - if pose is None: - pose = Pose() + 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_positions = [0] * len(joint_names) + self.set_joint_positions(dict(zip(joint_names, joint_positions))) - self.world: World = world if world is not None else World.current_world + def set_joint_positions(self, joint_poses: dict) -> 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. - self.name: str = name - self.obj_type: ObjectType = obj_type - self.color: Color = color + :param joint_poses: + """ + for joint_name, joint_position in joint_poses.items(): + self.joints[joint_name].position = joint_position + self.update_pose() + self._set_attached_objects_poses() - self.local_transformer = LocalTransformer() - self.original_pose = self.local_transformer.transform_pose(pose, "map") - self._current_pose = self.original_pose + 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. - self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) + :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() - self.tf_frame = ("prospection/" if self.world.is_prospection_world else "") + self.name + "_" + str(self.id) + def get_joint_position(self, joint_name: str) -> float: + return self.joints[joint_name].position - self._init_urdf_object() + def get_joint_damping(self, joint_name: str) -> float: + return self.joints[joint_name].damping - if self.urdf_object.name == robot_description.name: - self.world.set_robot_if_not_set(self) + def get_joint_upper_limit(self, joint_name: str) -> float: + return self.joints[joint_name].upper_limit - self._init_joint_name_and_id_map() - self._init_link_name_and_id_map() + def get_joint_lower_limit(self, joint_name: str) -> float: + return self.joints[joint_name].lower_limit - self._init_links_and_update_transforms() - self._init_joints() + def get_joint_axis(self, joint_name: str) -> Point: + return self.joints[joint_name].axis - self.attachments: Dict[Object, Attachment] = {} - self.saved_states: Dict[int, ObjectState] = {} + def get_joint_type(self, joint_name: str) -> JointType: + return self.joints[joint_name].type - if not self.world.is_prospection_world: - self._add_to_world_sync_obj_queue(self.path) + def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: + return self.joints[joint_name].limits - self.world.objects.append(self) + def find_joint_above(self, link_name: str, joint_type: JointType) -> str: + """ + Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. - def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, str]: + :param link_name: Link name above which the joint should be found + :param joint_type: Joint type that should be searched for + :return: Name of the first joint which has the given type """ - Loads an object to the given World with the given position and orientation. The rgba_color will only be - used when an .obj or .stl file is given. - If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created - and this URDf file will be loaded instead. - When spawning a URDf file a new file will be created in the cache directory, if there exists none. - This new file will have resolved mesh file paths, meaning there will be no references - to ROS packges instead there will be absolute file paths. + chain = self.object_description.get_chain(self.object_description.get_root(), link_name) + reversed_chain = reversed(chain) + container_joint = None + for element in reversed_chain: + if element in self.joint_name_to_id and self.get_joint_type(element) == joint_type: + container_joint = element + break + if not container_joint: + rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") + return container_joint - :param ignore_cached_files: Whether to ignore files in the cache directory. - :return: The unique id of the object and the path of the file that was loaded. + def get_positions_of_all_joints(self) -> Dict[str: float]: """ - pa = pathlib.Path(path) - extension = pa.suffix - if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): - for data_dir in World.data_directory: - path = get_path_from_data_dir(path, data_dir) - if path: - break + Returns the positions of all joints of the object as a dictionary of joint names and joint positions. - if not path: - raise FileNotFoundError( - f"File {pa.name} could not be found in the resource directory {World.data_directory}") - if not pathlib.Path(World.cach_dir).exists(): - os.mkdir(World.cach_dir) + :return: A dictionary with all joints positions'. + """ + return {j.name: j.position for j in self.joints.values()} - # if file is not yet cached corrcet the urdf and save if in the cache directory - if not _is_cached(path) or ignore_cached_files: - if extension == ".obj" or extension == ".stl": - path = self._generate_urdf_file(path) - elif extension == ".urdf": - with open(path, mode="r") as f: - urdf_string = fix_missing_inertial(f.read()) - urdf_string = remove_error_tags(urdf_string) - urdf_string = fix_link_attributes(urdf_string) - try: - urdf_string = _correct_urdf_string(urdf_string) - except rospkg.ResourceNotFound as e: - rospy.logerr(f"Could not find resource package linked in this URDF") - raise e - path = World.cach_dir + pa.name - with open(path, mode="w") as f: - f.write(urdf_string) - else: # Using the urdf from the parameter server - urdf_string = rospy.get_param(path) - path = World.cach_dir + self.name + ".urdf" - with open(path, mode="w") as f: - f.write(_correct_urdf_string(urdf_string)) - # save correct path in case the file is already in the cache directory - elif extension == ".obj" or extension == ".stl": - path = World.cach_dir + pa.stem + ".urdf" - elif extension == ".urdf": - path = World.cach_dir + pa.name + def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: + """ + Updates the transforms of all links of this object using time 'transform_time' or the current ros time. + """ + for link in self.links.values(): + link.update_transform(transform_time) + + def contact_points(self) -> List: + """ + Returns a list of contact points of this Object with other Objects. + + :return: A list of all contact points with other objects + """ + return self.world.get_object_contact_points(self) + + def contact_points_simulated(self) -> List: + """ + Returns a list of all contact points between this Object and other Objects after stepping the simulation once. + + :return: A list of contact points between this Object and other Objects + """ + state_id = self.world.save_state() + self.world.step() + contact_points = self.contact_points() + self.world.restore_state(state_id) + return contact_points + + def set_color(self, rgba_color: Color) -> None: + """ + Changes the color of this object, the color has to be given as a list + of RGBA values. + + :param rgba_color: The color as Color object with RGBA values between 0 and 1 + """ + # Check if there is only one link, this is the case for primitive + # forms or if loaded from an .stl or .obj file + if self.links != {}: + for link in self.links.values(): + link.color = rgba_color + else: + self.root_link.color = rgba_color + + def get_color(self) -> Union[Color, Dict[str, Color]]: + """ + This method returns the rgba_color of this object. The return is either: + + 1. A Color object with RGBA values, this is the case if the object only has one link (this + happens for example if the object is spawned from a .obj or .stl file) + 2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object. + Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which + the object is spawned. + + :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and + the rgba_color as value. + """ + link_to_color_dict = self.links_colors + + if len(link_to_color_dict) == 1: + return list(link_to_color_dict.values())[0] else: - path = World.cach_dir + self.name + ".urdf" + return link_to_color_dict + + @property + def links_colors(self) -> Dict[str, Color]: + """ + The color of each link as a dictionary with link names as keys and RGBA colors as values. + """ + return self.world.get_colors_of_object_links(self) + + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis aligned bounding box of this object. + """ + return self.world.get_object_axis_aligned_bounding_box(self) + + def get_base_origin(self) -> Pose: + """ + :return: the origin of the base/bottom of this object. + """ + aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() + base_width = np.absolute(aabb.min_x - aabb.max_x) + base_length = np.absolute(aabb.min_y - aabb.max_y) + return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], + self.get_pose().orientation_as_list()) + + +def filter_contact_points(contact_points, exclude_ids) -> List: + """ + Returns a list of contact points where Objects that are in the 'exclude_ids' list are removed. + + :param contact_points: A list of contact points + :param exclude_ids: A list of unique ids of Objects that should be removed from the list + :return: A list containing 'contact_points' without Objects that are in 'exclude_ids' + """ + return list(filter(lambda cp: cp[2] not in exclude_ids, contact_points)) + + +class EntityDescription(ABC): + + @property + @abstractmethod + def origin(self) -> Pose: + """ + Returns the origin of this entity. + """ + pass + + @property + @abstractmethod + def name(self) -> str: + """ + Returns the name of this entity. + """ + pass + - try: - obj_id = self.world.load_urdf_and_get_object_id(path, Pose(self.get_position_as_list(), - self.get_orientation_as_list())) - return obj_id, path - except Exception as e: - logging.error( - "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" - " the name of an URDF string on the parameter server.") - os.remove(path) - raise (e) +class ObjectDescription(EntityDescription): - def _generate_urdf_file(self, path) -> str: - """ - Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be - used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with - the name as filename. - - :return: The absolute path of the created file - """ - urdf_template = ' \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - ' - urdf_template = fix_missing_inertial(urdf_template) - rgb = " ".join(list(map(str, self.color.get_rgba()))) - pathlib_obj = pathlib.Path(path) - path = str(pathlib_obj.resolve()) - content = urdf_template.replace("~a", self.name).replace("~b", path).replace("~c", rgb) - with open(World.cach_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: - file.write(content) - return World.cach_dir + pathlib_obj.stem + ".urdf" - - def _init_urdf_object(self) -> None: - """ - Initializes the URDF object from the URDF file. - """ - with open(self.path) as f: - self.urdf_object = URDF.from_xml_string(f.read()) + file_extension: str - def _init_joint_name_and_id_map(self) -> None: + def __init__(self, path: Any): + self.parsed_description = self.from_description_file(path) + + @abstractmethod + def from_description_file(self, path: str) -> Any: """ - Creates a dictionary which maps the joint names to their unique ids and vice versa. + Return the object parsed from the description file. + :path: The path of the description file. """ - joint_names = self.world.get_joint_names(self) - n_joints = len(joint_names) - self.joint_name_to_id = dict(zip(joint_names, range(n_joints))) - self.joint_id_to_name = dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys())) + pass - def _init_link_name_and_id_map(self) -> None: + @classmethod + @abstractmethod + def generate_from_obj_file(cls, path: str) -> str: """ - Creates a dictionary which maps the link names to their unique ids and vice versa. + Generates a description file from an .obj file and returns the path of the generated file. + :param path: The path to the .obj file. + :return: The path of the generated description file. """ - link_names = self.world.get_link_names(self) - n_links = len(link_names) - self.link_name_to_id: Dict[str, int] = dict(zip(link_names, range(n_links))) - self.link_name_to_id[self.urdf_object.get_root()] = -1 - self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) - def _init_links_and_update_transforms(self) -> None: + @classmethod + @abstractmethod + def generate_from_stl_file(cls, path: str) -> str: """ - Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the - corresponding link objects. + Generates a description file from an .stl file and returns the path of the generated file. + :param path: The path to the .stl file. + :return: The path of the generated description file. """ - self.links = {} - for urdf_link in self.urdf_object.links: - link_name = urdf_link.name - link_id = self.link_name_to_id[link_name] - if link_name == self.urdf_object.get_root(): - self.links[link_name] = RootLink(self) - else: - self.links[link_name] = Link(link_id, urdf_link, self) - - - self.update_link_transforms() + pass - def _init_joints(self): + @classmethod + @abstractmethod + def preprocess_file(cls, path: str) -> str: """ - Initialize the joint objects from the URDF file and creates a dictionary which mas the joint names to the - corresponding joint objects + Preprocesses the given file and returns the preprocessed description string. + :param path: The path of the file to preprocess. + :return: The preprocessed description string. """ - self.joints = {} - for urdf_joint in self.urdf_object.joints: - joint_name = urdf_joint.name - joint_id = self.joint_name_to_id[joint_name] - self.joints[joint_name] = Joint(joint_id, urdf_joint, self) + pass - def _add_to_world_sync_obj_queue(self, path: str) -> None: + @classmethod + @abstractmethod + def preprocess_from_parameter_server(cls, name: str) -> str: """ - Adds this object to the objects queue of the WorldSync object of the World. - :param path: The path of the URDF file of this object. + Preprocesses the description from the ROS parameter server and returns the preprocessed description string. + :param name: The name of the description on the parameter server. + :return: The preprocessed description string. """ - self.world.world_sync.add_obj_queue.put( - [self.name, self.obj_type, path, self.get_position_as_list(), self.get_orientation_as_list(), - self.world.prospection_world, self.color, self]) + pass @property - def base_origin_shift(self) -> np.ndarray: + @abstractmethod + def links(self) -> List[LinkDescription]: """ - The shift between the base of the object and the origin of the object. - :return: A numpy array with the shift between the base of the object and the origin of the object. + :return: A list of links descriptions of this object. """ - return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) - - def __repr__(self): - skip_attr = ["links", "joints", "urdf_object", "attachments"] - return self.__class__.__qualname__ + f"(" + ', \n'.join( - [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" + pass - def remove(self) -> None: + @property + @abstractmethod + def joints(self) -> List[JointDescription]: """ - Removes this object from the World it currently resides in. - For the object to be removed it has to be detached from all objects it - is currently attached to. After this is done a call to world remove object is done - to remove this Object from the simulation/world. + :return: A list of joints descriptions of this object. """ - self.world.remove_object(self) + pass - def reset(self, remove_saved_states=True) -> None: + @abstractmethod + def get_root(self) -> str: """ - Resets the Object to the state it was first spawned in. - All attached objects will be detached, all joints will be set to the - default position of 0 and the object will be set to the position and - orientation in which it was spawned. - :param remove_saved_states: If True the saved states will be removed. + :return: the name of the root link of this object. """ - self.detach_all() - self.reset_all_joints_positions() - self.set_pose(self.original_pose) - if remove_saved_states: - self.saved_states = {} + pass - def attach(self, - child_object: Object, - parent_link: Optional[str] = None, - child_link: Optional[str] = None, - bidirectional: Optional[bool] = True) -> None: + @abstractmethod + def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: """ - Attaches another object to this object. This is done by - saving the transformation between the given link, if there is one, and - the base pose of the other object. Additionally, the name of the link, to - which the object is attached, will be saved. - Furthermore, a simulator constraint will be created so the attachment - also works while simulation. - Loose attachments means that the attachment will only be one-directional. For example, if this object moves the - other, attached, object will also move but not the other way around. + :return: the chain of links from 'start_link_name' to 'end_link_name'. + """ + pass - :param child_object: The other object that should be attached. - :param parent_link: The link name of this object. - :param child_link: The link name of the other object. - :param bidirectional: If the attachment should be a loose attachment. + +class LinkDescription(EntityDescription): + """ + A class that represents a link description of an object. + """ + + def __init__(self, parsed_link_description: Any): + self.parsed_description = parsed_link_description + + @property + @abstractmethod + def geometry(self) -> Shape: """ - parent_link = self.links[parent_link] if parent_link else self.get_root_link() - child_link = child_object.links[child_link] if child_link else child_object.get_root_link() + Returns the geometry type of the URDF collision element of this link. + """ + pass - attachment = Attachment(parent_link, child_link, bidirectional) - self.attachments[child_object] = attachment - child_object.attachments[self] = attachment.get_inverse() +class JointDescription(EntityDescription): + """ + A class that represents a joint description of a URDF joint. + """ - self.world.attachment_event(self, [self, child_object]) + def __init__(self, parsed_joint_description: Any): + self.parsed_description = parsed_joint_description - def detach(self, child_object: Object) -> None: + @property + @abstractmethod + def type(self) -> JointType: """ - Detaches another object from this object. This is done by - deleting the attachment from the attachments dictionary of both objects - and deleting the constraint of the simulator. - Afterward the detachment event of the corresponding World will be fired. + :return: The type of this joint. + """ + pass - :param child_object: The object which should be detached + @property + @abstractmethod + def axis(self) -> Point: """ - del self.attachments[child_object] - del child_object.attachments[self] + :return: The axis of this joint, for example the rotation axis for a revolute joint. + """ + pass - self.world.detachment_event(self, [self, child_object]) + @property + @abstractmethod + def has_limits(self) -> bool: + """ + Checks if this joint has limits. + :return: True if the joint has limits, False otherwise. + """ + pass - def detach_all(self) -> None: + @property + def limits(self) -> Tuple[float, float]: """ - Detach all objects attached to this object. + :return: The lower and upper limits of this joint. """ - attachments = self.attachments.copy() - for att in attachments.keys(): - self.detach(att) + lower, upper = self.lower_limit, self.upper_limit + if lower > upper: + lower, upper = upper, lower + return lower, upper - def update_attachment_with_object(self, child_object: Object): - self.attachments[child_object].update_transform_and_constraint() + @property + @abstractmethod + def lower_limit(self) -> Union[float, None]: + """ + :return: The lower limit of this joint, or None if the joint has no limits. + """ + pass - def get_position(self) -> Point: + @property + @abstractmethod + def upper_limit(self) -> Union[float, None]: """ - Returns the position of this Object as a list of xyz. + :return: The upper limit of this joint, or None if the joint has no limits. + """ + pass - :return: The current position of this object + @property + @abstractmethod + def parent_link_name(self) -> str: """ - return self.get_pose().position + :return: The name of the parent link of this joint. + """ + pass - def get_orientation(self) -> Pose.orientation: + @property + @abstractmethod + def child_link_name(self) -> str: """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. + :return: The name of the child link of this joint. + """ + pass - :return: A list of xyzw + @property + def damping(self) -> float: """ - return self.get_pose().orientation + :return: The damping of this joint. + """ + raise NotImplementedError - def get_position_as_list(self) -> List[float]: + @property + def friction(self) -> float: """ - Returns the position of this Object as a list of xyz. + :return: The friction of this joint. + """ + raise NotImplementedError - :return: The current position of this object + +class ObjectEntity(WorldEntity): + """ + An abstract base class that represents a physical part/entity of an Object. + This can be a link or a joint of an Object. + """ + + def __init__(self, _id: int, obj: Object): + WorldEntity.__init__(self, _id, obj.world) + self.object: Object = obj + + @property + @abstractmethod + def pose(self) -> Pose: """ - return self.get_pose().position_as_list() + :return: The pose of this entity relative to the world frame. + """ + pass - def get_orientation_as_list(self) -> List[float]: + @property + def transform(self) -> Transform: """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. + Returns the transform of this entity. - :return: A list of xyzw + :return: The transform of this entity. """ - return self.get_pose().orientation_as_list() + return self.pose.to_transform(self.tf_frame) - def get_pose(self) -> Pose: + @property + @abstractmethod + def tf_frame(self) -> str: """ - Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. + Returns the tf frame of this entity. - :return: The current pose of this object + :return: The tf frame of this entity. """ - return self._current_pose + pass - def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Optional[bool] = True) -> None: + @property + def object_id(self) -> int: """ - Sets the Pose of the object. + :return: the id of the object to which this entity belongs. + """ + return self.object.id - :param pose: New Pose for the object - :param base: If True places the object base instead of origin at the specified position and orientation - :param set_attachments: Whether to set the poses of the attached objects to this object or not. + def __repr__(self): + return self.__class__.__qualname__ + f"(" + ', \n'.join( + [f"{key}={value}" for key, value in self.__dict__.items()]) + ")" + + def __str__(self): + return self.__repr__() + + +class AbstractConstraint: + """ + Represents an abstract constraint concept, this could be used to create joints for example or any kind of constraint + between two links in the world. + """ + def __init__(self, + parent_link: Link, + child_link: Link, + _type: JointType, + parent_to_constraint: Transform, + child_to_constraint: Transform): + self.parent_link: Link = parent_link + self.child_link: Link = child_link + self.type: JointType = _type + self.parent_to_constraint = parent_to_constraint + self.child_to_constraint = child_to_constraint + self._parent_to_child = None + + @property + def parent_to_child_transform(self) -> Union[Transform, None]: + if self._parent_to_child is None: + if self.parent_to_constraint is not None and self.child_to_constraint is not None: + self._parent_to_child = self.parent_to_constraint * self.child_to_constraint.invert() + return self._parent_to_child + + @parent_to_child_transform.setter + def parent_to_child_transform(self, transform: Transform) -> None: + self._parent_to_child = transform + + @property + def parent_object_id(self) -> int: """ - pose_in_map = self.local_transformer.transform_pose(pose, "map") - if base: - pose_in_map.position = np.array(pose_in_map.position) + self.base_origin_shift + Returns the id of the parent object of the constraint. - self.reset_base_pose(pose_in_map) + :return: The id of the parent object of the constraint + """ + return self.parent_link.object_id - if set_attachments: - self._set_attached_objects_poses() + @property + def child_object_id(self) -> int: + """ + Returns the id of the child object of the constraint. + + :return: The id of the child object of the constraint + """ + return self.child_link.object_id + + @property + def parent_link_id(self) -> int: + """ + Returns the id of the parent link of the constraint. - def reset_base_pose(self, pose: Pose): - self.world.reset_object_base_pose(self, pose) - self.update_pose() + :return: The id of the parent link of the constraint + """ + return self.parent_link.id - def update_pose(self): + @property + def child_link_id(self) -> int: """ - Updates the current pose of this object from the world. + Returns the id of the child link of the constraint. + + :return: The id of the child link of the constraint """ - self._current_pose = self.world.get_object_pose(self) - self._update_all_joints_positions() - self._update_all_links_poses() - self.update_link_transforms() + return self.child_link.id - def _update_all_joints_positions(self): + @property + def position_wrt_parent_as_list(self) -> List[float]: """ - Updates the posisitons of all joints by getting them from the simulator. + Returns the constraint frame pose with respect to the parent origin as a list. + + :return: The constraint frame pose with respect to the parent origin as a list """ - for joint in self.joints.values(): - joint._update_position() + return self.pose_wrt_parent.position_as_list() - def _update_all_links_poses(self): + @property + def orientation_wrt_parent_as_list(self) -> List[float]: """ - Updates the poses of all links by getting them from the simulator. + Returns the constraint frame orientation with respect to the parent origin as a list. + + :return: The constraint frame orientation with respect to the parent origin as a list """ - for link in self.links.values(): - link._update_pose() + return self.pose_wrt_parent.orientation_as_list() - def move_base_to_origin_pos(self) -> None: + @property + def pose_wrt_parent(self) -> Pose: """ - Move the object such that its base will be at the current origin position. - This is useful when placing objects on surfaces where you want the object base in contact with the surface. + Returns the joint frame pose with respect to the parent origin. + + :return: The joint frame pose with respect to the parent origin """ - self.set_pose(self.get_pose(), base=True) + return self.parent_to_constraint.to_pose() - def save_state(self, state_id) -> None: + @property + def position_wrt_child_as_list(self) -> List[float]: """ - Saves the state of this object by saving the state of all links and attachments. - :param state_id: The unique id of the state. + Returns the constraint frame pose with respect to the child origin as a list. + + :return: The constraint frame pose with respect to the child origin as a list """ - self.save_links_states(state_id) - self.save_joints_states(state_id) - self.saved_states[state_id] = ObjectState(state_id, self.attachments.copy()) + return self.pose_wrt_child.position_as_list() - def save_links_states(self, state_id: int) -> None: + @property + def orientation_wrt_child_as_list(self) -> List[float]: """ - Saves the state of all links of this object. - :param state_id: The unique id of the state. + Returns the constraint frame orientation with respect to the child origin as a list. + + :return: The constraint frame orientation with respect to the child origin as a list """ - for link in self.links.values(): - link.save_state(state_id) + return self.pose_wrt_child.orientation_as_list() - def save_joints_states(self, state_id: int) -> None: + @property + def pose_wrt_child(self) -> Pose: """ - Saves the state of all joints of this object. - :param state_id: The unique id of the state. + Returns the joint frame pose with respect to the child origin. + + :return: The joint frame pose with respect to the child origin """ - for joint in self.joints.values(): - joint.save_state(state_id) + return self.child_to_constraint.to_pose() - def restore_state(self, state_id: int) -> None: + +class Constraint(AbstractConstraint): + """ + Represents a constraint between two links in the World. + """ + + def __init__(self, + parent_link: Link, + child_link: Link, + _type: JointType, + axis_in_child_frame: Point, + constraint_to_parent: Transform, + child_to_constraint: Transform): + parent_to_constraint = constraint_to_parent.invert() + AbstractConstraint.__init__(self, parent_link, child_link, _type, parent_to_constraint, child_to_constraint) + self.axis: Point = axis_in_child_frame + + @property + def axis_as_list(self) -> List[float]: """ - Restores the state of this object by restoring the state of all links and attachments. - :param state_id: The unique id of the state. + Returns the axis of this constraint as a list. + + :return: The axis of this constraint as a list of xyz """ - self.restore_attachments(state_id) - self.restore_links_states(state_id) - self.restore_joints_states(state_id) + return [self.axis.x, self.axis.y, self.axis.z] - def restore_attachments(self, state_id: int) -> None: + +class Joint(AbstractConstraint, ObjectEntity, JointDescription, ABC): + """ + Represents a joint of an Object in the World. + """ + + def __init__(self, _id: int, + joint_description: JointDescription, + obj: Object): + AbstractConstraint.__init__(self, + self.parent_link, + self.child_link, + joint_description.type, + self.parent_link.get_transform_to_link(self.child_link), + Transform(frame=self.child_link.tf_frame)) + ObjectEntity.__init__(self, _id, obj) + self._update_position() + + @property + def tf_frame(self) -> str: """ - Restores the attachments of this object from a saved state using the given state id. - :param state_id: The unique id of the state. + The tf frame of a joint is the tf frame of the child link. """ - self.attachments = self.saved_states[state_id].attachments + return self.child_link.tf_frame - def restore_links_states(self, state_id: int) -> None: + @property + def pose(self) -> Pose: """ - Restores the states of all links of this object from a saved state using the given state id. - :param state_id: The unique id of the state. + Returns the pose of this joint. The pose is the pose of the child link of this joint. + + :return: The pose of this joint. """ - for link in self.links.values(): - link.restore_state(state_id) + return self.child_link.pose - def restore_joints_states(self, state_id: int) -> None: + def _update_position(self) -> None: """ - Restores the states of all joints of this object from a saved state using the given state id. - :param state_id: The unique id of the state. + Updates the current position of the joint from the physics simulator. """ - for joint in self.joints.values(): - joint.restore_state(state_id) + self._current_position = self.world.get_joint_position(self.object, self.name) - def remove_saved_states(self) -> None: + @property + def parent_link(self) -> Link: """ - Removes all saved states of this object. + Returns the parent link of this joint. + :return: The parent link as a Link object. """ - self.saved_states = {} - self.remove_links_saved_states() - self.remove_joints_saved_states() + return self.object.links[self.parent_link_name] - def remove_links_saved_states(self) -> None: + @property + def child_link(self) -> Link: """ - Removes all saved states of the links of this object. + Returns the child link of this joint. + :return: The child link as a Link object. """ - for link in self.links.values(): - link.remove_saved_states() + return self.object.links[self.child_link_name] - def remove_joints_saved_states(self) -> None: + @property + def position(self) -> float: + return self._current_position + + def reset_position(self, position: float) -> None: + self.world.reset_joint_position(self.object, self.name, position) + self._update_position() + + def get_object_id(self) -> int: """ - Removes all saved states of the joints of this object. + Returns the id of the object to which this joint belongs. + :return: The integer id of the object to which this joint belongs. """ - for joint in self.joints.values(): - joint.remove_saved_states() + return self.object.id - def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: + @position.setter + def position(self, joint_position: float) -> None: """ - Updates the positions of all attached objects. This is done - by calculating the new pose in world coordinate frame and setting the - base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. + Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, as stated + in the URDF, an error will be printed. However, the joint will be set either way. - :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent - loops in the update. + :param joint_position: The target pose for this joint """ + # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly + if self.has_limits: + low_lim, up_lim = self.limits + if not low_lim <= joint_position <= up_lim: + logging.error( + f"The joint position has to be within the limits of the joint. The joint limits for {self.name}" + f" are {low_lim} and {up_lim}") + logging.error(f"The given joint position was: {joint_position}") + # Temporarily disabled because kdl outputs values exciting joint limits + # return + self.reset_position(joint_position) - if already_moved_objects is None: - already_moved_objects = [] + def enable_force_torque_sensor(self) -> None: + self.world.enable_joint_force_torque_sensor(self.object, self.id) - for child in self.attachments: + def disable_force_torque_sensor(self) -> None: + self.world.disable_joint_force_torque_sensor(self.object, self.id) - if child in already_moved_objects: - continue + def get_reaction_force_torque(self) -> List[float]: + return self.world.get_joint_reaction_force_torque(self.object, self.id) - attachment = self.attachments[child] - if not attachment.bidirectional: - self.update_attachment_with_object(child) - child.update_attachment_with_object(self) + def get_applied_motor_torque(self) -> float: + return self.world.get_applied_joint_motor_torque(self.object, self.id) - else: - link_to_object = attachment.parent_to_child_transform - child.set_pose(link_to_object.to_pose(), set_attachments=False) - child._set_attached_objects_poses(already_moved_objects + [self]) + def restore_state(self, state_id: int) -> None: + self.current_state = self.saved_states[state_id] - def set_position(self, position: Union[Pose, Point], base=False) -> None: - """ - Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position - instead of the origin in the center of the Object. The given position can either be a Pose, - in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. + @property + def current_state(self) -> JointState: + return JointState(self.position) - :param position: Target position as xyz. - :param base: If the bottom of the Object should be placed or the origin in the center. - """ - pose = Pose() - if isinstance(position, Pose): - target_position = position.position - pose.frame = position.frame - elif isinstance(position, Point): - target_position = position - elif isinstance(position, list): - target_position = position - else: - raise TypeError("The given position has to be a Pose, Point or a list of xyz.") + @current_state.setter + def current_state(self, joint_state: JointState) -> None: + self.position = joint_state.position - pose.position = target_position - pose.orientation = self.get_orientation() - self.set_pose(pose, base=base) - def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: - """ - Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only - the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. +class Link(ObjectEntity, LinkDescription, ABC): + """ + Represents a link of an Object in the World. + """ - :param orientation: Target orientation given as a list of xyzw. - """ - pose = Pose() - if isinstance(orientation, Pose): - target_orientation = orientation.orientation - pose.frame = orientation.frame - else: - target_orientation = orientation + def __init__(self, _id: int, obj: Object, parsed_link_description: Any): + ObjectEntity.__init__(self, _id, obj) + LinkDescription.__init__(self, parsed_link_description) + self.local_transformer: LocalTransformer = LocalTransformer() + self.constraint_ids: Dict[Link, int] = {} + self._update_pose() - pose.pose.position = self.get_position() - pose.pose.orientation = target_orientation - self.set_pose(pose) + @property + def current_state(self) -> LinkState: + return LinkState(self.constraint_ids.copy()) - def get_joint_id(self, name: str) -> int: - """ - Returns the unique id for a joint name. As used by the world/simulator. + @current_state.setter + def current_state(self, link_state: LinkState) -> None: + self.constraint_ids = link_state.constraint_ids - :param name: The joint name - :return: The unique id + def add_fixed_constraint_with_link(self, child_link: Link) -> int: """ - return self.joint_name_to_id[name] + 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. + :return: The unique id of the constraint. + """ + constraint_id = self.world.add_fixed_constraint(self, + child_link, + child_link.get_transform_from_link(self)) + self.constraint_ids[child_link] = constraint_id + child_link.constraint_ids[self] = constraint_id + return constraint_id - def get_root_urdf_link(self) -> urdf_parser_py.urdf.Link: + def remove_constraint_with_link(self, child_link: Link) -> None: """ - Returns the root link of the URDF of this object. - :return: The root link as defined in the URDF of this object. + Removes the constraint between this link and the given link. + :param child_link: The child link of the constraint that should be removed. + """ + self.world.remove_constraint(self.constraint_ids[child_link]) + del self.constraint_ids[child_link] + del child_link.constraint_ids[self] + + @property + def is_root(self) -> bool: """ - link_name = self.urdf_object.get_root() - for link in self.urdf_object.links: - if link.name == link_name: - return link + Returns whether this link is the root link of the object. + :return: True if this link is the root link, False otherwise. + """ + return self.object.get_root_link_id() == self.id - def get_root_link(self) -> Link: + def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: """ - Returns the root link of this object. - :return: The root link of this object. + Updates the transformation of this link at the given time. + :param transform_time: The time at which the transformation should be updated. """ - return self.links[self.urdf_object.get_root()] + self.local_transformer.update_transforms([self.transform], transform_time) - def get_root_link_id(self) -> int: + def get_transform_to_link(self, link: Link) -> Transform: """ - Returns the unique id of the root link of this object. - :return: The unique id of the root link of this object. + Returns the transformation from this link to the given link. + :param link: The link to which the transformation should be returned. + :return: A Transform object with the transformation from this link to the given link. """ - return self.get_link_id(self.urdf_object.get_root()) + return link.get_transform_from_link(self) - def get_link_id(self, link_name: str) -> int: + def get_transform_from_link(self, link: Link) -> Transform: """ - Returns a unique id for a link name. - :param link_name: The name of the link. - :return: The unique id of the link. + Returns the transformation from the given link to this link. + :param link: The link from which the transformation should be returned. + :return: A Transform object with the transformation from the given link to this link. """ - assert link_name is not None - return self.link_name_to_id[link_name] + return self.get_pose_wrt_link(link).to_transform(self.tf_frame) - def get_link_by_id(self, link_id: int) -> Link: + def get_pose_wrt_link(self, link: Link) -> Pose: """ - Returns the link for a given unique link id - :param link_id: The unique id of the link. - :return: The link object. + Returns the pose of this link with respect to the given link. + :param link: The link with respect to which the pose should be returned. + :return: A Pose object with the pose of this link with respect to the given link. """ - return self.links[self.link_id_to_name[link_id]] + return self.local_transformer.transform_pose(self.pose, link.tf_frame) - def get_joint_by_id(self, joint_id: int) -> str: + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ - Returns the joint name for a unique world id - - :param joint_id: The world id of for joint - :return: The joint name + Returns the axis aligned bounding box of this link. + :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. """ - return dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys()))[joint_id] + return self.world.get_link_axis_aligned_bounding_box(self) - def reset_all_joints_positions(self) -> None: + @property + def position(self) -> Point: """ - Sets the current position of all joints to 0. This is useful if the joints should be reset to their default + The getter for the position of the link relative to the world frame. + :return: A Point object containing the position of the link relative to the world frame. """ - joint_names = list(self.joint_name_to_id.keys()) - joint_positions = [0] * len(joint_names) - self.set_joint_positions(dict(zip(joint_names, joint_positions))) + return self.pose.position - def set_joint_positions(self, joint_poses: dict) -> None: + @property + def position_as_list(self) -> List[float]: """ - 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: + The getter for the position of the link relative to the world frame as a list. + :return: A list containing the position of the link relative to the world frame. """ - for joint_name, joint_position in joint_poses.items(): - self.joints[joint_name].position = joint_position - self.update_pose() - self._set_attached_objects_poses() + return self.pose.position_as_list() - def set_joint_position(self, joint_name: str, joint_position: float) -> None: + @property + def orientation(self) -> Quaternion: """ - 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 + The getter for the orientation of the link relative to the world frame. + :return: A Quaternion object containing the orientation of the link relative to the world frame. """ - 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: - return self.joints[joint_name].position - - def get_joint_rest_position(self, joint_name: str) -> float: - return self.joints[joint_name].rest_position - - def get_joint_damping(self, joint_name: str) -> float: - return self.joints[joint_name].damping - - def get_joint_upper_limit(self, joint_name: str) -> float: - return self.joints[joint_name].upper_limit - - def get_joint_lower_limit(self, joint_name: str) -> float: - return self.joints[joint_name].lower_limit - - def get_joint_axis(self, joint_name: str) -> Point: - return self.joints[joint_name].axis - - def get_joint_type(self, joint_name: str) -> JointType: - return self.joints[joint_name].type - - def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: - return self.joints[joint_name].limits + return self.pose.orientation - def find_joint_above(self, link_name: str, joint_type: JointType) -> str: + @property + def orientation_as_list(self) -> List[float]: """ - Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. - - :param link_name: Link name above which the joint should be found - :param joint_type: Joint type that should be searched for - :return: Name of the first joint which has the given type + The getter for the orientation of the link relative to the world frame as a list. + :return: A list containing the orientation of the link relative to the world frame. """ - chain = self.urdf_object.get_chain(self.urdf_object.get_root(), link_name) - reversed_chain = reversed(chain) - container_joint = None - for element in reversed_chain: - if element in self.joint_name_to_id and self.get_joint_type(element) == joint_type: - container_joint = element - break - if not container_joint: - rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") - return container_joint + return self.pose.orientation_as_list() - def get_positions_of_all_joints(self) -> Dict[str: float]: + def _update_pose(self) -> None: """ - Returns the positions of all joints of the object as a dictionary of joint names and joint positions. - - :return: A dictionary with all joints positions'. + Updates the current pose of this link from the world. """ - return {j.name: j.position for j in self.joints.values()} + self._current_pose = self.world.get_link_pose(self) - def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: + @property + def pose(self) -> Pose: """ - Updates the transforms of all links of this object using time 'transform_time' or the current ros time. + The pose of the link relative to the world frame. + :return: A Pose object containing the pose of the link relative to the world frame. """ - for link in self.links.values(): - link.update_transform(transform_time) + return self._current_pose - def contact_points(self) -> List: + @property + def pose_as_list(self) -> List[List[float]]: """ - Returns a list of contact points of this Object with other Objects. - - :return: A list of all contact points with other objects + The pose of the link relative to the world frame as a list. + :return: A list containing the position and orientation of the link relative to the world frame. """ - return self.world.get_object_contact_points(self) + return self.pose.to_list() - def contact_points_simulated(self) -> List: + def get_origin_transform(self) -> Transform: """ - Returns a list of all contact points between this Object and other Objects after stepping the simulation once. - - :return: A list of contact points between this Object and other Objects + Returns the transformation between the link frame and the origin frame of this link. """ - state_id = self.world.save_state() - self.world.step() - contact_points = self.contact_points() - self.world.restore_state(state_id) - return contact_points + return self.origin.to_transform(self.tf_frame) - def set_color(self, color: Color) -> None: + @property + def color(self) -> Color: """ - Changes the rgba_color of this object, the rgba_color has to be given as a list - of RGBA values. All links of this object will be colored. - - :param color: The rgba_color as RGBA values between 0 and 1 + The getter for the rgba_color of this link. + :return: A Color object containing the rgba_color of this link. """ - self.world.set_object_color(self, color) + return self.world.get_link_color(self) - def get_color(self) -> Union[Color, Dict[str, Color]]: + @color.setter + def color(self, color: List[float]) -> None: """ - :return: The rgba_color of this object or a dictionary of link names and their colors. + The setter for the color of this link, could be rgb or rgba. + :param color: The color as a list of floats, either rgb or rgba. """ - return self.world.get_object_color(self) + self.world.set_link_color(self, Color.from_list(color)) - def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + @property + def origin_transform(self) -> Transform: """ - :return: The axis aligned bounding box of this object. + :return: The transform from world to origin of entity. """ - return self.world.get_object_axis_aligned_bounding_box(self) + return self.origin.to_transform(self.tf_frame) - def get_base_origin(self) -> Pose: + @property + def tf_frame(self) -> str: """ - :return: the origin of the base/bottom of this object. + The name of the tf frame of this link. """ - aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() - base_width = np.absolute(aabb.min_x - aabb.max_x) - base_length = np.absolute(aabb.min_y - aabb.max_y) - return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], - self.get_pose().orientation_as_list()) + return f"{self.object.tf_frame}/{self.name}" -def filter_contact_points(contact_points, exclude_ids) -> List: +class RootLink(Link, ABC): """ - Returns a list of contact points where Objects that are in the 'exclude_ids' list are removed. - - :param contact_points: A list of contact points - :param exclude_ids: A list of unique ids of Objects that should be removed from the list - :return: A list containing 'contact_points' without Objects that are in 'exclude_ids' + Represents the root link of an Object in the World. + It differs from the normal Link class in that the pose ande the tf_frame is the same as that of the object. """ - return list(filter(lambda cp: cp[2] not in exclude_ids, contact_points)) + def __init__(self, obj: Object): + super().__init__(obj.get_root_link_id(), obj, obj.get_root_link_description()) -def get_path_from_data_dir(file_name: str, data_directory: str) -> str: - """ - Returns the full path for a given file name in the given directory. If there is no file with the given filename - this method returns None. + @property + def tf_frame(self) -> str: + """ + Returns the tf frame of the root link, which is the same as the tf frame of the object. + """ + return self.object.tf_frame - :param file_name: The filename of the searched file. - :param data_directory: The directory in which to search for the file. - :return: The full path in the filesystem or None if there is no file with the filename in the directory - """ - for file in os.listdir(data_directory): - if file == file_name: - return data_directory + f"/{file_name}" + def _update_pose(self) -> None: + self._current_pose = self.object.get_pose() -class Attachment: +class Attachment(AbstractConstraint): def __init__(self, parent_link: Link, child_link: Link, @@ -2431,17 +2625,16 @@ def __init__(self, :param parent_to_child_transform: The transform from the parent link to the child object link. :param constraint_id: The id of the constraint in the simulator. """ - self.parent_link: Link = parent_link - self.child_link: Link = child_link + super().__init__(parent_link, child_link, JointType.FIXED, parent_to_child_transform, + Transform(frame=child_link.tf_frame)) + self.id = constraint_id self.bidirectional: bool = bidirectional self._loose: bool = False - self.parent_to_child_transform: Transform = parent_to_child_transform if self.parent_to_child_transform is None: self.update_transform() - self.constraint_id: int = constraint_id - if self.constraint_id is None: + if self.id is None: self.add_fixed_constraint() def update_transform_and_constraint(self) -> None: @@ -2468,8 +2661,7 @@ def add_fixed_constraint(self) -> None: """ Adds a fixed constraint between the parent link and the child link. """ - cid = self.parent_link.add_fixed_constraint_with_link(self.child_link) - self.constraint_id = cid + self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link) def calculate_transform(self) -> Transform: """ @@ -2489,7 +2681,7 @@ def get_inverse(self) -> Attachment: :return: A new Attachment object with the parent and child links swapped. """ attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, - constraint_id=self.constraint_id) + constraint_id=self.id) attachment.loose = not self._loose return attachment @@ -2522,106 +2714,63 @@ def __del__(self) -> None: self.remove_constraint_if_exists() -def _correct_urdf_string(urdf_string: str) -> str: - """ - Changes paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legac) - can't deal with ROS package paths. - - :param urdf_string: The name of the URDf on the parameter server - :return: The URDF string with paths in the filesystem instead of ROS packages - """ - r = rospkg.RosPack() - new_urdf_string = "" - for line in urdf_string.split('\n'): - if "package://" in line: - s = line.split('//') - s1 = s[1].split('/') - path = r.get_path(s1[0]) - line = line.replace("package://" + s1[0], path) - new_urdf_string += line + '\n' - - return fix_missing_inertial(new_urdf_string) - - -def fix_missing_inertial(urdf_string: str) -> str: - """ - Insert inertial tags for every URDF link that has no inertia. - This is used to prevent Legacy(PyBullet) from dumping warnings in the terminal - - :param urdf_string: The URDF description as string - :returns: The new, corrected URDF description as string. - """ - - inertia_tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.Element("inertial")) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("mass", {"value": "0.1"})) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) - inertia_tree.getroot().append(xml.etree.ElementTree.Element("inertia", {"ixx": "0.01", - "ixy": "0", - "ixz": "0", - "iyy": "0.01", - "iyz": "0", - "izz": "0.01"})) - - # create tree from string - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) - - for link_element in tree.iter("link"): - inertial = [*link_element.iter("inertial")] - if len(inertial) == 0: - link_element.append(inertia_tree.getroot()) - - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') - - -def remove_error_tags(urdf_string: str) -> str: - """ - Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the - URDF_parser - - :param urdf_string: String of the URDF from which the tags should be removed - :return: The URDF string with the tags removed - """ - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) - removing_tags = ["gazebo", "transmission"] - for tag_name in removing_tags: - all_tags = tree.findall(tag_name) - for tag in all_tags: - tree.getroot().remove(tag) - - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') - - -def fix_link_attributes(urdf_string: str) -> str: - """ - Removes the attribute 'type' from links since this is not parsable by the URDF parser. +class CacheManager: + @staticmethod + def _look_for_file_in_data_dir(path: str, path_object: pathlib.Path) -> str: + """ + Looks for a file in the data directory of the World. If the file is not found in the data directory, this method + raises a FileNotFoundError. + """ + if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): + for data_dir in World.data_directory: + path = get_path_from_data_dir(path, data_dir) + if path: + break - :param urdf_string: The string of the URDF from which the attributes should be removed - :return: The URDF string with the attributes removed - """ - tree = xml.etree.ElementTree.ElementTree(xml.etree.ElementTree.fromstring(urdf_string)) + if not path: + raise FileNotFoundError( + f"File {path_object.name} could not be found in the resource directory {World.data_directory}") - for link in tree.iter("link"): - if "type" in link.attrib.keys(): - del link.attrib["type"] + return path - return xml.etree.ElementTree.tostring(tree.getroot(), encoding='unicode') + @staticmethod + def get_path_from_data_dir(file_name: str, data_directory: str) -> str: + """ + Returns the full path for a given file name in the given directory. If there is no file with the given filename + this method returns None. + :param file_name: The filename of the searched file. + :param data_directory: The directory in which to search for the file. + :return: The full path in the filesystem or None if there is no file with the filename in the directory + """ + for file in os.listdir(data_directory): + if file == file_name: + return data_directory + f"/{file_name}" -def _is_cached(path) -> bool: - """ - Checks if the file in the given path is already cached or if - there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from - the parameter server is used. + @staticmethod + def _create_cache_dir_if_not_exists(): + """ + Creates the cache directory if it does not exist. + """ + if not pathlib.Path(World.cache_dir).exists(): + os.mkdir(World.cache_dir) - :return: True if there already exists a cached file, False in any other case. - """ - file_name = pathlib.Path(path).name - full_path = pathlib.Path(World.cach_dir + file_name) - if full_path.exists(): - return True - # Returns filename without the filetype, e.g. returns "test" for "test.txt" - file_stem = pathlib.Path(path).stem - full_path = pathlib.Path(World.cach_dir + file_stem + ".urdf") - if full_path.exists(): - return True - return False + @staticmethod + def _is_cached(path) -> bool: + """ + Checks if the file in the given path is already cached or if + there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from + the parameter server is used. + + :return: True if there already exists a cached file, False in any other case. + """ + file_name = pathlib.Path(path).name + full_path = pathlib.Path(World.cache_dir + file_name) + if full_path.exists(): + return True + # Returns filename without the filetype, e.g. returns "test" for "test.txt" + file_stem = pathlib.Path(path).stem + full_path = pathlib.Path(World.cache_dir + file_stem + ObjectDescription.file_extension) + if full_path.exists(): + return True + return False diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index fc96cd60e..e344dcf5e 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -5,6 +5,16 @@ from abc import ABC, abstractmethod +def get_point_as_list(point: Point) -> List[float]: + """ + Returns the point as a list. + + :param point: The point + :return: The point as a list + """ + return [point.x, point.y, point.z] + + @dataclass class Color: """ @@ -78,6 +88,54 @@ class Constraint: joint_frame_pose_wrt_parent_origin: Pose joint_frame_pose_wrt_child_origin: Pose + def get_parent_object_id(self) -> int: + """ + Returns the id of the parent object of the constraint. + + :return: The id of the parent object of the constraint + """ + return self.parent_link.object.id + + def get_child_object_id(self) -> int: + """ + Returns the id of the child object of the constraint. + + :return: The id of the child object of the constraint + """ + return self.child_link.object.id + + def get_parent_link_id(self) -> int: + """ + Returns the id of the parent link of the constraint. + + :return: The id of the parent link of the constraint + """ + return self.parent_link.id + + def get_child_link_id(self) -> int: + """ + Returns the id of the child link of the constraint. + + :return: The id of the child link of the constraint + """ + return self.child_link.id + + def get_joint_axis_as_list(self) -> List[float]: + """ + Returns the joint axis of the constraint as a list. + + :return: The joint axis of the constraint as a list + """ + return get_point_as_list(self.joint_axis_in_child_link_frame) + + def get_joint_position_wrt_parent_as_list(self) -> List[float]: + """ + Returns the joint frame pose with respect to the parent origin as a list. + + :return: The joint frame pose with respect to the parent origin as a list + """ + return self.joint_frame_pose_wrt_parent_origin.position_as_list() + @dataclass class AxisAlignedBoundingBox: @@ -262,16 +320,26 @@ def visual_geometry_type(self) -> Shape: @dataclass -class ObjectState: +class State(ABC): + pass + + +@dataclass +class WorldState(State): + state_id: int + + +@dataclass +class ObjectState(State): state_id: int attachments: Dict['Object', 'Attachment'] @dataclass -class LinkState: +class LinkState(State): constraint_ids: Dict['Link', int] @dataclass -class JointState: +class JointState(State): position: float diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 15e46e637..42725316f 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -26,8 +26,8 @@ def test_robot_orientation(self): def test_save_and_restore_state(self): self.robot.attach(self.milk) state_id = self.world.save_state() - robot_link = self.robot.get_root_link() - milk_link = self.milk.get_root_link() + robot_link = self.robot.root_link + milk_link = self.milk.root_link cid = robot_link.constraint_ids[milk_link] self.assertTrue(cid == self.robot.attachments[self.milk].constraint_id) self.world.remove_constraint(cid) @@ -45,24 +45,6 @@ def test_remove_object(self): self.assertTrue(milk_id not in [obj.id for obj in self.world.objects]) BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - def test_get_joint_rest_pose(self): - self.assertEqual(self.robot.get_joint_rest_position("head_pan_joint"), 0.0) - - def test_get_joint_damping(self): - self.assertEqual(self.robot.get_joint_damping("head_pan_joint"), 0.5) - - def test_get_joint_upper_limit(self): - self.assertEqual(self.robot.get_joint_upper_limit("head_pan_joint"), 3.007) - - def test_get_joint_lower_limit(self): - self.assertEqual(self.robot.get_joint_lower_limit("head_pan_joint"), -3.007) - - def test_get_joint_axis(self): - self.assertEqual(self.robot.get_joint_axis("head_pan_joint"), Point(0.0, 0.0, 1.0)) - - def test_get_joint_type(self): - self.assertEqual(self.robot.get_joint_type("head_pan_joint"), JointType.REVOLUTE) - def test_get_joint_position(self): self.assertEqual(self.robot.get_joint_position("head_pan_joint"), 0.0) diff --git a/test/test_links.py b/test/test_links.py index a2f7f1c4f..1205b4e23 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -4,23 +4,23 @@ class TestLinks(BulletWorldTestCase): def test_add_constraint(self): - milk_link = self.milk.get_root_link() - robot_link = self.robot.get_root_link() + milk_link = self.milk.root_link + robot_link = self.robot.root_link robot_link.add_fixed_constraint_with_link(milk_link) self.assertTrue(milk_link in robot_link.constraint_ids) self.assertTrue(robot_link in milk_link.constraint_ids) def test_remove_constraint(self): - milk_link = self.milk.get_root_link() - robot_link = self.robot.get_root_link() + milk_link = self.milk.root_link + robot_link = self.robot.root_link robot_link.add_fixed_constraint_with_link(milk_link) robot_link.remove_constraint_with_link(milk_link) self.assertTrue(milk_link not in robot_link.constraint_ids) self.assertTrue(robot_link not in milk_link.constraint_ids) def test_transform_link(self): - milk_link = self.milk.get_root_link() - robot_link = self.robot.get_root_link() + milk_link = self.milk.root_link + robot_link = self.robot.root_link milk_to_robot = milk_link.get_transform_to_link(robot_link) robot_to_milk = robot_link.get_transform_to_link(milk_link) self.assertAlmostEqual(robot_to_milk, milk_to_robot.invert()) diff --git a/test/test_object.py b/test/test_object.py index 409b1db9e..3391eddd6 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -1,4 +1,5 @@ from bullet_world_testcase import BulletWorldTestCase +from pycram.enums import JointType from pycram.pose import Pose from geometry_msgs.msg import Point @@ -17,6 +18,21 @@ def test_set_position_as_list(self): self.milk.set_position([1, 2, 3]) self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) + def test_get_joint_axis(self): + self.assertEqual(self.robot.get_joint_axis("head_pan_joint"), Point(0.0, 0.0, 1.0)) + + def test_get_joint_type(self): + self.assertEqual(self.robot.get_joint_type("head_pan_joint"), JointType.REVOLUTE) + + def test_get_joint_lower_limit(self): + self.assertEqual(self.robot.get_joint_lower_limit("head_pan_joint"), -3.007) + + def test_get_joint_upper_limit(self): + self.assertEqual(self.robot.get_joint_upper_limit("head_pan_joint"), 3.007) + + def test_get_joint_damping(self): + self.assertEqual(self.robot.get_joint_damping("head_pan_joint"), 0.5) + def test_save_state(self): self.robot.attach(self.milk) self.robot.save_state(1) From 85a6cb3e526a5322514325efa2a369d70af45254 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 9 Feb 2024 21:43:33 +0100 Subject: [PATCH 47/69] [Abstract World] Abstracted URDF dependency. Added a SateEntity for abstract state preserving classes. Cleaned code. Still needs to run tests. --- src/pycram/external_interfaces/giskard.py | 6 +- src/pycram/urdf_interface.py | 234 ++++++------- src/pycram/world.py | 396 +++++++++++++++------- src/pycram/world_dataclasses.py | 6 +- 4 files changed, 394 insertions(+), 248 deletions(-) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index fd4958f6c..686f6bfab 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -159,7 +159,7 @@ def spawn_object(object: Object) -> None: :param object: World object that should be spawned """ if len(object.link_name_to_id) == 1: - geometry = object.object_description.link_map[object.object_description.get_root()].collision.geometry + geometry = object.description.link_map[object.description.get_root()].collision.geometry if isinstance(geometry, urdf_parser_py.urdf.Mesh): filename = geometry.filename spawn_mesh(object.name + "_" + str(object.id), filename, object.get_pose()) @@ -239,8 +239,8 @@ def _manage_par_motion_goals(goal_func, *args) -> Optional['MoveResult']: if "tip_link" in par_value_pair.keys() and "root_link" in par_value_pair.keys(): if par_value_pair["tip_link"] == robot_description.base_link: continue - chain = World.robot.object_description.get_chain(par_value_pair["root_link"], - par_value_pair["tip_link"]) + chain = World.robot.description.get_chain(par_value_pair["root_link"], + par_value_pair["tip_link"]) if set(chain).intersection(used_joints) != set(): giskard_wrapper.cmd_seq = tmp raise AttributeError(f"The joint(s) {set(chain).intersection(used_joints)} is used by multiple Designators") diff --git a/src/pycram/urdf_interface.py b/src/pycram/urdf_interface.py index 240e6b955..ce82bde9a 100644 --- a/src/pycram/urdf_interface.py +++ b/src/pycram/urdf_interface.py @@ -1,13 +1,11 @@ -import os import pathlib -import re from xml.etree import ElementTree import rospkg import rospy from geometry_msgs.msg import Point from tf.transformations import quaternion_from_euler -from typing_extensions import Union, List +from typing_extensions import Union, List, Optional from urdf_parser_py import urdf from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, Sphere as URDF_Sphere, Mesh as URDF_Mesh) @@ -15,7 +13,8 @@ from pycram.enums import JointType, Shape from pycram.pose import Pose from pycram.world import JointDescription as AbstractJointDescription, Joint as AbstractJoint, \ - LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription, World, _is_cached + LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription +from pycram.world_dataclasses import Color class LinkDescription(AbstractLinkDescription): @@ -149,64 +148,18 @@ class ObjectDescription(AbstractObjectDescription): """ A class that represents an object description of an object. """ - def from_description_file(self, path) -> URDF: - """ - Create an object description from a description file. - :param path: The path to the description file. - """ + def load_description(self, path) -> URDF: with open(path, 'r') as file: return URDF.from_xml_string(file.read()) - def preprocess_file(self, path: str, ignore_cached_files: bool) -> str: - """ - Preprocesses the file, if it is a .obj or .stl file it will be converted to an URDF file. - """ - path_object = pathlib.Path(path) - extension = path_object.suffix - - path = _look_for_file_in_data_dir(path, path_object) - - _create_cache_dir_if_not_exists() - - # if file is not yet cached correct the urdf and save if in the cache directory - if not _is_cached(path) or ignore_cached_files: - if extension == ".obj" or extension == ".stl": - path = self._generate_urdf_file(path) - elif extension == ".urdf": - with open(path, mode="r") as f: - urdf_string = fix_missing_inertial(f.read()) - urdf_string = remove_error_tags(urdf_string) - urdf_string = fix_link_attributes(urdf_string) - try: - urdf_string = _correct_urdf_string(urdf_string) - except rospkg.ResourceNotFound as e: - rospy.logerr(f"Could not find resource package linked in this URDF") - raise e - path = World.cache_dir + path_object.name - with open(path, mode="w") as f: - f.write(urdf_string) - else: # Using the urdf from the parameter server - urdf_string = rospy.get_param(path) - path = World.cache_dir + self.name + ".urdf" - with open(path, mode="w") as f: - f.write(_correct_urdf_string(urdf_string)) - # save correct path in case the file is already in the cache directory - elif extension == ".obj" or extension == ".stl": - path = World.cache_dir + path_object.stem + ".urdf" - elif extension == ".urdf": - path = World.cache_dir + path_object.name - else: - path = World.cache_dir + self.name + ".urdf" - - return path - - def _generate_urdf_file(self, path) -> str: + def generate_from_mesh_file(self, path, color: Optional[Color] = Color()) -> str: """ Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be - used to crate a material tag in the URDF. The resulting file will then be saved in the cach_dir path with - the name as filename. + used to create a material tag in the URDF. + :param path: The path to the mesh file. + :param color: The color of the object. :return: The absolute path of the created file """ urdf_template = ' \n \ @@ -227,14 +180,28 @@ def _generate_urdf_file(self, path) -> str: \n \ \n \ ' - urdf_template = fix_missing_inertial(urdf_template) - rgb = " ".join(list(map(str, self.color.get_rgba()))) + urdf_template = self.fix_missing_inertial(urdf_template) + rgb = " ".join(list(map(str, color.get_rgba()))) pathlib_obj = pathlib.Path(path) path = str(pathlib_obj.resolve()) content = urdf_template.replace("~a", self.name).replace("~b", path).replace("~c", rgb) - with open(World.cache_dir + pathlib_obj.stem + ".urdf", "w", encoding="utf-8") as file: - file.write(content) - return World.cache_dir + pathlib_obj.stem + ".urdf" + return content + + def generate_from_description_file(self, path: str) -> str: + with open(path, mode="r") as f: + urdf_string = self.fix_missing_inertial(f.read()) + urdf_string = self.remove_error_tags(urdf_string) + urdf_string = self.fix_link_attributes(urdf_string) + try: + urdf_string = self.correct_urdf_string(urdf_string) + except rospkg.ResourceNotFound as e: + rospy.logerr(f"Could not find resource package linked in this URDF") + raise e + return urdf_string + + def generate_from_parameter_server(self, name: str) -> str: + urdf_string = rospy.get_param(name) + return self.correct_urdf_string(urdf_string) @property def links(self) -> List[LinkDescription]: @@ -262,87 +229,102 @@ def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: """ return self.parsed_description.get_chain(start_link_name, end_link_name) + def correct_urdf_string(self, urdf_string: str) -> str: + """ + Changes paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legacy) + can't deal with ROS package paths. -def _correct_urdf_string(urdf_string: str) -> str: - """ - Changes paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legac) - can't deal with ROS package paths. - - :param urdf_string: The name of the URDf on the parameter server - :return: The URDF string with paths in the filesystem instead of ROS packages - """ - r = rospkg.RosPack() - new_urdf_string = "" - for line in urdf_string.split('\n'): - if "package://" in line: - s = line.split('//') - s1 = s[1].split('/') - path = r.get_path(s1[0]) - line = line.replace("package://" + s1[0], path) - new_urdf_string += line + '\n' + :param urdf_string: The name of the URDf on the parameter server + :return: The URDF string with paths in the filesystem instead of ROS packages + """ + r = rospkg.RosPack() + new_urdf_string = "" + for line in urdf_string.split('\n'): + if "package://" in line: + s = line.split('//') + s1 = s[1].split('/') + path = r.get_path(s1[0]) + line = line.replace("package://" + s1[0], path) + new_urdf_string += line + '\n' - return fix_missing_inertial(new_urdf_string) + return self.fix_missing_inertial(new_urdf_string) + @staticmethod + def fix_missing_inertial(urdf_string: str) -> str: + """ + Insert inertial tags for every URDF link that has no inertia. + This is used to prevent Legacy(PyBullet) from dumping warnings in the terminal -def fix_missing_inertial(urdf_string: str) -> str: - """ - Insert inertial tags for every URDF link that has no inertia. - This is used to prevent Legacy(PyBullet) from dumping warnings in the terminal + :param urdf_string: The URDF description as string + :returns: The new, corrected URDF description as string. + """ - :param urdf_string: The URDF description as string - :returns: The new, corrected URDF description as string. - """ + inertia_tree = ElementTree.ElementTree(ElementTree.Element("inertial")) + inertia_tree.getroot().append(ElementTree.Element("mass", {"value": "0.1"})) + inertia_tree.getroot().append(ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) + inertia_tree.getroot().append(ElementTree.Element("inertia", {"ixx": "0.01", + "ixy": "0", + "ixz": "0", + "iyy": "0.01", + "iyz": "0", + "izz": "0.01"})) - inertia_tree = ElementTree.ElementTree(ElementTree.Element("inertial")) - inertia_tree.getroot().append(ElementTree.Element("mass", {"value": "0.1"})) - inertia_tree.getroot().append(ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) - inertia_tree.getroot().append(ElementTree.Element("inertia", {"ixx": "0.01", - "ixy": "0", - "ixz": "0", - "iyy": "0.01", - "iyz": "0", - "izz": "0.01"})) + # create tree from string + tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) - # create tree from string - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + for link_element in tree.iter("link"): + inertial = [*link_element.iter("inertial")] + if len(inertial) == 0: + link_element.append(inertia_tree.getroot()) - for link_element in tree.iter("link"): - inertial = [*link_element.iter("inertial")] - if len(inertial) == 0: - link_element.append(inertia_tree.getroot()) + return ElementTree.tostring(tree.getroot(), encoding='unicode') - return ElementTree.tostring(tree.getroot(), encoding='unicode') + @staticmethod + def remove_error_tags(urdf_string: str) -> str: + """ + Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the + URDF_parser + :param urdf_string: String of the URDF from which the tags should be removed + :return: The URDF string with the tags removed + """ + tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + removing_tags = ["gazebo", "transmission"] + for tag_name in removing_tags: + all_tags = tree.findall(tag_name) + for tag in all_tags: + tree.getroot().remove(tag) -def remove_error_tags(urdf_string: str) -> str: - """ - Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the - URDF_parser + return ElementTree.tostring(tree.getroot(), encoding='unicode') - :param urdf_string: String of the URDF from which the tags should be removed - :return: The URDF string with the tags removed - """ - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) - removing_tags = ["gazebo", "transmission"] - for tag_name in removing_tags: - all_tags = tree.findall(tag_name) - for tag in all_tags: - tree.getroot().remove(tag) + @staticmethod + def fix_link_attributes(urdf_string: str) -> str: + """ + Removes the attribute 'type' from links since this is not parsable by the URDF parser. - return ElementTree.tostring(tree.getroot(), encoding='unicode') + :param urdf_string: The string of the URDF from which the attributes should be removed + :return: The URDF string with the attributes removed + """ + tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + for link in tree.iter("link"): + if "type" in link.attrib.keys(): + del link.attrib["type"] -def fix_link_attributes(urdf_string: str) -> str: - """ - Removes the attribute 'type' from links since this is not parsable by the URDF parser. + return ElementTree.tostring(tree.getroot(), encoding='unicode') - :param urdf_string: The string of the URDF from which the attributes should be removed - :return: The URDF string with the attributes removed - """ - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + @property + def file_extension(self) -> str: + """ + :return: The file extension of the URDF file. + """ + return '.urdf' - for link in tree.iter("link"): - if "type" in link.attrib.keys(): - del link.attrib["type"] + @property + def origin(self) -> Pose: + return Pose(self.parsed_description.origin.xyz, + quaternion_from_euler(*self.parsed_description.origin.rpy)) - return ElementTree.tostring(tree.getroot(), encoding='unicode') + @property + def name(self) -> str: + return self.parsed_description.name diff --git a/src/pycram/world.py b/src/pycram/world.py index 08c80e300..e01ad2955 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -13,7 +13,7 @@ import numpy as np import rospy from geometry_msgs.msg import Quaternion, Point -from typing_extensions import List, Optional, Dict, Tuple, Callable, Any +from typing_extensions import List, Optional, Dict, Tuple, Callable, Any, Type from typing_extensions import Union from .enums import JointType, ObjectType, WorldMode, Shape @@ -43,13 +43,14 @@ def saved_states(self) -> Dict[int, State]: """ return self._saved_states - def save_state(self, state_id: int) -> None: + def save_state(self, state_id: int) -> int: """ Saves the state of this entity with the given state id. :param state_id: The unique id of the state. """ self._saved_states[state_id] = self.current_state + return state_id @property @abstractmethod @@ -138,6 +139,8 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ World.current_world = self World.simulation_frequency = simulation_frequency + self.cache_manager = CacheManager() + self.is_prospection_world: bool = is_prospection_world self._init_and_sync_prospection_world() @@ -202,6 +205,17 @@ def _sync_prospection_world(self): self.world_sync: WorldSync = WorldSync(self, self.prospection_world) self.world_sync.start() + def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, + obj: Object) -> str: + """ + Updates the cache directory with the given object. + + :param path: The path to the object. + :param ignore_cached_files: If the cached files should be ignored. + :param obj: The object to be added to the cache directory. + """ + return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, obj.description) + @property def simulation_time_step(self): """ @@ -613,18 +627,34 @@ def save_state(self, state_id: Optional[int] = None) -> int: :return: A unique id of the state """ - return self.current_state.state_id + state_id = self.save_physics_simulator_state() + self.save_objects_state(state_id) + return super().save_state(state_id) @property def current_state(self) -> WorldState: - state_id = self.save_physics_simulator_state() - self.save_objects_state(state_id) - self.saved_states.append(state_id) - return WorldState(state_id) + return WorldState(self.save_physics_simulator_state(), self.object_states) @current_state.setter def current_state(self, state: WorldState) -> None: - self.restore_state(state.state_id) + self.restore_physics_simulator_state(state.simulator_state_id) + self.object_states = state.object_states + + @property + def object_states(self) -> Dict[int, ObjectState]: + """ + Returns the states of all objects in the World. + :return: A dictionary with the object id as key and the object state as value. + """ + return {obj.id: obj.current_state for obj in self.objects} + + @object_states.setter + def object_states(self, states: Dict[int, ObjectState]) -> None: + """ + Sets the states of all objects in the World. + """ + for obj_id, obj_state in states.items(): + self.get_object_by_id(obj_id).current_state = obj_state def save_objects_state(self, state_id: int) -> None: """ @@ -1055,10 +1085,11 @@ def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): # self.equal_states = False for i in range(self.add_obj_queue.qsize()): obj = self.add_obj_queue.get() - # [name, type, path, position, orientation, self.world.prospection_world, rgba_color, world object] - o = Object(obj[0], obj[1], obj[2], Pose(obj[3], obj[4]), obj[5], obj[6]) + # [0:name, 1:type, 2:path, 3:description, 4:position, 5:orientation, + # 6:self.world.prospection_world, 7:rgba_color, 8:world object] + o = Object(obj[0], obj[1], obj[2], obj[3], Pose(obj[4], obj[5]), obj[6], obj[7]) # Maps the World object to the prospection world object - self.object_mapping[obj[7]] = o + self.object_mapping[obj[8]] = o self.add_obj_queue.task_done() for i in range(self.remove_obj_queue.qsize()): obj = self.remove_obj_queue.get() @@ -1121,13 +1152,15 @@ class Object(WorldEntity): """ Represents a spawned Object in the World. """ - object_description: ObjectDescription + prospection_world_prefix: str = "prospection/" """ The ObjectDescription of the object, this contains the name and type of the object as well as the path to the source file. """ + def __init__(self, name: str, obj_type: ObjectType, path: str, + description: Type[ObjectDescription], pose: Optional[Pose] = None, world: Optional[World] = None, color: Optional[Color] = Color(), @@ -1140,8 +1173,9 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, :param name: The name of the object :param obj_type: The type of the object as an ObjectType enum. - :param path: The path to the source file, if only a filename is provided then the resourcer directories will be - searched + :param path: The path to the source file, if only a filename is provided then the resources directories will be + searched. + :param description: The ObjectDescription of the object, this contains the joints and links of the object. :param pose: The pose at which the Object should be spawned :param world: The World in which the object should be spawned, if no world is specified the :py:attr:`~World.current_world` will be used. @@ -1162,14 +1196,16 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, _id, self.path = self._load_object_and_get_id(path, ignore_cached_files) + self.description = description(self.path) + super().__init__(_id, world) self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") + f"{self.name}_{self.id}") - # self.object_description = ObjectDescription(self.path) + self._update_object_description_from_file(self.path) - if self.object_description.name == robot_description.name: + if self.description.name == robot_description.name: self.world.set_robot_if_not_set(self) self._init_joint_name_and_id_map() @@ -1179,7 +1215,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self._init_joints() self.attachments: Dict[Object, Attachment] = {} - self.saved_states: Dict[int, ObjectState] = {} if not self.world.is_prospection_world: self._add_to_world_sync_obj_queue(self.path) @@ -1200,7 +1235,7 @@ def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, :return: The unique id of the object and the path of the file that was loaded. """ - path = self.sync_cache_dir(path, ignore_cached_files) + path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) try: obj_id = self.world.load_description_and_get_object_id(path, Pose(self.get_position_as_list(), @@ -1213,42 +1248,12 @@ def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, os.remove(path) raise (e) - def sync_cache_dir(self, path: str, ignore_cached_files: bool) -> str: + def _update_object_description_from_file(self, path: str) -> None: """ - Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. + Updates the object description from the given file path. + :param path: The path to the file from which the object description should be updated. """ - path_object = pathlib.Path(path) - extension = path_object.suffix - - path = _look_for_file_in_data_dir(path, path_object) - - _create_cache_dir_if_not_exists() - - # if file is not yet cached preprocess the description file and save it in the cache directory. - if not _is_cached(path) or ignore_cached_files: - if extension == ".obj": - path = ObjectDescription.generate_from_obj_file(path) - elif extension == ".stl": - path = ObjectDescription.generate_from_stl_file(path) - elif extension == ObjectDescription.file_extension: - description_string = ObjectDescription.preprocess_file(path) - path = World.cache_dir + path_object.name - with open(path, mode="w") as f: - f.write(description_string) - else: # Using the description from the parameter server - description_string = ObjectDescription.preprocess_from_parameter_server(path) - path = f"{World.cache_dir}{self.name}{ObjectDescription.file_extension}" - with open(path, mode="w") as f: - f.write(description_string) - # save correct path in case the file is already in the cache directory - elif extension == ".obj" or extension == ".stl": - path = World.cache_dir + path_object.stem + ObjectDescription.file_extension - elif extension == ObjectDescription.file_extension: - path = World.cache_dir + path_object.name - else: - path = World.cache_dir + self.name + ObjectDescription.file_extension - - return path + self.description.update_description_from_file(path) def _init_joint_name_and_id_map(self) -> None: """ @@ -1264,7 +1269,7 @@ def _init_link_name_and_id_map(self) -> None: """ n_links = len(self.link_names) self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) - self.link_name_to_id[self.object_description.get_root()] = -1 + self.link_name_to_id[self.description.get_root()] = -1 self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) def _init_links_and_update_transforms(self) -> None: @@ -1273,10 +1278,10 @@ def _init_links_and_update_transforms(self) -> None: corresponding link objects. """ self.links = {} - for link_description in self.object_description.links: + for link_description in self.description.links: link_name = link_description.name link_id = self.link_name_to_id[link_name] - if link_name == self.object_description.get_root(): + if link_name == self.description.get_root(): self.links[link_name] = RootLink(self) else: self.links[link_name] = Link(link_id, link_description, self) @@ -1289,7 +1294,7 @@ def _init_joints(self): corresponding joint objects """ self.joints = {} - for joint_description in self.object_description.joints: + for joint_description in self.description.joints: joint_name = joint_description.name joint_id = self.joint_name_to_id[joint_name] self.joints[joint_name] = Joint(joint_id, joint_description, self) @@ -1300,7 +1305,8 @@ def _add_to_world_sync_obj_queue(self, path: str) -> None: :param path: The path of the URDF file of this object. """ self.world.world_sync.add_obj_queue.put( - [self.name, self.obj_type, path, self.get_position_as_list(), self.get_orientation_as_list(), + [self.name, self.obj_type, path, type(self.description), self.get_position_as_list(), + self.get_orientation_as_list(), self.world.prospection_world, self.color, self]) @property @@ -1308,14 +1314,14 @@ def link_names(self) -> List[str]: """ :return: The name of each link as a list. """ - return [link.name for link in self.object_description.links] + return [link.name for link in self.description.links] @property def joint_names(self) -> List[str]: """ :return: The name of each joint as a list. """ - return [joint.name for joint in self.object_description.joints] + return [joint.name for joint in self.description.joints] @property def base_origin_shift(self) -> np.ndarray: @@ -1326,7 +1332,7 @@ def base_origin_shift(self) -> np.ndarray: return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) def __repr__(self): - skip_attr = ["links", "joints", "object_description", "attachments"] + skip_attr = ["links", "joints", "description", "attachments"] return self.__class__.__qualname__ + f"(" + ', \n'.join( [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" @@ -1351,7 +1357,7 @@ def reset(self, remove_saved_states=True) -> None: self.reset_all_joints_positions() self.set_pose(self.original_pose) if remove_saved_states: - self.saved_states = {} + self.remove_saved_states() def attach(self, child_object: Object, @@ -1506,7 +1512,7 @@ def save_state(self, state_id) -> None: """ self.save_links_states(state_id) self.save_joints_states(state_id) - self.saved_states[state_id] = ObjectState(state_id, self.attachments.copy()) + super().save_state(state_id) def save_links_states(self, state_id: int) -> None: """ @@ -1524,6 +1530,50 @@ def save_joints_states(self, state_id: int) -> None: for joint in self.joints.values(): joint.save_state(state_id) + @property + def current_state(self) -> ObjectState: + return ObjectState(self.attachments.copy(), self.link_states, self.joint_states) + + @current_state.setter + def current_state(self, state: ObjectState) -> None: + self.attachments = state.attachments + self.link_states = state.link_states + self.joint_states = state.joint_states + + @property + def link_states(self) -> Dict[int, LinkState]: + """ + Returns the current state of all links of this object. + :return: A dictionary with the link id as key and the current state of the link as value. + """ + return {link.id: link.current_state for link in self.links.values()} + + @link_states.setter + def link_states(self, link_states: Dict[int, LinkState]) -> None: + """ + Sets the current state of all links of this object. + :param link_states: A dictionary with the link id as key and the current state of the link as value. + """ + for link in self.links.values(): + link.current_state = link_states[link.id] + + @property + def joint_states(self) -> Dict[int, JointState]: + """ + Returns the current state of all joints of this object. + :return: A dictionary with the joint id as key and the current state of the joint as value. + """ + return {joint.id: joint.current_state for joint in self.joints.values()} + + @joint_states.setter + def joint_states(self, joint_states: Dict[int, JointState]) -> None: + """ + Sets the current state of all joints of this object. + :param joint_states: A dictionary with the joint id as key and the current state of the joint as value. + """ + for joint in self.joints.values(): + joint.current_state = joint_states[joint.id] + def restore_state(self, state_id: int) -> None: """ Restores the state of this object by restoring the state of all links and attachments. @@ -1560,7 +1610,7 @@ def remove_saved_states(self) -> None: """ Removes all saved states of this object. """ - self.saved_states = {} + super().remove_saved_states() self.remove_links_saved_states() self.remove_joints_saved_states() @@ -1664,8 +1714,8 @@ def get_root_link_description(self) -> LinkDescription: Returns the root link of the URDF of this object. :return: The root link as defined in the URDF of this object. """ - root_link_name = self.object_description.get_root() - for link_description in self.object_description.links: + root_link_name = self.description.get_root() + for link_description in self.description.links: if link_description.name == root_link_name: return link_description @@ -1675,14 +1725,14 @@ def root_link(self) -> Link: Returns the root link of this object. :return: The root link of this object. """ - return self.links[self.object_description.get_root()] + return self.links[self.description.get_root()] def get_root_link_id(self) -> int: """ Returns the unique id of the root link of this object. :return: The unique id of the root link of this object. """ - return self.get_link_id(self.object_description.get_root()) + return self.get_link_id(self.description.get_root()) def get_link_id(self, link_name: str) -> int: """ @@ -1771,7 +1821,7 @@ def find_joint_above(self, link_name: str, joint_type: JointType) -> str: :param joint_type: Joint type that should be searched for :return: Name of the first joint which has the given type """ - chain = self.object_description.get_chain(self.object_description.get_root(), link_name) + chain = self.description.get_chain(self.description.get_root(), link_name) reversed_chain = reversed(chain) container_joint = None for element in reversed_chain: @@ -1907,42 +1957,91 @@ def name(self) -> str: class ObjectDescription(EntityDescription): + MESH_EXTENSIONS: Tuple[str] = (".obj", ".stl") - file_extension: str + def __init__(self, path: Optional[str] = None): + if path: + self.update_description_from_file(path) + else: + self._parsed_description = None - def __init__(self, path: Any): - self.parsed_description = self.from_description_file(path) + def update_description_from_file(self, path: str) -> None: + """ + Updates the description of this object from the file at the given path. + :param path: The path of the file to update from. + """ + self._parsed_description = self.load_description(path) - @abstractmethod - def from_description_file(self, path: str) -> Any: + @property + def parsed_description(self) -> Any: """ Return the object parsed from the description file. - :path: The path of the description file. """ - pass + return self._parsed_description + + @parsed_description.setter + def parsed_description(self, parsed_description: Any): + """ + Return the object parsed from the description file. + :param parsed_description: The parsed description object. + """ + self._parsed_description = parsed_description - @classmethod @abstractmethod - def generate_from_obj_file(cls, path: str) -> str: + def load_description(self, path: str) -> Any: """ - Generates a description file from an .obj file and returns the path of the generated file. - :param path: The path to the .obj file. - :return: The path of the generated description file. + Loads the description from the file at the given path. + :param path: The path to the source file, if only a filename is provided then the resources directories will be + searched. + """ + pass + + def generate_description_from_file(self, path: str, extension: str) -> str: + """ + Generates and preprocesses the description from the file at the given path and returns the preprocessed + description as a string. + :param path: The path of the file to preprocess. + :param extension: The file extension of the file to preprocess. + :return: The processed description string. + """ + + if extension in self.MESH_EXTENSIONS: + description_string = self.generate_from_mesh_file(path) + elif extension == self.file_extension: + description_string = self.generate_from_description_file(path) + else: + # Using the description from the parameter server + description_string = self.generate_from_parameter_server(path) + + return description_string + + def get_file_name(self, path_object: pathlib.Path, extension: str) -> str: """ + Returns the file name of the description file. + """ + if extension in self.MESH_EXTENSIONS: + file_name = path_object.stem + self.file_extension + elif extension == self.file_extension: + file_name = path_object.name + else: + file_name = self.name + self.file_extension + + return file_name @classmethod @abstractmethod - def generate_from_stl_file(cls, path: str) -> str: + def generate_from_mesh_file(cls, path: str) -> str: """ - Generates a description file from an .stl file and returns the path of the generated file. - :param path: The path to the .stl file. + Generates a description file from one of the mesh types defined in the MESH_EXTENSIONS and + returns the path of the generated file. + :param path: The path to the .obj file. :return: The path of the generated description file. """ pass @classmethod @abstractmethod - def preprocess_file(cls, path: str) -> str: + def generate_from_description_file(cls, path: str) -> str: """ Preprocesses the given file and returns the preprocessed description string. :param path: The path of the file to preprocess. @@ -1952,7 +2051,7 @@ def preprocess_file(cls, path: str) -> str: @classmethod @abstractmethod - def preprocess_from_parameter_server(cls, name: str) -> str: + def generate_from_parameter_server(cls, name: str) -> str: """ Preprocesses the description from the ROS parameter server and returns the preprocessed description string. :param name: The name of the description on the parameter server. @@ -1990,6 +2089,14 @@ def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: """ pass + @property + @abstractmethod + def file_extension(self) -> str: + """ + :return: The file extension of the description file. + """ + pass + class LinkDescription(EntityDescription): """ @@ -2155,6 +2262,7 @@ class AbstractConstraint: Represents an abstract constraint concept, this could be used to create joints for example or any kind of constraint between two links in the world. """ + def __init__(self, parent_link: Link, child_link: Link, @@ -2415,7 +2523,7 @@ class Link(ObjectEntity, LinkDescription, ABC): Represents a link of an Object in the World. """ - def __init__(self, _id: int, obj: Object, parsed_link_description: Any): + def __init__(self, _id: int, parsed_link_description: Any, obj: Object): ObjectEntity.__init__(self, _id, obj) LinkDescription.__init__(self, parsed_link_description) self.local_transformer: LocalTransformer = LocalTransformer() @@ -2596,7 +2704,7 @@ class RootLink(Link, ABC): """ def __init__(self, obj: Object): - super().__init__(obj.get_root_link_id(), obj, obj.get_root_link_description()) + super().__init__(obj.get_root_link_id(), obj.get_root_link_description(), obj) @property def tf_frame(self) -> str: @@ -2715,15 +2823,72 @@ def __del__(self) -> None: class CacheManager: + cache_dir: str = World.cache_dir + """ + The directory where the cached files are stored. + """ + + mesh_extensions: List[str] = [".obj", ".stl"] + """ + The file extensions of mesh files. + """ + + def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, + object_description: ObjectDescription) -> str: + """ + Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. + """ + path_object = pathlib.Path(path) + extension = path_object.suffix + + path = self.look_for_file_in_data_dir(path, path_object) + + self.create_cache_dir_if_not_exists() + + # save correct path in case the file is already in the cache directory + cache_path = self.cache_dir + object_description.get_file_name(path_object, extension) + + # if file is not yet cached preprocess the description file and save it in the cache directory. + if not self.is_cached(path, object_description) or ignore_cached_files: + self.generate_description_and_write_to_cache(path, extension, cache_path, object_description) + + return cache_path + + def generate_description_and_write_to_cache(self, path: str, extension: str, cache_path: str, + object_description: ObjectDescription) -> None: + """ + Generates the description from the file at the given path and writes it to the cache directory. + :param path: The path of the file to preprocess. + :param extension: The file extension of the file to preprocess. + :param cache_path: The path of the file in the cache directory. + :param object_description: The object description of the file. + """ + description_string = object_description.generate_description_from_file(path, extension) + self.write_to_cache(description_string, cache_path) + @staticmethod - def _look_for_file_in_data_dir(path: str, path_object: pathlib.Path) -> str: + def write_to_cache(description_string: str, cache_path: str) -> None: + """ + Writes the description string to the cache directory. + :param description_string: The description string to write to the cache directory. + :param cache_path: The path of the file in the cache directory. + """ + with open(cache_path, "w") as file: + file.write(description_string) + + @staticmethod + def look_for_file_in_data_dir(path: str, path_object: pathlib.Path) -> str: """ Looks for a file in the data directory of the World. If the file is not found in the data directory, this method raises a FileNotFoundError. + :param path: The path of the file to look for. + :param path_object: The pathlib object of the file to look for. """ if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): for data_dir in World.data_directory: - path = get_path_from_data_dir(path, data_dir) + for file in os.listdir(data_dir): + if file == path: + return data_dir + f"/{path}" if path: break @@ -2733,44 +2898,41 @@ def _look_for_file_in_data_dir(path: str, path_object: pathlib.Path) -> str: return path - @staticmethod - def get_path_from_data_dir(file_name: str, data_directory: str) -> str: - """ - Returns the full path for a given file name in the given directory. If there is no file with the given filename - this method returns None. - - :param file_name: The filename of the searched file. - :param data_directory: The directory in which to search for the file. - :return: The full path in the filesystem or None if there is no file with the filename in the directory - """ - for file in os.listdir(data_directory): - if file == file_name: - return data_directory + f"/{file_name}" - - @staticmethod - def _create_cache_dir_if_not_exists(): + def create_cache_dir_if_not_exists(self): """ Creates the cache directory if it does not exist. """ - if not pathlib.Path(World.cache_dir).exists(): - os.mkdir(World.cache_dir) + if not pathlib.Path(self.cache_dir).exists(): + os.mkdir(self.cache_dir) - @staticmethod - def _is_cached(path) -> bool: + def is_cached(self, path: str, object_description: ObjectDescription) -> bool: """ Checks if the file in the given path is already cached or if there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from the parameter server is used. + :param path: The path of the file to check. + :param object_description: The object description of the file. :return: True if there already exists a cached file, False in any other case. """ + return True if self.check_with_extension(path) else self.check_without_extension(path, object_description) + + def check_with_extension(self, path: str) -> bool: + """ + Checks if the file in the given ath exists in the cache directory including file extension. + :param path: The path of the file to check. + """ file_name = pathlib.Path(path).name - full_path = pathlib.Path(World.cache_dir + file_name) - if full_path.exists(): - return True - # Returns filename without the filetype, e.g. returns "test" for "test.txt" + full_path = pathlib.Path(self.cache_dir + file_name) + return full_path.exists() + + def check_without_extension(self, path: str, object_description: ObjectDescription) -> bool: + """ + Checks if the file in the given path exists in the cache directory without file extension, + the extension is added after the file name manually in this case. + :param path: The path of the file to check. + :param object_description: The object description of the file. + """ file_stem = pathlib.Path(path).stem - full_path = pathlib.Path(World.cache_dir + file_stem + ObjectDescription.file_extension) - if full_path.exists(): - return True - return False + full_path = pathlib.Path(self.cache_dir + file_stem + object_description.file_extension) + return full_path.exists() diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index e344dcf5e..e7b0a8692 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -326,13 +326,15 @@ class State(ABC): @dataclass class WorldState(State): - state_id: int + simulator_state_id: int + object_states: Dict[int, 'ObjectState'] @dataclass class ObjectState(State): - state_id: int attachments: Dict['Object', 'Attachment'] + link_states: Dict[int, 'LinkState'] + joint_states: Dict[int, 'JointState'] @dataclass From d2b9f9759cda6a572dab2c96ed5ee3ea1d2e1e24 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 13 Feb 2024 16:54:06 +0100 Subject: [PATCH 48/69] [AbstractWorld] Fixing tests for Description Abstraction --- src/pycram/bullet_world.py | 25 ++- src/pycram/urdf_interface.py | 19 +- src/pycram/world.py | 379 ++++++++++++++++++--------------- test/bullet_world_testcase.py | 14 +- test/test_action_designator.py | 4 +- test/test_bullet_world.py | 13 +- 6 files changed, 257 insertions(+), 197 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index a4317cce4..8556035d7 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -10,10 +10,15 @@ import rosgraph import rospy -from .enums import JointType, ObjectType, WorldMode +from .enums import ObjectType, WorldMode from .pose import Pose -from .world import World, Object, Link, Constraint +from .world import World, Object, Constraint from .world_dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape +from .urdf_interface import ObjectDescription + +Link = ObjectDescription.Link +RootLink = ObjectDescription.RootLink +Joint = ObjectDescription.Joint class BulletWorld(World): @@ -23,6 +28,8 @@ class is the main interface to the Bullet Physics Engine and should be used to s manipulate the Bullet World. """ + extension: str = ObjectDescription.get_file_extension() + # Check is for sphinx autoAPI to be able to work in a CI workflow if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): rospy.init_node('pycram') @@ -53,7 +60,8 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self.set_gravity([0, 0, -9.8]) if not is_prospection_world: - plane = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) + plane = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, ObjectDescription, + world=self) def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: return p.loadURDF(path, @@ -71,10 +79,10 @@ def add_constraint(self, constraint: Constraint) -> int: constraint.child_link_id, constraint.type.value, constraint.axis_as_list, - constraint.position_wrt_parent_as_list(), - constraint.position_wrt_child_as_list(), - constraint.orientation_wrt_parent_as_list(), - constraint.orientation_wrt_child_as_list(), + constraint.position_wrt_parent_as_list, + constraint.position_wrt_child_as_list, + constraint.orientation_wrt_parent_as_list, + constraint.orientation_wrt_child_as_list, physicsClientId=self.id) return constraint_id @@ -85,7 +93,8 @@ def get_joint_position(self, obj: Object, joint_name: str) -> float: return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.id)[0] def get_link_pose(self, link: Link) -> Pose: - return Pose(*p.getLinkState(link.object_id, link.id, physicsClientId=self.id)[4:6]) + bullet_link_state = p.getLinkState(link.object_id, link.id, physicsClientId=self.id)[4:6] + return Pose(*bullet_link_state) def perform_collision_detection(self) -> None: p.performCollisionDetection(physicsClientId=self.id) diff --git a/src/pycram/urdf_interface.py b/src/pycram/urdf_interface.py index ce82bde9a..357dbf8e9 100644 --- a/src/pycram/urdf_interface.py +++ b/src/pycram/urdf_interface.py @@ -12,7 +12,7 @@ from pycram.enums import JointType, Shape from pycram.pose import Pose -from pycram.world import JointDescription as AbstractJointDescription, Joint as AbstractJoint, \ +from pycram.world import JointDescription as AbstractJointDescription, \ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription from pycram.world_dataclasses import Color @@ -140,15 +140,20 @@ def friction(self) -> float: return self.parsed_description.dynamics.friction -class Joint(AbstractJoint, JointDescription): - ... - - class ObjectDescription(AbstractObjectDescription): """ A class that represents an object description of an object. """ + class Link(AbstractObjectDescription.Link, LinkDescription): + ... + + class RootLink(AbstractObjectDescription.RootLink, Link): + ... + + class Joint(AbstractObjectDescription.Joint, JointDescription): + ... + def load_description(self, path) -> URDF: with open(path, 'r') as file: return URDF.from_xml_string(file.read()) @@ -313,8 +318,8 @@ def fix_link_attributes(urdf_string: str) -> str: return ElementTree.tostring(tree.getroot(), encoding='unicode') - @property - def file_extension(self) -> str: + @staticmethod + def get_file_extension() -> str: """ :return: The file extension of the URDF file. """ diff --git a/src/pycram/world.py b/src/pycram/world.py index e01ad2955..a93d4d8cf 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -214,7 +214,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, :param ignore_cached_files: If the cached files should be ignored. :param obj: The object to be added to the cache directory. """ - return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, obj.description) + return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, obj.description, obj.name) @property def simulation_time_step(self): @@ -350,7 +350,7 @@ def get_link_pose(self, link: Link) -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. - :param link: The link as a Link object. + :param link: The link as a AbstractLink object. :return: The pose of the link as a Pose object. """ pass @@ -1166,7 +1166,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, color: Optional[Color] = Color(), ignore_cached_files: Optional[bool] = False): """ - The constructor loads the urdf file into the given World, if no World is specified the + The constructor loads the description file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. The rgba_color parameter is only used when loading .stl or .obj files, for URDFs :func:`~Object.set_color` can be used. @@ -1183,22 +1183,24 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ + super().__init__(-1, world) + if pose is None: pose = Pose() self.name: str = name self.obj_type: ObjectType = obj_type self.color: Color = color + self.description = description() + self.cache_manager = self.world.cache_manager self.local_transformer = LocalTransformer() self.original_pose = self.local_transformer.transform_pose(pose, "map") self._current_pose = self.original_pose - _id, self.path = self._load_object_and_get_id(path, ignore_cached_files) - - self.description = description(self.path) + self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) - super().__init__(_id, world) + self.description.update_description_from_file(self.path) self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") + f"{self.name}_{self.id}") @@ -1267,8 +1269,8 @@ def _init_link_name_and_id_map(self) -> None: """ Creates a dictionary which maps the link names to their unique ids and vice versa. """ - n_links = len(self.link_names) - self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) + n_links = len(self.link_names_without_root) + self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names_without_root, range(n_links))) self.link_name_to_id[self.description.get_root()] = -1 self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) @@ -1282,9 +1284,9 @@ def _init_links_and_update_transforms(self) -> None: link_name = link_description.name link_id = self.link_name_to_id[link_name] if link_name == self.description.get_root(): - self.links[link_name] = RootLink(self) + self.links[link_name] = self.description.RootLink(self) else: - self.links[link_name] = Link(link_id, link_description, self) + self.links[link_name] = self.description.Link(link_id, link_description, self) self.update_link_transforms() @@ -1297,7 +1299,7 @@ def _init_joints(self): for joint_description in self.description.joints: joint_name = joint_description.name joint_id = self.joint_name_to_id[joint_name] - self.joints[joint_name] = Joint(joint_id, joint_description, self) + self.joints[joint_name] = self.description.Joint(joint_id, joint_description, self) def _add_to_world_sync_obj_queue(self, path: str) -> None: """ @@ -1309,6 +1311,13 @@ def _add_to_world_sync_obj_queue(self, path: str) -> None: self.get_orientation_as_list(), self.world.prospection_world, self.color, self]) + @property + def link_names_without_root(self): + """ + :return: The name of each link except the root link as a list. + """ + return list(filter(lambda x: x != self.root_link_name, self.link_names)) + @property def link_names(self) -> List[str]: """ @@ -1316,6 +1325,13 @@ def link_names(self) -> List[str]: """ return [link.name for link in self.description.links] + @property + def number_of_links(self) -> int: + """ + :return: The number of links of this object. + """ + return len(self.description.links) + @property def joint_names(self) -> List[str]: """ @@ -1323,6 +1339,13 @@ def joint_names(self) -> List[str]: """ return [joint.name for joint in self.description.joints] + @property + def number_of_joints(self) -> int: + """ + :return: The number of joints of this object. + """ + return len(self.description.joints) + @property def base_origin_shift(self) -> np.ndarray: """ @@ -1727,6 +1750,14 @@ def root_link(self) -> Link: """ return self.links[self.description.get_root()] + @property + def root_link_name(self) -> str: + """ + Returns the name of the root link of this object. + :return: The name of the root link of this object. + """ + return self.description.get_root() + def get_root_link_id(self) -> int: """ Returns the unique id of the root link of this object. @@ -1817,7 +1848,7 @@ def find_joint_above(self, link_name: str, joint_type: JointType) -> str: """ Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. - :param link_name: Link name above which the joint should be found + :param link_name: AbstractLink name above which the joint should be found :param joint_type: Joint type that should be searched for :return: Name of the first joint which has the given type """ @@ -1956,148 +1987,6 @@ def name(self) -> str: pass -class ObjectDescription(EntityDescription): - MESH_EXTENSIONS: Tuple[str] = (".obj", ".stl") - - def __init__(self, path: Optional[str] = None): - if path: - self.update_description_from_file(path) - else: - self._parsed_description = None - - def update_description_from_file(self, path: str) -> None: - """ - Updates the description of this object from the file at the given path. - :param path: The path of the file to update from. - """ - self._parsed_description = self.load_description(path) - - @property - def parsed_description(self) -> Any: - """ - Return the object parsed from the description file. - """ - return self._parsed_description - - @parsed_description.setter - def parsed_description(self, parsed_description: Any): - """ - Return the object parsed from the description file. - :param parsed_description: The parsed description object. - """ - self._parsed_description = parsed_description - - @abstractmethod - def load_description(self, path: str) -> Any: - """ - Loads the description from the file at the given path. - :param path: The path to the source file, if only a filename is provided then the resources directories will be - searched. - """ - pass - - def generate_description_from_file(self, path: str, extension: str) -> str: - """ - Generates and preprocesses the description from the file at the given path and returns the preprocessed - description as a string. - :param path: The path of the file to preprocess. - :param extension: The file extension of the file to preprocess. - :return: The processed description string. - """ - - if extension in self.MESH_EXTENSIONS: - description_string = self.generate_from_mesh_file(path) - elif extension == self.file_extension: - description_string = self.generate_from_description_file(path) - else: - # Using the description from the parameter server - description_string = self.generate_from_parameter_server(path) - - return description_string - - def get_file_name(self, path_object: pathlib.Path, extension: str) -> str: - """ - Returns the file name of the description file. - """ - if extension in self.MESH_EXTENSIONS: - file_name = path_object.stem + self.file_extension - elif extension == self.file_extension: - file_name = path_object.name - else: - file_name = self.name + self.file_extension - - return file_name - - @classmethod - @abstractmethod - def generate_from_mesh_file(cls, path: str) -> str: - """ - Generates a description file from one of the mesh types defined in the MESH_EXTENSIONS and - returns the path of the generated file. - :param path: The path to the .obj file. - :return: The path of the generated description file. - """ - pass - - @classmethod - @abstractmethod - def generate_from_description_file(cls, path: str) -> str: - """ - Preprocesses the given file and returns the preprocessed description string. - :param path: The path of the file to preprocess. - :return: The preprocessed description string. - """ - pass - - @classmethod - @abstractmethod - def generate_from_parameter_server(cls, name: str) -> str: - """ - Preprocesses the description from the ROS parameter server and returns the preprocessed description string. - :param name: The name of the description on the parameter server. - :return: The preprocessed description string. - """ - pass - - @property - @abstractmethod - def links(self) -> List[LinkDescription]: - """ - :return: A list of links descriptions of this object. - """ - pass - - @property - @abstractmethod - def joints(self) -> List[JointDescription]: - """ - :return: A list of joints descriptions of this object. - """ - pass - - @abstractmethod - def get_root(self) -> str: - """ - :return: the name of the root link of this object. - """ - pass - - @abstractmethod - def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: - """ - :return: the chain of links from 'start_link_name' to 'end_link_name'. - """ - pass - - @property - @abstractmethod - def file_extension(self) -> str: - """ - :return: The file extension of the description file. - """ - pass - - class LinkDescription(EntityDescription): """ A class that represents a link description of an object. @@ -2404,7 +2293,7 @@ def axis_as_list(self) -> List[float]: return [self.axis.x, self.axis.y, self.axis.z] -class Joint(AbstractConstraint, ObjectEntity, JointDescription, ABC): +class Joint(ObjectEntity, JointDescription, ABC): """ Represents a joint of an Object in the World. """ @@ -2412,13 +2301,8 @@ class Joint(AbstractConstraint, ObjectEntity, JointDescription, ABC): def __init__(self, _id: int, joint_description: JointDescription, obj: Object): - AbstractConstraint.__init__(self, - self.parent_link, - self.child_link, - joint_description.type, - self.parent_link.get_transform_to_link(self.child_link), - Transform(frame=self.child_link.tf_frame)) ObjectEntity.__init__(self, _id, obj) + JointDescription.__init__(self, joint_description.parsed_description) self._update_position() @property @@ -2447,7 +2331,7 @@ def _update_position(self) -> None: def parent_link(self) -> Link: """ Returns the parent link of this joint. - :return: The parent link as a Link object. + :return: The parent link as a AbstractLink object. """ return self.object.links[self.parent_link_name] @@ -2455,7 +2339,7 @@ def parent_link(self) -> Link: def child_link(self) -> Link: """ Returns the child link of this joint. - :return: The child link as a Link object. + :return: The child link as a AbstractLink object. """ return self.object.links[self.child_link_name] @@ -2523,9 +2407,9 @@ class Link(ObjectEntity, LinkDescription, ABC): Represents a link of an Object in the World. """ - def __init__(self, _id: int, parsed_link_description: Any, obj: Object): + def __init__(self, _id: int, link_description: LinkDescription, obj: Object): ObjectEntity.__init__(self, _id, obj) - LinkDescription.__init__(self, parsed_link_description) + LinkDescription.__init__(self, link_description.parsed_description) self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} self._update_pose() @@ -2700,7 +2584,7 @@ def tf_frame(self) -> str: class RootLink(Link, ABC): """ Represents the root link of an Object in the World. - It differs from the normal Link class in that the pose ande the tf_frame is the same as that of the object. + It differs from the normal AbstractLink class in that the pose ande the tf_frame is the same as that of the object. """ def __init__(self, obj: Object): @@ -2717,6 +2601,161 @@ def _update_pose(self) -> None: self._current_pose = self.object.get_pose() +class ObjectDescription(EntityDescription): + MESH_EXTENSIONS: Tuple[str] = (".obj", ".stl") + + class Link(Link, ABC): + ... + + class RootLink(RootLink, ABC): + ... + + class Joint(Joint, ABC): + ... + + def __init__(self, path: Optional[str] = None): + if path: + self.update_description_from_file(path) + else: + self._parsed_description = None + + def update_description_from_file(self, path: str) -> None: + """ + Updates the description of this object from the file at the given path. + :param path: The path of the file to update from. + """ + self._parsed_description = self.load_description(path) + + @property + def parsed_description(self) -> Any: + """ + Return the object parsed from the description file. + """ + return self._parsed_description + + @parsed_description.setter + def parsed_description(self, parsed_description: Any): + """ + Return the object parsed from the description file. + :param parsed_description: The parsed description object. + """ + self._parsed_description = parsed_description + + @abstractmethod + def load_description(self, path: str) -> Any: + """ + Loads the description from the file at the given path. + :param path: The path to the source file, if only a filename is provided then the resources directories will be + searched. + """ + pass + + def generate_description_from_file(self, path: str, extension: str) -> str: + """ + Generates and preprocesses the description from the file at the given path and returns the preprocessed + description as a string. + :param path: The path of the file to preprocess. + :param extension: The file extension of the file to preprocess. + :return: The processed description string. + """ + + if extension in self.MESH_EXTENSIONS: + description_string = self.generate_from_mesh_file(path) + elif extension == self.get_file_extension(): + description_string = self.generate_from_description_file(path) + else: + # Using the description from the parameter server + description_string = self.generate_from_parameter_server(path) + + return description_string + + def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: str) -> str: + """ + Returns the file name of the description file. + :param path_object: The path object of the description file or the mesh file. + :param extension: The file extension of the description file or the mesh file. + :param object_name: The name of the object. + :return: The file name of the description file. + """ + if extension in self.MESH_EXTENSIONS: + file_name = path_object.stem + self.get_file_extension() + elif extension == self.get_file_extension(): + file_name = path_object.name + else: + file_name = object_name + self.get_file_extension() + + return file_name + + @classmethod + @abstractmethod + def generate_from_mesh_file(cls, path: str) -> str: + """ + Generates a description file from one of the mesh types defined in the MESH_EXTENSIONS and + returns the path of the generated file. + :param path: The path to the .obj file. + :return: The path of the generated description file. + """ + pass + + @classmethod + @abstractmethod + def generate_from_description_file(cls, path: str) -> str: + """ + Preprocesses the given file and returns the preprocessed description string. + :param path: The path of the file to preprocess. + :return: The preprocessed description string. + """ + pass + + @classmethod + @abstractmethod + def generate_from_parameter_server(cls, name: str) -> str: + """ + Preprocesses the description from the ROS parameter server and returns the preprocessed description string. + :param name: The name of the description on the parameter server. + :return: The preprocessed description string. + """ + pass + + @property + @abstractmethod + def links(self) -> List[LinkDescription]: + """ + :return: A list of links descriptions of this object. + """ + pass + + @property + @abstractmethod + def joints(self) -> List[JointDescription]: + """ + :return: A list of joints descriptions of this object. + """ + pass + + @abstractmethod + def get_root(self) -> str: + """ + :return: the name of the root link of this object. + """ + pass + + @abstractmethod + def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: + """ + :return: the chain of links from 'start_link_name' to 'end_link_name'. + """ + pass + + @staticmethod + @abstractmethod + def get_file_extension() -> str: + """ + :return: The file extension of the description file. + """ + pass + + class Attachment(AbstractConstraint): def __init__(self, parent_link: Link, @@ -2834,7 +2873,7 @@ class CacheManager: """ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - object_description: ObjectDescription) -> str: + object_description: ObjectDescription, object_name: str) -> str: """ Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. """ @@ -2846,7 +2885,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, self.create_cache_dir_if_not_exists() # save correct path in case the file is already in the cache directory - cache_path = self.cache_dir + object_description.get_file_name(path_object, extension) + cache_path = self.cache_dir + object_description.get_file_name(path_object, extension, object_name) # if file is not yet cached preprocess the description file and save it in the cache directory. if not self.is_cached(path, object_description) or ignore_cached_files: @@ -2934,5 +2973,5 @@ def check_without_extension(self, path: str, object_description: ObjectDescripti :param object_description: The object description of the file. """ file_stem = pathlib.Path(path).stem - full_path = pathlib.Path(self.cache_dir + file_stem + object_description.file_extension) + full_path = pathlib.Path(self.cache_dir + file_stem + object_description.get_file_extension()) return full_path.exists() diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 755dfbb9a..9b53f0cd4 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -5,19 +5,23 @@ from pycram.robot_descriptions import robot_description from pycram.process_module import ProcessModule from pycram.enums import ObjectType, WorldMode +from pycram.urdf_interface import ObjectDescription class BulletWorldTestCase(unittest.TestCase): world: BulletWorld + extension: str = ObjectDescription.get_file_extension() @classmethod def setUpClass(cls): - cls.world = BulletWorld(WorldMode.DIRECT) - cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf") - cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) + cls.world = BulletWorld(WorldMode.GUI) + cls.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + cls.robot = Object(robot_description.name, ObjectType.ROBOT, + robot_description.name + cls.extension, ObjectDescription) + cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension, ObjectDescription) + cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", + ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False def setUp(self): diff --git a/test/test_action_designator.py b/test/test_action_designator.py index d6867b6f5..4d8258f4b 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -43,6 +43,7 @@ def test_grip(self): self.assertEqual(description.ground().object_designator.name, "milk") def test_park_arms(self): + time.sleep(5) description = action_designator.ParkArmsAction([pycram.enums.Arms.BOTH]) self.assertEqual(description.ground().arm, pycram.enums.Arms.BOTH) with simulated_robot: @@ -119,7 +120,8 @@ def test_transport(self): action_designator.MoveTorsoAction([0.2]).resolve().perform() description.resolve().perform() self.assertEqual(description.ground().object_designator.name, "milk") - dist = np.linalg.norm(np.array(self.milk.get_pose().position_as_list()) - np.array([-1.35, 0.78, 0.95])) + milk_position = np.array(self.milk.get_pose().position_as_list()) + dist = np.linalg.norm(milk_position - np.array([-1.35, 0.78, 0.95])) self.assertTrue(dist < 0.01) def test_grasping(self): diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 42725316f..9542edf98 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -2,13 +2,14 @@ import unittest import rospkg -from geometry_msgs.msg import Point from bullet_world_testcase import BulletWorldTestCase from pycram.pose import Pose -from pycram.world import fix_missing_inertial, Object, ObjectType +from pycram.world import Object, ObjectType import xml.etree.ElementTree as ET -from pycram.enums import JointType +from pycram.urdf_interface import ObjectDescription + +fix_missing_inertial = ObjectDescription.fix_missing_inertial class BulletWorldTest(BulletWorldTestCase): @@ -29,13 +30,13 @@ def test_save_and_restore_state(self): robot_link = self.robot.root_link milk_link = self.milk.root_link cid = robot_link.constraint_ids[milk_link] - self.assertTrue(cid == self.robot.attachments[self.milk].constraint_id) + self.assertTrue(cid == self.robot.attachments[self.milk].id) self.world.remove_constraint(cid) self.world.restore_state(state_id) cid = robot_link.constraint_ids[milk_link] self.assertTrue(milk_link in robot_link.constraint_ids) self.assertTrue(self.milk in self.robot.attachments) - self.assertTrue(cid == self.robot.attachments[self.milk].constraint_id) + self.assertTrue(cid == self.robot.attachments[self.milk].id) def test_remove_object(self): time.sleep(2) @@ -43,7 +44,7 @@ def test_remove_object(self): self.assertTrue(milk_id in [obj.id for obj in self.world.objects]) self.world.remove_object(self.milk) self.assertTrue(milk_id not in [obj.id for obj in self.world.objects]) - BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) def test_get_joint_position(self): self.assertEqual(self.robot.get_joint_position("head_pan_joint"), 0.0) From 0d8094514ac6dc9ca1431557bd73c26654d01117 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 13 Feb 2024 21:45:58 +0100 Subject: [PATCH 49/69] [Abstract World] Fixing tests (test_detect) --- src/pycram/bullet_world.py | 5 ++--- src/pycram/world.py | 21 +++++++++++++++++---- test/bullet_world_testcase.py | 2 +- test/test_action_designator.py | 8 ++++---- 4 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 8556035d7..b026dd991 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -260,9 +260,8 @@ def get_images_for_target(self, near = 0.2 far = 100 - view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1], - physicsClientId=self.id) - projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far, physicsClientId=self.id) + view_matrix = p.computeViewMatrix(cam_pose.position_as_list(), target_pose.position_as_list(), [0, 0, 1]) + projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) return list(p.getCameraImage(size, size, view_matrix, projection_matrix, physicsClientId=self.id))[2:5] diff --git a/src/pycram/world.py b/src/pycram/world.py index a93d4d8cf..517464448 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -629,11 +629,12 @@ def save_state(self, state_id: Optional[int] = None) -> int: """ state_id = self.save_physics_simulator_state() self.save_objects_state(state_id) + self._current_state = WorldState(state_id, self.object_states) return super().save_state(state_id) @property def current_state(self) -> WorldState: - return WorldState(self.save_physics_simulator_state(), self.object_states) + return self._current_state @current_state.setter def current_state(self, state: WorldState) -> None: @@ -1500,10 +1501,9 @@ def reset_base_pose(self, pose: Pose): def update_pose(self): """ - Updates the current pose of this object from the world. + Updates the current pose of this object from the world, and updates the poses of all links. """ self._current_pose = self.world.get_object_pose(self) - self._update_all_joints_positions() self._update_all_links_poses() self.update_link_transforms() @@ -1808,7 +1808,9 @@ def set_joint_positions(self, joint_poses: dict) -> None: """ for joint_name, joint_position in joint_poses.items(): self.joints[joint_name].position = joint_position - self.update_pose() + # 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: @@ -2349,7 +2351,18 @@ def position(self) -> float: def reset_position(self, position: float) -> None: self.world.reset_joint_position(self.object, self.name, position) + self._current_position = position self._update_position() + # if self.name == "r_upper_arm_roll_joint": + # if position == -1.463: + # print("here") + # print(self.object.name) + # print(self.object.world.id) + # print(self.name) + # print("required", position) + # print("actual", self._current_position) + # print(position == self._current_position) + # print("===================================") def get_object_id(self) -> int: """ diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 9b53f0cd4..9a8f5a808 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -15,7 +15,7 @@ class BulletWorldTestCase(unittest.TestCase): @classmethod def setUpClass(cls): - cls.world = BulletWorld(WorldMode.GUI) + cls.world = BulletWorld(WorldMode.DIRECT) cls.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + cls.extension, ObjectDescription) diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 4d8258f4b..66f56b2f6 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -43,19 +43,19 @@ def test_grip(self): self.assertEqual(description.ground().object_designator.name, "milk") def test_park_arms(self): - time.sleep(5) description = action_designator.ParkArmsAction([pycram.enums.Arms.BOTH]) self.assertEqual(description.ground().arm, pycram.enums.Arms.BOTH) with simulated_robot: description.resolve().perform() for joint, pose in robot_description.get_static_joint_chain("right", "park").items(): - self.assertEqual(self.world.robot.get_joint_position(joint), pose) + joint_position = self.world.robot.get_joint_position(joint) + self.assertEqual(joint_position, pose) for joint, pose in robot_description.get_static_joint_chain("left", "park").items(): self.assertEqual(self.world.robot.get_joint_position(joint), pose) def test_navigate(self): - description = action_designator.NavigateAction([Pose([0, 0, 0], [0, 0, 0, 1])]) - self.assertEqual(description.ground().target_location, Pose([0, 0, 0], [0, 0, 0, 1])) + description = action_designator.NavigateAction([Pose([1, 0, 0], [0, 0, 0, 1])]) + self.assertEqual(description.ground().target_location, Pose([1, 0, 0], [0, 0, 0, 1])) def test_pick_up(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) From 17018f930fee44b1197f09def92f528590c3263c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 13 Feb 2024 21:46:47 +0100 Subject: [PATCH 50/69] [Abstract World] Fixing tests (test_detect) --- src/pycram/world.py | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 517464448..ebe147520 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -2351,18 +2351,7 @@ def position(self) -> float: def reset_position(self, position: float) -> None: self.world.reset_joint_position(self.object, self.name, position) - self._current_position = position self._update_position() - # if self.name == "r_upper_arm_roll_joint": - # if position == -1.463: - # print("here") - # print(self.object.name) - # print(self.object.world.id) - # print(self.name) - # print("required", position) - # print("actual", self._current_position) - # print(position == self._current_position) - # print("===================================") def get_object_id(self) -> int: """ From 3fa6880af1430729eb57f901b4e747f7627a3576 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Feb 2024 16:55:52 +0100 Subject: [PATCH 51/69] [Abstract World] Tests are running. --- src/pycram/bullet_world.py | 21 +++++++++-- src/pycram/urdf_interface.py | 18 +++++++++ src/pycram/world.py | 58 ++++++++++++++++++++--------- src/pycram/world_reasoning.py | 2 +- test/test_action_designator.py | 1 - test/test_bullet_world_reasoning.py | 4 +- test/test_failure_handling.py | 6 ++- test/test_object.py | 6 +-- 8 files changed, 89 insertions(+), 27 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index b026dd991..0875edcd2 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -92,9 +92,23 @@ def remove_constraint(self, constraint_id): def get_joint_position(self, obj: Object, joint_name: str) -> float: return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.id)[0] + 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_link_pose(self, link: Link) -> Pose: - bullet_link_state = p.getLinkState(link.object_id, link.id, physicsClientId=self.id)[4:6] - return Pose(*bullet_link_state) + bullet_link_state = p.getLinkState(link.object_id, link.id, physicsClientId=self.id) + return Pose(*bullet_link_state[4:6]) + + def get_object_link_names(self, obj: Object) -> List[str]: + num_links = self.get_object_number_of_links(obj) + return [p.getJointInfo(obj.id, i, physicsClientId=self.id)[12].decode('utf-8') + for i in range(num_links)] + + def get_object_number_of_links(self, obj: Object) -> int: + return p.getNumJoints(obj.id, physicsClientId=self.id) + + get_object_number_of_joints = get_object_number_of_links def perform_collision_detection(self) -> None: p.performCollisionDetection(physicsClientId=self.id) @@ -228,7 +242,8 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L physicsClientId=self.id) def create_visual_shape(self, visual_shape: VisualShape) -> int: - return p.createVisualShape(visual_shape.visual_geometry_type.value, rgbaColor=visual_shape.rgba_color, + return p.createVisualShape(visual_shape.visual_geometry_type.value, + rgbaColor=visual_shape.rgba_color.get_rgba(), visualFramePosition=visual_shape.visual_frame_position, physicsClientId=self.id, **visual_shape.shape_data()) diff --git a/src/pycram/urdf_interface.py b/src/pycram/urdf_interface.py index 357dbf8e9..aa755d1c6 100644 --- a/src/pycram/urdf_interface.py +++ b/src/pycram/urdf_interface.py @@ -208,6 +208,15 @@ def generate_from_parameter_server(self, name: str) -> str: urdf_string = rospy.get_param(name) return self.correct_urdf_string(urdf_string) + def get_link_by_name(self, link_name: str) -> LinkDescription: + """ + :return: The link description with the given name. + """ + for link in self.links: + if link.name == link_name: + return link + raise ValueError(f"Link with name {link_name} not found") + @property def links(self) -> List[LinkDescription]: """ @@ -215,6 +224,15 @@ def links(self) -> List[LinkDescription]: """ return [LinkDescription(link) for link in self.parsed_description.links] + def get_joint_by_name(self, joint_name: str) -> JointDescription: + """ + :return: The joint description with the given name. + """ + for joint in self.joints: + if joint.name == joint_name: + return joint + raise ValueError(f"Joint with name {joint_name} not found") + @property def joints(self) -> List[JointDescription]: """ diff --git a/src/pycram/world.py b/src/pycram/world.py index ebe147520..fbc6ffd4f 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -345,6 +345,16 @@ def get_joint_position(self, obj: Object, joint_name: str) -> float: """ pass + @abstractmethod + def get_object_joint_names(self, obj: Object) -> List[str]: + """ + Returns the names of all joints of this object. + + :param obj: The object. + :return: A list of joint names. + """ + pass + @abstractmethod def get_link_pose(self, link: Link) -> Pose: """ @@ -355,6 +365,15 @@ def get_link_pose(self, link: Link) -> Pose: """ pass + @abstractmethod + def get_object_link_names(self, obj: Object) -> List[str]: + """ + Returns the names of all links of this object. + :param obj: The object. + :return: A list of link names. + """ + pass + def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: """ Simulates Physics in the World for a given amount of seconds. Usually this simulation is faster than real @@ -1270,8 +1289,8 @@ def _init_link_name_and_id_map(self) -> None: """ Creates a dictionary which maps the link names to their unique ids and vice versa. """ - n_links = len(self.link_names_without_root) - self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names_without_root, range(n_links))) + n_links = len(self.link_names) + self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) self.link_name_to_id[self.description.get_root()] = -1 self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) @@ -1281,9 +1300,8 @@ def _init_links_and_update_transforms(self) -> None: corresponding link objects. """ self.links = {} - for link_description in self.description.links: - link_name = link_description.name - link_id = self.link_name_to_id[link_name] + for link_name, link_id in self.link_name_to_id.items(): + link_description = self.description.get_link_by_name(link_name) if link_name == self.description.get_root(): self.links[link_name] = self.description.RootLink(self) else: @@ -1297,9 +1315,8 @@ def _init_joints(self): corresponding joint objects """ self.joints = {} - for joint_description in self.description.joints: - joint_name = joint_description.name - joint_id = self.joint_name_to_id[joint_name] + 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) def _add_to_world_sync_obj_queue(self, path: str) -> None: @@ -1312,19 +1329,12 @@ def _add_to_world_sync_obj_queue(self, path: str) -> None: self.get_orientation_as_list(), self.world.prospection_world, self.color, self]) - @property - def link_names_without_root(self): - """ - :return: The name of each link except the root link as a list. - """ - return list(filter(lambda x: x != self.root_link_name, self.link_names)) - @property def link_names(self) -> List[str]: """ :return: The name of each link as a list. """ - return [link.name for link in self.description.links] + return self.world.get_object_link_names(self) @property def number_of_links(self) -> int: @@ -1338,7 +1348,7 @@ def joint_names(self) -> List[str]: """ :return: The name of each joint as a list. """ - return [joint.name for joint in self.description.joints] + return self.world.get_object_joint_names(self) @property def number_of_joints(self) -> int: @@ -2727,6 +2737,13 @@ def links(self) -> List[LinkDescription]: """ pass + @abstractmethod + def get_link_by_name(self, link_name: str) -> LinkDescription: + """ + :return: The link description with the given name. + """ + pass + @property @abstractmethod def joints(self) -> List[JointDescription]: @@ -2735,6 +2752,13 @@ def joints(self) -> List[JointDescription]: """ pass + @abstractmethod + def get_joint_by_name(self, joint_name: str) -> JointDescription: + """ + :return: The joint description with the given name. + """ + pass + @abstractmethod def get_root(self) -> str: """ diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 80cfa0ae9..9c8f4f764 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -111,7 +111,7 @@ def visible( state_id = World.current_world.save_state() for obj in World.current_world.objects: - if obj == prospection_obj or World.robot and obj == prospection_robot: + if obj == prospection_obj or (World.robot and obj == prospection_robot): continue else: obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 66f56b2f6..37a5f13d4 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -1,4 +1,3 @@ -import time import unittest from pycram.designators import action_designator, object_designator from pycram.robot_descriptions import robot_description diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index c69b34093..2643eb40e 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -20,7 +20,9 @@ def test_visible(self): self.milk.set_pose(Pose([1.5, 0, 1.2])) self.robot.set_pose(Pose()) time.sleep(1) - self.assertTrue(btr.visible(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, + camera_frame = robot_description.get_camera_frame() + self.world.add_vis_axis(self.robot.links[camera_frame].pose) + self.assertTrue(btr.visible(self.milk, self.robot.links[camera_frame].pose, robot_description.front_facing_axis)) def test_occluding(self): diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index 7a97d308a..cc9e7fd5e 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -10,6 +10,9 @@ from pycram.plan_failures import PlanFailure from pycram.process_module import ProcessModule, simulated_robot from pycram.robot_descriptions import robot_description +from pycram.urdf_interface import ObjectDescription + +extension = ObjectDescription.get_file_extension() # start ik_and_description.launch @@ -30,7 +33,8 @@ class FailureHandlingTest(unittest.TestCase): @classmethod def setUpClass(cls): cls.world = BulletWorld(WorldMode.DIRECT) - cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf") + cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + extension, + ObjectDescription) ProcessModule.execution_delay = True def setUp(self): diff --git a/test/test_object.py b/test/test_object.py index 3391eddd6..ff9361eb5 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -39,7 +39,7 @@ def test_save_state(self): self.assertEqual(self.robot.saved_states[1].attachments, self.robot.attachments) self.assertTrue(self.milk in self.robot.saved_states[1].attachments) for link in self.robot.links.values(): - self.assertEqual(link.get_current_state(), link.saved_states[1]) + self.assertEqual(link.current_state, link.saved_states[1]) def test_restore_state(self): self.robot.attach(self.milk) @@ -52,7 +52,7 @@ def test_restore_state(self): self.milk.restore_state(1) self.assertTrue(self.milk in self.robot.attachments) for link in self.robot.links.values(): - curr_state = link.get_current_state() + curr_state = link.current_state saved_state = link.saved_states[1] self.assertEqual(curr_state, saved_state) - self.assertEqual(curr_state.constraint_ids, saved_state.constraint_ids) \ No newline at end of file + self.assertEqual(curr_state.constraint_ids, saved_state.constraint_ids) From 8433035742a414325ca4a53ca97d49c340b1be8f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Feb 2024 21:30:42 +0100 Subject: [PATCH 52/69] [Abstract World] Refactored File structure (Moved Classes to separate Files). Tests are running. --- demos/pycram_bullet_world_demo/demo.py | 32 +- src/pycram/bullet_world.py | 8 +- src/pycram/cache_manager.py | 123 + src/pycram/costmaps.py | 4 +- src/pycram/description.py | 669 ++++++ src/pycram/designator.py | 3 +- src/pycram/designators/motion_designator.py | 3 +- src/pycram/designators/object_designator.py | 3 +- src/pycram/external_interfaces/giskard.py | 3 +- src/pycram/external_interfaces/ik.py | 2 +- src/pycram/helper.py | 2 +- src/pycram/pose_generator_and_validator.py | 3 +- .../process_modules/pr2_process_modules.py | 3 +- src/pycram/urdf_interface.py | 2 +- src/pycram/world.py | 1978 +---------------- src/pycram/world_constraints.py | 257 +++ src/pycram/world_object.py | 822 +++++++ src/pycram/world_reasoning.py | 4 +- test/bullet_world_testcase.py | 3 +- test/test_bullet_world.py | 19 +- test/test_database_merger.py | 3 +- test/test_database_resolver.py | 2 +- test/test_jpt_resolver.py | 2 +- test/test_language.py | 1 + test/test_links.py | 9 + 25 files changed, 2021 insertions(+), 1939 deletions(-) create mode 100644 src/pycram/cache_manager.py create mode 100644 src/pycram/description.py create mode 100644 src/pycram/world_constraints.py create mode 100644 src/pycram/world_object.py diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index eabdd2ac2..24917d05d 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -1,22 +1,28 @@ +from pycram.bullet_world import BulletWorld from pycram.designators.action_designator import * from pycram.designators.location_designator import * from pycram.designators.object_designator import * +from pycram.enums import ObjectType from pycram.pose import Pose -from pycram.bullet_world import BulletWorld +from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.urdf_interface import ObjectDescription from pycram.world import Object from pycram.world_dataclasses import Color -from pycram.process_module import simulated_robot, with_simulated_robot -from pycram.enums import ObjectType + +extension = ObjectDescription.get_file_extension() world = BulletWorld() -robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf", pose=Pose([1, 2, 0])) -apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") - -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]), color=Color(1, 0, 0, 1)) -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]), - color=[0, 1, 0, 1]) -spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=Color(0, 0, 1, 1)) -bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=Color(1, 1, 0, 1)) +robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", ObjectDescription, pose=Pose([1, 2, 0])) +apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}", ObjectDescription) + +milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([2.5, 2, 1.02]), + color=Color(1, 0, 0, 1)) +cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", ObjectDescription, + pose=Pose([2.5, 2.3, 1.05]), color=Color(0, 1, 0, 1)) +spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", ObjectDescription, pose=Pose([2.4, 2.2, 0.85]), + color=Color(0, 0, 1, 1)) +bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", ObjectDescription, pose=Pose([2.5, 2.2, 1.02]), + color=Color(1, 1, 0, 1)) apartment.attach(spoon, 'cabinet10_drawer_top') pick_pose = Pose([2.7, 2.15, 1]) @@ -55,7 +61,8 @@ def move_and_detect(obj_type): # Finding and navigating to the drawer holding the spoon handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) - drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(), robot_desig=robot_desig.resolve()).resolve() + drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(), + robot_desig=robot_desig.resolve()).resolve() NavigateAction([drawer_open_loc.pose]).resolve().perform() @@ -91,4 +98,3 @@ def move_and_detect(obj_type): PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() - diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 0875edcd2..5cb332084 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -12,7 +12,9 @@ from .enums import ObjectType, WorldMode from .pose import Pose -from .world import World, Object, Constraint +from .world import World +from .world_object import Object +from .world_constraints import Constraint from .world_dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape from .urdf_interface import ObjectDescription @@ -140,7 +142,7 @@ def get_object_pose(self, obj: Object) -> Pose: return Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.id)) def set_link_color(self, link: Link, rgba_color: Color): - p.changeVisualShape(link.object_id, link.id, rgbaColor=rgba_color, physicsClientId=self.id) + p.changeVisualShape(link.object_id, link.id, rgbaColor=rgba_color.get_rgba(), physicsClientId=self.id) def get_link_color(self, link: Link) -> Color: return self.get_colors_of_object_links(link.object)[link.name] @@ -148,7 +150,7 @@ def get_link_color(self, link: Link) -> Color: def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: visual_data = p.getVisualShapeData(obj.id, physicsClientId=self.id) link_id_to_name = {v: k for k, v in obj.link_name_to_id.items()} - links = list(map(lambda x: link_id_to_name[x[1]] if x[1] != -1 else "base", visual_data)) + links = list(map(lambda x: link_id_to_name[x[1]], visual_data)) colors = list(map(lambda x: Color.from_rgba(x[7]), visual_data)) link_to_color = dict(zip(links, colors)) return link_to_color diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py new file mode 100644 index 000000000..24d3989e7 --- /dev/null +++ b/src/pycram/cache_manager.py @@ -0,0 +1,123 @@ +import os +import pathlib +import re + +from typing_extensions import List + + +class CacheManager: + + mesh_extensions: List[str] = [".obj", ".stl"] + """ + The file extensions of mesh files. + """ + + def __init__(self, cache_dir: str, data_directory: List[str]): + self.cache_dir = cache_dir + # The directory where the cached files are stored. + + self.data_directory = data_directory + # The directory where all resource files are stored. + + def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, + object_description: 'ObjectDescription', object_name: str) -> str: + """ + Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. + """ + path_object = pathlib.Path(path) + extension = path_object.suffix + + path = self.look_for_file_in_data_dir(path, path_object) + + self.create_cache_dir_if_not_exists() + + # save correct path in case the file is already in the cache directory + cache_path = self.cache_dir + object_description.get_file_name(path_object, extension, object_name) + + # if file is not yet cached preprocess the description file and save it in the cache directory. + if not self.is_cached(path, object_description) or ignore_cached_files: + self.generate_description_and_write_to_cache(path, extension, cache_path, object_description) + + return cache_path + + def generate_description_and_write_to_cache(self, path: str, extension: str, cache_path: str, + object_description: 'ObjectDescription') -> None: + """ + Generates the description from the file at the given path and writes it to the cache directory. + :param path: The path of the file to preprocess. + :param extension: The file extension of the file to preprocess. + :param cache_path: The path of the file in the cache directory. + :param object_description: The object description of the file. + """ + description_string = object_description.generate_description_from_file(path, extension) + self.write_to_cache(description_string, cache_path) + + @staticmethod + def write_to_cache(description_string: str, cache_path: str) -> None: + """ + Writes the description string to the cache directory. + :param description_string: The description string to write to the cache directory. + :param cache_path: The path of the file in the cache directory. + """ + with open(cache_path, "w") as file: + file.write(description_string) + + def look_for_file_in_data_dir(self, path: str, path_object: pathlib.Path) -> str: + """ + Looks for a file in the data directory of the World. If the file is not found in the data directory, this method + raises a FileNotFoundError. + :param path: The path of the file to look for. + :param path_object: The pathlib object of the file to look for. + """ + if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): + for data_dir in self.data_directory: + for file in os.listdir(data_dir): + if file == path: + return data_dir + f"/{path}" + if path: + break + + if not path: + raise FileNotFoundError( + f"File {path_object.name} could not be found in the resource directory {self.data_directory}") + + return path + + def create_cache_dir_if_not_exists(self): + """ + Creates the cache directory if it does not exist. + """ + if not pathlib.Path(self.cache_dir).exists(): + os.mkdir(self.cache_dir) + + def is_cached(self, path: str, object_description: 'ObjectDescription') -> bool: + """ + Checks if the file in the given path is already cached or if + there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from + the parameter server is used. + + :param path: The path of the file to check. + :param object_description: The object description of the file. + :return: True if there already exists a cached file, False in any other case. + """ + return True if self.check_with_extension(path) else self.check_without_extension(path, object_description) + + def check_with_extension(self, path: str) -> bool: + """ + Checks if the file in the given ath exists in the cache directory including file extension. + :param path: The path of the file to check. + """ + file_name = pathlib.Path(path).name + full_path = pathlib.Path(self.cache_dir + file_name) + return full_path.exists() + + def check_without_extension(self, path: str, object_description: 'ObjectDescription') -> bool: + """ + Checks if the file in the given path exists in the cache directory without file extension, + the extension is added after the file name manually in this case. + :param path: The path of the file to check. + :param object_description: The object description of the file. + """ + file_stem = pathlib.Path(path).stem + full_path = pathlib.Path(self.cache_dir + file_stem + object_description.get_file_extension()) + return full_path.exists() diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index db5a83a3d..11ae3ab0f 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -10,7 +10,9 @@ from matplotlib import colors from nav_msgs.msg import OccupancyGrid, MapMetaData -from .world import UseProspectionWorld, Object, Link +from .world import UseProspectionWorld +from .world_object import Object +from .description import Link from .local_transformer import LocalTransformer from .pose import Pose, Transform from .world import World diff --git a/src/pycram/description.py b/src/pycram/description.py new file mode 100644 index 000000000..8c86a7ea9 --- /dev/null +++ b/src/pycram/description.py @@ -0,0 +1,669 @@ +import pathlib +from abc import ABC, abstractmethod +import logging + +import rospy +from geometry_msgs.msg import Point, Quaternion +from typing_extensions import Tuple, Union, Any, List, Optional, Dict + +from pycram.enums import Shape, JointType +from pycram.local_transformer import LocalTransformer +from pycram.pose import Pose, Transform +from pycram.world import WorldEntity +from pycram.world_object import Object +from pycram.world_dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState + + +class EntityDescription(ABC): + + @property + @abstractmethod + def origin(self) -> Pose: + """ + Returns the origin of this entity. + """ + pass + + @property + @abstractmethod + def name(self) -> str: + """ + Returns the name of this entity. + """ + pass + + +class LinkDescription(EntityDescription): + """ + A class that represents a link description of an object. + """ + + def __init__(self, parsed_link_description: Any): + self.parsed_description = parsed_link_description + + @property + @abstractmethod + def geometry(self) -> Shape: + """ + Returns the geometry type of the URDF collision element of this link. + """ + pass + + +class JointDescription(EntityDescription): + """ + A class that represents a joint description of a URDF joint. + """ + + def __init__(self, parsed_joint_description: Any): + self.parsed_description = parsed_joint_description + + @property + @abstractmethod + def type(self) -> JointType: + """ + :return: The type of this joint. + """ + pass + + @property + @abstractmethod + def axis(self) -> Point: + """ + :return: The axis of this joint, for example the rotation axis for a revolute joint. + """ + pass + + @property + @abstractmethod + def has_limits(self) -> bool: + """ + Checks if this joint has limits. + :return: True if the joint has limits, False otherwise. + """ + pass + + @property + def limits(self) -> Tuple[float, float]: + """ + :return: The lower and upper limits of this joint. + """ + lower, upper = self.lower_limit, self.upper_limit + if lower > upper: + lower, upper = upper, lower + return lower, upper + + @property + @abstractmethod + def lower_limit(self) -> Union[float, None]: + """ + :return: The lower limit of this joint, or None if the joint has no limits. + """ + pass + + @property + @abstractmethod + def upper_limit(self) -> Union[float, None]: + """ + :return: The upper limit of this joint, or None if the joint has no limits. + """ + pass + + @property + @abstractmethod + def parent_link_name(self) -> str: + """ + :return: The name of the parent link of this joint. + """ + pass + + @property + @abstractmethod + def child_link_name(self) -> str: + """ + :return: The name of the child link of this joint. + """ + pass + + @property + def damping(self) -> float: + """ + :return: The damping of this joint. + """ + raise NotImplementedError + + @property + def friction(self) -> float: + """ + :return: The friction of this joint. + """ + raise NotImplementedError + + +class ObjectEntity(WorldEntity): + """ + An abstract base class that represents a physical part/entity of an Object. + This can be a link or a joint of an Object. + """ + + def __init__(self, _id: int, obj: Object): + WorldEntity.__init__(self, _id, obj.world) + self.object: Object = obj + + @property + @abstractmethod + def pose(self) -> Pose: + """ + :return: The pose of this entity relative to the world frame. + """ + pass + + @property + def transform(self) -> Transform: + """ + Returns the transform of this entity. + + :return: The transform of this entity. + """ + return self.pose.to_transform(self.tf_frame) + + @property + @abstractmethod + def tf_frame(self) -> str: + """ + Returns the tf frame of this entity. + + :return: The tf frame of this entity. + """ + pass + + @property + def object_id(self) -> int: + """ + :return: the id of the object to which this entity belongs. + """ + return self.object.id + + def __repr__(self): + return self.__class__.__qualname__ + f"(" + ', \n'.join( + [f"{key}={value}" for key, value in self.__dict__.items()]) + ")" + + def __str__(self): + return self.__repr__() + + +class Link(ObjectEntity, LinkDescription, ABC): + """ + Represents a link of an Object in the World. + """ + + def __init__(self, _id: int, link_description: LinkDescription, obj: Object): + ObjectEntity.__init__(self, _id, obj) + LinkDescription.__init__(self, link_description.parsed_description) + self.local_transformer: LocalTransformer = LocalTransformer() + self.constraint_ids: Dict[Link, int] = {} + self._update_pose() + + @property + def current_state(self) -> LinkState: + return LinkState(self.constraint_ids.copy()) + + @current_state.setter + 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') -> 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. + :return: The unique id of the constraint. + """ + constraint_id = self.world.add_fixed_constraint(self, + child_link, + child_link.get_transform_from_link(self)) + self.constraint_ids[child_link] = constraint_id + child_link.constraint_ids[self] = constraint_id + return constraint_id + + def remove_constraint_with_link(self, child_link: 'Link') -> None: + """ + Removes the constraint between this link and the given link. + :param child_link: The child link of the constraint that should be removed. + """ + self.world.remove_constraint(self.constraint_ids[child_link]) + del self.constraint_ids[child_link] + del child_link.constraint_ids[self] + + @property + def is_root(self) -> bool: + """ + Returns whether this link is the root link of the object. + :return: True if this link is the root link, False otherwise. + """ + return self.object.get_root_link_id() == self.id + + def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: + """ + Updates the transformation of this link at the given time. + :param transform_time: The time at which the transformation should be updated. + """ + self.local_transformer.update_transforms([self.transform], transform_time) + + def get_transform_to_link(self, link: 'Link') -> Transform: + """ + Returns the transformation from this link to the given link. + :param link: The link to which the transformation should be returned. + :return: A Transform object with the transformation from this link to the given link. + """ + return link.get_transform_from_link(self) + + def get_transform_from_link(self, link: 'Link') -> Transform: + """ + Returns the transformation from the given link to this link. + :param link: The link from which the transformation should be returned. + :return: A Transform object with the transformation from the given link to this link. + """ + return self.get_pose_wrt_link(link).to_transform(self.tf_frame) + + def get_pose_wrt_link(self, link: 'Link') -> Pose: + """ + Returns the pose of this link with respect to the given link. + :param link: The link with respect to which the pose should be returned. + :return: A Pose object with the pose of this link with respect to the given link. + """ + return self.local_transformer.transform_pose(self.pose, link.tf_frame) + + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + Returns the axis aligned bounding box of this link. + :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. + """ + return self.world.get_link_axis_aligned_bounding_box(self) + + @property + def position(self) -> Point: + """ + The getter for the position of the link relative to the world frame. + :return: A Point object containing the position of the link relative to the world frame. + """ + return self.pose.position + + @property + def position_as_list(self) -> List[float]: + """ + The getter for the position of the link relative to the world frame as a list. + :return: A list containing the position of the link relative to the world frame. + """ + return self.pose.position_as_list() + + @property + def orientation(self) -> Quaternion: + """ + The getter for the orientation of the link relative to the world frame. + :return: A Quaternion object containing the orientation of the link relative to the world frame. + """ + return self.pose.orientation + + @property + def orientation_as_list(self) -> List[float]: + """ + The getter for the orientation of the link relative to the world frame as a list. + :return: A list containing the orientation of the link relative to the world frame. + """ + return self.pose.orientation_as_list() + + def _update_pose(self) -> None: + """ + Updates the current pose of this link from the world. + """ + self._current_pose = self.world.get_link_pose(self) + + @property + def pose(self) -> Pose: + """ + The pose of the link relative to the world frame. + :return: A Pose object containing the pose of the link relative to the world frame. + """ + return self._current_pose + + @property + def pose_as_list(self) -> List[List[float]]: + """ + The pose of the link relative to the world frame as a list. + :return: A list containing the position and orientation of the link relative to the world frame. + """ + return self.pose.to_list() + + def get_origin_transform(self) -> Transform: + """ + Returns the transformation between the link frame and the origin frame of this link. + """ + return self.origin.to_transform(self.tf_frame) + + @property + def color(self) -> Color: + """ + The getter for the rgba_color of this link. + :return: A Color object containing the rgba_color of this link. + """ + return self.world.get_link_color(self) + + @color.setter + def color(self, color: Color) -> None: + """ + The setter for the color of this link, could be rgb or rgba. + :param color: The color as a list of floats, either rgb or rgba. + """ + self.world.set_link_color(self, color) + + @property + def origin_transform(self) -> Transform: + """ + :return: The transform from world to origin of entity. + """ + return self.origin.to_transform(self.tf_frame) + + @property + def tf_frame(self) -> str: + """ + The name of the tf frame of this link. + """ + return f"{self.object.tf_frame}/{self.name}" + + +class RootLink(Link, ABC): + """ + Represents the root link of an Object in the World. + It differs from the normal AbstractLink class in that the pose ande the tf_frame is the same as that of the object. + """ + + def __init__(self, obj: Object): + super().__init__(obj.get_root_link_id(), obj.get_root_link_description(), obj) + + @property + def tf_frame(self) -> str: + """ + Returns the tf frame of the root link, which is the same as the tf frame of the object. + """ + return self.object.tf_frame + + def _update_pose(self) -> None: + self._current_pose = self.object.get_pose() + + +class Joint(ObjectEntity, JointDescription, ABC): + """ + Represents a joint of an Object in the World. + """ + + def __init__(self, _id: int, + joint_description: JointDescription, + obj: Object): + ObjectEntity.__init__(self, _id, obj) + JointDescription.__init__(self, joint_description.parsed_description) + self._update_position() + + @property + def tf_frame(self) -> str: + """ + The tf frame of a joint is the tf frame of the child link. + """ + return self.child_link.tf_frame + + @property + def pose(self) -> Pose: + """ + Returns the pose of this joint. The pose is the pose of the child link of this joint. + + :return: The pose of this joint. + """ + return self.child_link.pose + + def _update_position(self) -> None: + """ + Updates the current position of the joint from the physics simulator. + """ + self._current_position = self.world.get_joint_position(self.object, self.name) + + @property + def parent_link(self) -> Link: + """ + Returns the parent link of this joint. + :return: The parent link as a AbstractLink object. + """ + return self.object.links[self.parent_link_name] + + @property + def child_link(self) -> Link: + """ + Returns the child link of this joint. + :return: The child link as a AbstractLink object. + """ + return self.object.links[self.child_link_name] + + @property + def position(self) -> float: + return self._current_position + + def reset_position(self, position: float) -> None: + self.world.reset_joint_position(self.object, self.name, position) + self._update_position() + + def get_object_id(self) -> int: + """ + Returns the id of the object to which this joint belongs. + :return: The integer id of the object to which this joint belongs. + """ + return self.object.id + + @position.setter + def position(self, joint_position: float) -> None: + """ + Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, as stated + in the URDF, an error will be printed. However, the joint will be set either way. + + :param joint_position: The target pose for this joint + """ + # TODO Limits for rotational (infinite) joints are 0 and 1, they should be considered separately + if self.has_limits: + low_lim, up_lim = self.limits + if not low_lim <= joint_position <= up_lim: + logging.error( + f"The joint position has to be within the limits of the joint. The joint limits for {self.name}" + f" are {low_lim} and {up_lim}") + logging.error(f"The given joint position was: {joint_position}") + # Temporarily disabled because kdl outputs values exciting joint limits + # return + self.reset_position(joint_position) + + def enable_force_torque_sensor(self) -> None: + self.world.enable_joint_force_torque_sensor(self.object, self.id) + + def disable_force_torque_sensor(self) -> None: + self.world.disable_joint_force_torque_sensor(self.object, self.id) + + def get_reaction_force_torque(self) -> List[float]: + return self.world.get_joint_reaction_force_torque(self.object, self.id) + + def get_applied_motor_torque(self) -> float: + return self.world.get_applied_joint_motor_torque(self.object, self.id) + + def restore_state(self, state_id: int) -> None: + self.current_state = self.saved_states[state_id] + + @property + def current_state(self) -> JointState: + return JointState(self.position) + + @current_state.setter + def current_state(self, joint_state: JointState) -> None: + self.position = joint_state.position + + +class ObjectDescription(EntityDescription): + MESH_EXTENSIONS: Tuple[str] = (".obj", ".stl") + + class Link(Link, ABC): + ... + + class RootLink(RootLink, ABC): + ... + + class Joint(Joint, ABC): + ... + + def __init__(self, path: Optional[str] = None): + if path: + self.update_description_from_file(path) + else: + self._parsed_description = None + + def update_description_from_file(self, path: str) -> None: + """ + Updates the description of this object from the file at the given path. + :param path: The path of the file to update from. + """ + self._parsed_description = self.load_description(path) + + @property + def parsed_description(self) -> Any: + """ + Return the object parsed from the description file. + """ + return self._parsed_description + + @parsed_description.setter + def parsed_description(self, parsed_description: Any): + """ + Return the object parsed from the description file. + :param parsed_description: The parsed description object. + """ + self._parsed_description = parsed_description + + @abstractmethod + def load_description(self, path: str) -> Any: + """ + Loads the description from the file at the given path. + :param path: The path to the source file, if only a filename is provided then the resources directories will be + searched. + """ + pass + + def generate_description_from_file(self, path: str, extension: str) -> str: + """ + Generates and preprocesses the description from the file at the given path and returns the preprocessed + description as a string. + :param path: The path of the file to preprocess. + :param extension: The file extension of the file to preprocess. + :return: The processed description string. + """ + + if extension in self.MESH_EXTENSIONS: + description_string = self.generate_from_mesh_file(path) + elif extension == self.get_file_extension(): + description_string = self.generate_from_description_file(path) + else: + # Using the description from the parameter server + description_string = self.generate_from_parameter_server(path) + + return description_string + + def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: str) -> str: + """ + Returns the file name of the description file. + :param path_object: The path object of the description file or the mesh file. + :param extension: The file extension of the description file or the mesh file. + :param object_name: The name of the object. + :return: The file name of the description file. + """ + if extension in self.MESH_EXTENSIONS: + file_name = path_object.stem + self.get_file_extension() + elif extension == self.get_file_extension(): + file_name = path_object.name + else: + file_name = object_name + self.get_file_extension() + + return file_name + + @classmethod + @abstractmethod + def generate_from_mesh_file(cls, path: str) -> str: + """ + Generates a description file from one of the mesh types defined in the MESH_EXTENSIONS and + returns the path of the generated file. + :param path: The path to the .obj file. + :return: The path of the generated description file. + """ + pass + + @classmethod + @abstractmethod + def generate_from_description_file(cls, path: str) -> str: + """ + Preprocesses the given file and returns the preprocessed description string. + :param path: The path of the file to preprocess. + :return: The preprocessed description string. + """ + pass + + @classmethod + @abstractmethod + def generate_from_parameter_server(cls, name: str) -> str: + """ + Preprocesses the description from the ROS parameter server and returns the preprocessed description string. + :param name: The name of the description on the parameter server. + :return: The preprocessed description string. + """ + pass + + @property + @abstractmethod + def links(self) -> List[LinkDescription]: + """ + :return: A list of links descriptions of this object. + """ + pass + + @abstractmethod + def get_link_by_name(self, link_name: str) -> LinkDescription: + """ + :return: The link description with the given name. + """ + pass + + @property + @abstractmethod + def joints(self) -> List[JointDescription]: + """ + :return: A list of joints descriptions of this object. + """ + pass + + @abstractmethod + def get_joint_by_name(self, joint_name: str) -> JointDescription: + """ + :return: The joint description with the given name. + """ + pass + + @abstractmethod + def get_root(self) -> str: + """ + :return: the name of the root link of this object. + """ + pass + + @abstractmethod + def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: + """ + :return: the chain of links from 'start_link_name' to 'end_link_name'. + """ + pass + + @staticmethod + @abstractmethod + def get_file_extension() -> str: + """ + :return: The file extension of the description file. + """ + pass diff --git a/src/pycram/designator.py b/src/pycram/designator.py index a22f51a74..4dc524b4e 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -8,7 +8,8 @@ from sqlalchemy.orm.session import Session import rospy -from .world import World, Object as WorldObject +from .world import World +from .world_object import Object as WorldObject from .helper import GeneratorList, bcolors from threading import Lock from time import time diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 4efa64e49..88604d71f 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -2,7 +2,8 @@ from sqlalchemy.orm import Session from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject -from ..world import Object, World +from ..world import World +from ..world_object import Object from ..designator import DesignatorError from ..plan_failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index e52be0235..23dacdbdf 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,7 +1,8 @@ import dataclasses from typing_extensions import List, Optional, Callable import sqlalchemy.orm -from ..world import World, Object as WorldObject +from ..world import World +from ..world_object import Object as WorldObject from ..designator import ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 686f6bfab..d2c83e8e4 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -11,7 +11,8 @@ from ..pose import Pose from ..robot_descriptions import robot_description -from ..world import World, Object +from ..world import World +from ..world_object import Object from ..robot_description import ManipulatorDescription from typing_extensions import List, Tuple, Dict, Callable, Optional diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 48657fa97..4d60d7d52 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -6,7 +6,7 @@ from moveit_msgs.srv import GetPositionIK from sensor_msgs.msg import JointState -from ..world import Object +from ..world_object import Object from ..helper import calculate_wrist_tool_offset, _apply_ik from ..local_transformer import LocalTransformer from ..pose import Pose diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 4503e7a4d..a0c9fb4ae 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -13,7 +13,7 @@ from pytransform3d.rotations import quaternion_wxyz_from_xyzw, quaternion_xyzw_from_wxyz from pytransform3d.transformations import transform_from_pq, transform_from, pq_from_transform -from .world import Object as WorldObject +from .world_object import Object as WorldObject from .pose import Transform, Pose import math diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index dfcc50176..a1bc00019 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -1,7 +1,8 @@ import tf import numpy as np -from .world import Object, World +from .world import World +from .world_object import Object from .world_reasoning import contact from .costmaps import Costmap from .pose import Pose, Transform diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index e4d02d306..aa2c3bc53 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -18,7 +18,8 @@ DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ MoveGripperMotion, OpeningMotion, ClosingMotion, MotionDesignatorDescription from ..robot_descriptions import robot_description -from ..world import World, Object +from ..world import World +from ..world_object import Object from ..pose import Pose from ..enums import JointType, ObjectType from ..external_interfaces import giskard diff --git a/src/pycram/urdf_interface.py b/src/pycram/urdf_interface.py index aa755d1c6..51ce8c3b3 100644 --- a/src/pycram/urdf_interface.py +++ b/src/pycram/urdf_interface.py @@ -12,7 +12,7 @@ from pycram.enums import JointType, Shape from pycram.pose import Pose -from pycram.world import JointDescription as AbstractJointDescription, \ +from pycram.description import JointDescription as AbstractJointDescription, \ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription from pycram.world_dataclasses import Color diff --git a/src/pycram/world.py b/src/pycram/world.py index fbc6ffd4f..463ddf62f 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -1,30 +1,29 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -import logging import os -import pathlib -import re import threading import time from abc import ABC, abstractmethod +from copy import copy from queue import Queue import numpy as np import rospy -from geometry_msgs.msg import Quaternion, Point -from typing_extensions import List, Optional, Dict, Tuple, Callable, Any, Type +from geometry_msgs.msg import Point +from typing_extensions import List, Optional, Dict, Tuple, Callable from typing_extensions import Union -from .enums import JointType, ObjectType, WorldMode, Shape +from .cache_manager import CacheManager +from .enums import JointType, ObjectType, WorldMode from .event import Event from .local_transformer import LocalTransformer from .pose import Pose, Transform -from .robot_descriptions import robot_description +from .world_constraints import Constraint from .world_dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - LinkState, ObjectState, JointState, State, WorldState) + ObjectState, State, WorldState) class StateEntity: @@ -87,6 +86,17 @@ def remove_saved_states(self) -> None: self._saved_states = {} +class WorldEntity(StateEntity, ABC): + """ + A data class that represents an entity of the world, such as an object or a link. + """ + + def __init__(self, _id: int, world: Optional[World] = None): + StateEntity.__init__(self) + self.id = _id + self.world: World = world if world is not None else World.current_world + + class World(StateEntity, ABC): """ The World Class represents the physics Simulation and belief state, it is the main interface for reasoning about @@ -107,9 +117,9 @@ class World(StateEntity, ABC): used at the moment. """ - robot: Optional[Object] = None + robot: Optional['Object'] = None """ - Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in + Global reference to the spawned 'Object'that represents the robot. The robot is identified by checking the name in the URDF with the name of the URDF on the parameter server. """ @@ -139,7 +149,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ World.current_world = self World.simulation_frequency = simulation_frequency - self.cache_manager = CacheManager() + self.cache_manager = CacheManager(self.cache_dir, self.data_directory) self.is_prospection_world: bool = is_prospection_world self._init_and_sync_prospection_world() @@ -147,7 +157,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.local_transformer = LocalTransformer() self._update_local_transformer_worlds() - self.objects: List[Object] = [] + self.objects: List['Object'] = [] # List of all Objects in the World self.id: int = -1 @@ -156,7 +166,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.mode: WorldMode = mode # The mode of the simulation, can be "GUI" or "DIRECT" - self.coll_callbacks: Dict[Tuple[Object, Object], CollisionCallbacks] = {} + self.coll_callbacks: Dict[Tuple['Object', 'Object'], CollisionCallbacks] = {} self._init_events() @@ -206,7 +216,7 @@ def _sync_prospection_world(self): self.world_sync.start() def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - obj: Object) -> str: + obj: 'Object') -> str: """ Updates the cache directory with the given object. @@ -235,7 +245,7 @@ def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: pass # TODO: This is not used anywhere, should it be removed? - def get_objects_by_name(self, name: str) -> List[Object]: + def get_objects_by_name(self, name: str) -> List['Object']: """ Returns a list of all Objects in this World with the same name as the given one. @@ -244,7 +254,7 @@ def get_objects_by_name(self, name: str) -> List[Object]: """ return list(filter(lambda obj: obj.name == name, self.objects)) - def get_objects_by_type(self, obj_type: ObjectType) -> List[Object]: + def get_objects_by_type(self, obj_type: ObjectType) -> List['Object']: """ Returns a list of all Objects which have the type 'obj_type'. @@ -253,29 +263,29 @@ def get_objects_by_type(self, obj_type: ObjectType) -> List[Object]: """ return list(filter(lambda obj: obj.obj_type == obj_type, self.objects)) - def get_object_by_id(self, obj_id: int) -> Object: + def get_object_by_id(self, obj_id: int) -> 'Object': """ - Returns the single Object that has the unique id. + Returns the single 'Object'that has the unique id. - :param obj_id: The unique id for which the Object should be returned. - :return: The Object with the id 'id'. + :param obj_id: The unique id for which the 'Object'should be returned. + :return: The 'Object'with the id 'id'. """ return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] @abstractmethod - def remove_object_from_simulator(self, obj: Object) -> None: + def remove_object_from_simulator(self, obj: 'Object') -> None: """ Removes an object from the physics simulator. :param obj: The object to be removed. """ pass - def remove_object(self, obj: Object) -> None: + def remove_object(self, obj: 'Object') -> None: """ Removes this object from the current world. For the object to be removed it has to be detached from all objects it is currently attached to. After this is done a call to world remove object is done - to remove this Object from the simulation/world. + to remove this 'Object'from the simulation/world. :param obj: The object to be removed. """ @@ -294,7 +304,8 @@ def remove_object(self, obj: Object) -> None: if World.robot == self: World.robot = None - def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: + def add_fixed_constraint(self, parent_link: 'Link', child_link: 'Link', + child_to_parent_transform: Transform) -> int: """ Creates a fixed joint constraint between the given parent and child links, the joint frame will be at the origin of the child link frame, and would have the same orientation @@ -335,7 +346,7 @@ def remove_constraint(self, constraint_id) -> None: pass @abstractmethod - def get_joint_position(self, obj: Object, joint_name: str) -> float: + def get_joint_position(self, obj: 'Object', joint_name: str) -> float: """ Get the position of a joint of an articulated object @@ -346,7 +357,7 @@ def get_joint_position(self, obj: Object, joint_name: str) -> float: pass @abstractmethod - def get_object_joint_names(self, obj: Object) -> List[str]: + def get_object_joint_names(self, obj: 'Object') -> List[str]: """ Returns the names of all joints of this object. @@ -356,7 +367,7 @@ def get_object_joint_names(self, obj: Object) -> List[str]: pass @abstractmethod - def get_link_pose(self, link: Link) -> Pose: + def get_link_pose(self, link: 'Link') -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. @@ -366,7 +377,7 @@ def get_link_pose(self, link: Link) -> Pose: pass @abstractmethod - def get_object_link_names(self, obj: Object) -> List[str]: + def get_object_link_names(self, obj: 'Object') -> List[str]: """ Returns the names of all links of this object. :param obj: The object. @@ -374,7 +385,7 @@ def get_object_link_names(self, obj: Object) -> List[str]: """ pass - def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: + def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: """ Simulates Physics in the World for a given amount of seconds. Usually this simulation is faster than real time. By setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal @@ -383,6 +394,7 @@ def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: :param seconds: The amount of seconds that should be simulated. :param real_time: If the simulation should happen in real time or faster. """ + self.set_realtime(real_time) for i in range(0, int(seconds * self.simulation_frequency)): curr_time = rospy.Time.now() self.step() @@ -394,7 +406,8 @@ def simulate(self, seconds: float, real_time: Optional[float] = False) -> None: callbacks.no_collision_cb() if real_time: loop_time = rospy.Time.now() - curr_time - time.sleep(max(0, self.simulation_time_step - loop_time.to_sec())) + time_diff = self.simulation_time_step - loop_time.to_sec() + time.sleep(max(0, time_diff)) self.update_all_objects_poses() def update_all_objects_poses(self) -> None: @@ -405,7 +418,7 @@ def update_all_objects_poses(self) -> None: obj.update_pose() @abstractmethod - def get_object_pose(self, obj: Object) -> Pose: + def get_object_pose(self, obj: 'Object') -> Pose: """ Get the pose of an object in the world frame from the current object pose in the simulator. """ @@ -419,9 +432,9 @@ def perform_collision_detection(self) -> None: pass @abstractmethod - def get_object_contact_points(self, obj: Object) -> List: + def get_object_contact_points(self, obj: 'Object') -> List: """ - Returns a list of contact points of this Object with all other Objects. + Returns a list of contact points of this 'Object'with all other Objects. :param obj: The object. :return: A list of all contact points with other objects @@ -429,7 +442,7 @@ def get_object_contact_points(self, obj: Object) -> List: pass @abstractmethod - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + def get_contact_points_between_two_objects(self, obj1: 'Object', obj2: 'Object') -> List: """ Returns a list of contact points between obj1 and obj2. @@ -440,7 +453,7 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> pass @abstractmethod - def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + def reset_joint_position(self, obj: 'Object', joint_name: str, joint_pose: float) -> None: """ Reset the joint position instantly without physics simulation @@ -451,7 +464,7 @@ def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) pass @abstractmethod - def reset_object_base_pose(self, obj: Object, pose: Pose): + def reset_object_base_pose(self, obj: 'Object', pose: Pose): """ 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. @@ -469,7 +482,7 @@ def step(self): pass @abstractmethod - def set_link_color(self, link: Link, rgba_color: Color): + def set_link_color(self, link: 'Link', rgba_color: Color): """ Changes the rgba_color of a link of this object, the rgba_color has to be given as Color object. @@ -479,7 +492,7 @@ def set_link_color(self, link: Link, rgba_color: Color): pass @abstractmethod - def get_link_color(self, link: Link) -> Color: + def get_link_color(self, link: 'Link') -> Color: """ This method returns the rgba_color of this link. :param link: The link for which the rgba_color should be returned. @@ -488,7 +501,7 @@ def get_link_color(self, link: Link) -> Color: pass @abstractmethod - def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: + def get_colors_of_object_links(self, obj: 'Object') -> Dict[str, Color]: """ Get the RGBA colors of each link in the object as a dictionary from link name to rgba_color. @@ -498,7 +511,7 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: pass @abstractmethod - def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: + def get_object_axis_aligned_bounding_box(self, obj: 'Object') -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. @@ -509,7 +522,7 @@ def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundi pass @abstractmethod - def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: + def get_link_axis_aligned_bounding_box(self, link: 'Link') -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. @@ -560,21 +573,21 @@ def set_gravity(self, gravity_vector: List[float]) -> None: """ pass - def set_robot_if_not_set(self, robot: Object) -> None: + def set_robot_if_not_set(self, robot: 'Object') -> None: """ Sets the robot if it is not set yet. - :param robot: The Object reference to the Object representing the robot. + :param robot: The 'Object'reference to the 'Object'representing the robot. """ if not self.robot_is_set(): self.set_robot(robot) @staticmethod - def set_robot(robot: Union[Object, None]) -> None: + def set_robot(robot: Union['Object', None]) -> None: """ - Sets the global variable for the robot Object. This should be set on spawning the robot. + Sets the global variable for the robot 'Object' This should be set on spawning the robot. - :param robot: The Object reference to the Object representing the robot. + :param robot: The 'Object'reference to the 'Object'representing the robot. """ World.robot = robot @@ -738,7 +751,7 @@ def get_images_for_target(self, 1. An RGB image 2. A depth image - 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object. + 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible 'Object' :param target_pose: The pose to which the camera should point. :param cam_pose: The pose of the camera. @@ -748,8 +761,8 @@ def get_images_for_target(self, pass def register_two_objects_collision_callbacks(self, - object_a: Object, - object_b: Object, + object_a: 'Object', + object_b: 'Object', on_collision_callback: Callable, on_collision_removal_callback: Optional[Callable] = None) -> None: """ @@ -768,16 +781,16 @@ def register_two_objects_collision_callbacks(self, def add_resource_path(cls, path: str) -> None: """ Adds a resource path in which the World will search for files. This resource directory is searched if an - Object is spawned only with a filename. + 'Object'is spawned only with a filename. :param path: A path in the filesystem in which to search for files. """ cls.data_directory.append(path) - def get_prospection_object_for_object(self, obj: Object) -> Object: + def get_prospection_object_for_object(self, obj: 'Object') -> 'Object': """ Returns the corresponding object from the prospection world for a given object in the main world. - If the given Object is already in the prospection world, it is returned. + If the given 'Object'is already in the prospection world, it is returned. :param obj: The object for which the corresponding object in the prospection World should be found. :return: The corresponding object in the prospection world. @@ -794,7 +807,7 @@ def get_prospection_object_for_object(self, obj: Object) -> Object: f" the object isn't anymore in the main (graphical) World" f" or if the given object is already a prospection object. ") - def get_object_for_prospection_object(self, prospection_object: Object) -> Object: + def get_object_for_prospection_object(self, prospection_object: 'Object') -> 'Object': """ Returns the corresponding object from the main World for a given object in the prospection world. If the given object is not in the prospection @@ -989,7 +1002,7 @@ def remove_text(self, text_id: Optional[int] = None) -> None: """ raise NotImplementedError - def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: + def enable_joint_force_torque_sensor(self, obj: 'Object', fts_joint_idx: int) -> None: """ You can enable a joint force/torque sensor in each joint. Once enabled, if you perform a simulation step, the get_joint_reaction_force_torque will report the joint reaction forces in @@ -1001,7 +1014,7 @@ def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> N """ raise NotImplementedError - def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: + def disable_joint_force_torque_sensor(self, obj: 'Object', joint_id: int) -> None: """ Disables the force torque sensor of a joint. :param obj: The object in which the joint is located. @@ -1009,7 +1022,7 @@ def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: """ raise NotImplementedError - def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[float]: + def get_joint_reaction_force_torque(self, obj: 'Object', joint_id: int) -> List[float]: """ Returns the joint reaction forces and torques of the specified joint. :param obj: The object in which the joint is located. @@ -1018,7 +1031,7 @@ def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[fl """ raise NotImplementedError - def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: + def get_applied_joint_motor_torque(self, obj: 'Object', joint_id: int) -> float: """ Returns the applied torque by a joint motor. :param obj: The object in which the joint is located. @@ -1086,7 +1099,7 @@ def __init__(self, world: World, prospection_world: World): self.remove_obj_queue: Queue = Queue() self.pause_sync: bool = False # Maps world to prospection world objects - self.object_mapping: Dict[Object, Object] = {} + self.object_mapping: Dict['Object', 'Object'] = {} self.equal_states = False def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): @@ -1107,9 +1120,8 @@ def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): obj = self.add_obj_queue.get() # [0:name, 1:type, 2:path, 3:description, 4:position, 5:orientation, # 6:self.world.prospection_world, 7:rgba_color, 8:world object] - o = Object(obj[0], obj[1], obj[2], obj[3], Pose(obj[4], obj[5]), obj[6], obj[7]) # Maps the World object to the prospection world object - self.object_mapping[obj[8]] = o + self.object_mapping[obj[8]] = copy(obj[8]) self.add_obj_queue.task_done() for i in range(self.remove_obj_queue.qsize()): obj = self.remove_obj_queue.get() @@ -1155,1849 +1167,3 @@ def check_for_equal(self) -> None: for obj, prospection_obj in self.object_mapping.items(): eql = eql and obj.get_pose() == prospection_obj.get_pose() self.equal_states = eql - - -class WorldEntity(StateEntity, ABC): - """ - A data class that represents an entity of the world, such as an object or a link. - """ - - def __init__(self, _id: int, world: Optional[World] = None): - StateEntity.__init__(self) - self.id = _id - self.world: World = world if world is not None else World.current_world - - -class Object(WorldEntity): - """ - Represents a spawned Object in the World. - """ - - prospection_world_prefix: str = "prospection/" - """ - The ObjectDescription of the object, this contains the name and type of the object as well as the path to the source - file. - """ - - def __init__(self, name: str, obj_type: ObjectType, path: str, - description: Type[ObjectDescription], - pose: Optional[Pose] = None, - world: Optional[World] = None, - color: Optional[Color] = Color(), - ignore_cached_files: Optional[bool] = False): - """ - The constructor loads the description file into the given World, if no World is specified the - :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. - The rgba_color parameter is only used when loading .stl or .obj files, - for URDFs :func:`~Object.set_color` can be used. - - :param name: The name of the object - :param obj_type: The type of the object as an ObjectType enum. - :param path: The path to the source file, if only a filename is provided then the resources directories will be - searched. - :param description: The ObjectDescription of the object, this contains the joints and links of the object. - :param pose: The pose at which the Object should be spawned - :param world: The World in which the object should be spawned, - if no world is specified the :py:attr:`~World.current_world` will be used. - :param color: The rgba_color with which the object should be spawned. - :param ignore_cached_files: If true the file will be spawned while ignoring cached files. - """ - - super().__init__(-1, world) - - if pose is None: - pose = Pose() - - self.name: str = name - self.obj_type: ObjectType = obj_type - self.color: Color = color - self.description = description() - self.cache_manager = self.world.cache_manager - - self.local_transformer = LocalTransformer() - self.original_pose = self.local_transformer.transform_pose(pose, "map") - self._current_pose = self.original_pose - - self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) - - self.description.update_description_from_file(self.path) - - self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") - + f"{self.name}_{self.id}") - - self._update_object_description_from_file(self.path) - - if self.description.name == robot_description.name: - self.world.set_robot_if_not_set(self) - - self._init_joint_name_and_id_map() - self._init_link_name_and_id_map() - - self._init_links_and_update_transforms() - self._init_joints() - - self.attachments: Dict[Object, Attachment] = {} - - if not self.world.is_prospection_world: - self._add_to_world_sync_obj_queue(self.path) - - self.world.objects.append(self) - - def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, str]: - """ - Loads an object to the given World with the given position and orientation. The rgba_color will only be - used when an .obj or .stl file is given. - If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created - and this URDf file will be loaded instead. - When spawning a URDf file a new file will be created in the cache directory, if there exists none. - This new file will have resolved mesh file paths, meaning there will be no references - to ROS packges instead there will be absolute file paths. - - :param ignore_cached_files: Whether to ignore files in the cache directory. - :return: The unique id of the object and the path of the file that was loaded. - """ - - path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) - - try: - obj_id = self.world.load_description_and_get_object_id(path, Pose(self.get_position_as_list(), - self.get_orientation_as_list())) - return obj_id, path - except Exception as e: - logging.error( - "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" - " the name of an URDF string on the parameter server.") - os.remove(path) - raise (e) - - def _update_object_description_from_file(self, path: str) -> None: - """ - Updates the object description from the given file path. - :param path: The path to the file from which the object description should be updated. - """ - self.description.update_description_from_file(path) - - def _init_joint_name_and_id_map(self) -> None: - """ - Creates a dictionary which maps the joint names to their unique ids and vice versa. - """ - n_joints = len(self.joint_names) - self.joint_name_to_id = dict(zip(self.joint_names, range(n_joints))) - self.joint_id_to_name = dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys())) - - def _init_link_name_and_id_map(self) -> None: - """ - Creates a dictionary which maps the link names to their unique ids and vice versa. - """ - n_links = len(self.link_names) - self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) - self.link_name_to_id[self.description.get_root()] = -1 - self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) - - def _init_links_and_update_transforms(self) -> None: - """ - Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the - corresponding link objects. - """ - self.links = {} - for link_name, link_id in self.link_name_to_id.items(): - link_description = self.description.get_link_by_name(link_name) - if link_name == self.description.get_root(): - self.links[link_name] = self.description.RootLink(self) - else: - self.links[link_name] = self.description.Link(link_id, link_description, self) - - self.update_link_transforms() - - def _init_joints(self): - """ - Initialize the joint objects from the URDF file and creates a dictionary which mas the joint names to the - corresponding joint objects - """ - 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) - - def _add_to_world_sync_obj_queue(self, path: str) -> None: - """ - Adds this object to the objects queue of the WorldSync object of the World. - :param path: The path of the URDF file of this object. - """ - self.world.world_sync.add_obj_queue.put( - [self.name, self.obj_type, path, type(self.description), self.get_position_as_list(), - self.get_orientation_as_list(), - self.world.prospection_world, self.color, self]) - - @property - def link_names(self) -> List[str]: - """ - :return: The name of each link as a list. - """ - return self.world.get_object_link_names(self) - - @property - def number_of_links(self) -> int: - """ - :return: The number of links of this object. - """ - return len(self.description.links) - - @property - def joint_names(self) -> List[str]: - """ - :return: The name of each joint as a list. - """ - return self.world.get_object_joint_names(self) - - @property - def number_of_joints(self) -> int: - """ - :return: The number of joints of this object. - """ - return len(self.description.joints) - - @property - def base_origin_shift(self) -> np.ndarray: - """ - The shift between the base of the object and the origin of the object. - :return: A numpy array with the shift between the base of the object and the origin of the object. - """ - return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) - - def __repr__(self): - skip_attr = ["links", "joints", "description", "attachments"] - return self.__class__.__qualname__ + f"(" + ', \n'.join( - [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" - - def remove(self) -> None: - """ - Removes this object from the World it currently resides in. - For the object to be removed it has to be detached from all objects it - is currently attached to. After this is done a call to world remove object is done - to remove this Object from the simulation/world. - """ - self.world.remove_object(self) - - def reset(self, remove_saved_states=True) -> None: - """ - Resets the Object to the state it was first spawned in. - All attached objects will be detached, all joints will be set to the - default position of 0 and the object will be set to the position and - orientation in which it was spawned. - :param remove_saved_states: If True the saved states will be removed. - """ - self.detach_all() - self.reset_all_joints_positions() - self.set_pose(self.original_pose) - if remove_saved_states: - self.remove_saved_states() - - def attach(self, - child_object: Object, - parent_link: Optional[str] = None, - child_link: Optional[str] = None, - bidirectional: Optional[bool] = True) -> None: - """ - Attaches another object to this object. This is done by - saving the transformation between the given link, if there is one, and - the base pose of the other object. Additionally, the name of the link, to - which the object is attached, will be saved. - Furthermore, a simulator constraint will be created so the attachment - also works while simulation. - Loose attachments means that the attachment will only be one-directional. For example, if this object moves the - other, attached, object will also move but not the other way around. - - :param child_object: The other object that should be attached. - :param parent_link: The link name of this object. - :param child_link: The link name of the other object. - :param bidirectional: If the attachment should be a loose attachment. - """ - parent_link = self.links[parent_link] if parent_link else self.root_link - child_link = child_object.links[child_link] if child_link else child_object.root_link - - attachment = Attachment(parent_link, child_link, bidirectional) - - self.attachments[child_object] = attachment - child_object.attachments[self] = attachment.get_inverse() - - self.world.attachment_event(self, [self, child_object]) - - def detach(self, child_object: Object) -> None: - """ - Detaches another object from this object. This is done by - deleting the attachment from the attachments dictionary of both objects - and deleting the constraint of the simulator. - Afterward the detachment event of the corresponding World will be fired. - - :param child_object: The object which should be detached - """ - del self.attachments[child_object] - del child_object.attachments[self] - - self.world.detachment_event(self, [self, child_object]) - - def detach_all(self) -> None: - """ - Detach all objects attached to this object. - """ - attachments = self.attachments.copy() - for att in attachments.keys(): - self.detach(att) - - def update_attachment_with_object(self, child_object: Object): - self.attachments[child_object].update_transform_and_constraint() - - def get_position(self) -> Point: - """ - Returns the position of this Object as a list of xyz. - - :return: The current position of this object - """ - return self.get_pose().position - - def get_orientation(self) -> Pose.orientation: - """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. - - :return: A list of xyzw - """ - return self.get_pose().orientation - - def get_position_as_list(self) -> List[float]: - """ - Returns the position of this Object as a list of xyz. - - :return: The current position of this object - """ - return self.get_pose().position_as_list() - - def get_orientation_as_list(self) -> List[float]: - """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. - - :return: A list of xyzw - """ - return self.get_pose().orientation_as_list() - - def get_pose(self) -> Pose: - """ - Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. - - :return: The current pose of this object - """ - return self._current_pose - - def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Optional[bool] = True) -> None: - """ - Sets the Pose of the object. - - :param pose: New Pose for the object - :param base: If True places the object base instead of origin at the specified position and orientation - :param set_attachments: Whether to set the poses of the attached objects to this object or not. - """ - pose_in_map = self.local_transformer.transform_pose(pose, "map") - if base: - pose_in_map.position = np.array(pose_in_map.position) + self.base_origin_shift - - self.reset_base_pose(pose_in_map) - - if set_attachments: - self._set_attached_objects_poses() - - def reset_base_pose(self, pose: Pose): - self.world.reset_object_base_pose(self, pose) - self.update_pose() - - def update_pose(self): - """ - Updates the current pose of this object from the world, and updates the poses of all links. - """ - self._current_pose = self.world.get_object_pose(self) - self._update_all_links_poses() - self.update_link_transforms() - - def _update_all_joints_positions(self): - """ - Updates the posisitons of all joints by getting them from the simulator. - """ - for joint in self.joints.values(): - joint._update_position() - - def _update_all_links_poses(self): - """ - Updates the poses of all links by getting them from the simulator. - """ - for link in self.links.values(): - link._update_pose() - - def move_base_to_origin_pos(self) -> None: - """ - Move the object such that its base will be at the current origin position. - This is useful when placing objects on surfaces where you want the object base in contact with the surface. - """ - self.set_pose(self.get_pose(), base=True) - - def save_state(self, state_id) -> None: - """ - Saves the state of this object by saving the state of all links and attachments. - :param state_id: The unique id of the state. - """ - self.save_links_states(state_id) - self.save_joints_states(state_id) - super().save_state(state_id) - - def save_links_states(self, state_id: int) -> None: - """ - Saves the state of all links of this object. - :param state_id: The unique id of the state. - """ - for link in self.links.values(): - link.save_state(state_id) - - def save_joints_states(self, state_id: int) -> None: - """ - Saves the state of all joints of this object. - :param state_id: The unique id of the state. - """ - for joint in self.joints.values(): - joint.save_state(state_id) - - @property - def current_state(self) -> ObjectState: - return ObjectState(self.attachments.copy(), self.link_states, self.joint_states) - - @current_state.setter - def current_state(self, state: ObjectState) -> None: - self.attachments = state.attachments - self.link_states = state.link_states - self.joint_states = state.joint_states - - @property - def link_states(self) -> Dict[int, LinkState]: - """ - Returns the current state of all links of this object. - :return: A dictionary with the link id as key and the current state of the link as value. - """ - return {link.id: link.current_state for link in self.links.values()} - - @link_states.setter - def link_states(self, link_states: Dict[int, LinkState]) -> None: - """ - Sets the current state of all links of this object. - :param link_states: A dictionary with the link id as key and the current state of the link as value. - """ - for link in self.links.values(): - link.current_state = link_states[link.id] - - @property - def joint_states(self) -> Dict[int, JointState]: - """ - Returns the current state of all joints of this object. - :return: A dictionary with the joint id as key and the current state of the joint as value. - """ - return {joint.id: joint.current_state for joint in self.joints.values()} - - @joint_states.setter - def joint_states(self, joint_states: Dict[int, JointState]) -> None: - """ - Sets the current state of all joints of this object. - :param joint_states: A dictionary with the joint id as key and the current state of the joint as value. - """ - for joint in self.joints.values(): - joint.current_state = joint_states[joint.id] - - def restore_state(self, state_id: int) -> None: - """ - Restores the state of this object by restoring the state of all links and attachments. - :param state_id: The unique id of the state. - """ - self.restore_attachments(state_id) - self.restore_links_states(state_id) - self.restore_joints_states(state_id) - - def restore_attachments(self, state_id: int) -> None: - """ - Restores the attachments of this object from a saved state using the given state id. - :param state_id: The unique id of the state. - """ - self.attachments = self.saved_states[state_id].attachments - - def restore_links_states(self, state_id: int) -> None: - """ - Restores the states of all links of this object from a saved state using the given state id. - :param state_id: The unique id of the state. - """ - for link in self.links.values(): - link.restore_state(state_id) - - def restore_joints_states(self, state_id: int) -> None: - """ - Restores the states of all joints of this object from a saved state using the given state id. - :param state_id: The unique id of the state. - """ - for joint in self.joints.values(): - joint.restore_state(state_id) - - def remove_saved_states(self) -> None: - """ - Removes all saved states of this object. - """ - super().remove_saved_states() - self.remove_links_saved_states() - self.remove_joints_saved_states() - - def remove_links_saved_states(self) -> None: - """ - Removes all saved states of the links of this object. - """ - for link in self.links.values(): - link.remove_saved_states() - - def remove_joints_saved_states(self) -> None: - """ - Removes all saved states of the joints of this object. - """ - for joint in self.joints.values(): - joint.remove_saved_states() - - def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: - """ - Updates the positions of all attached objects. This is done - by calculating the new pose in world coordinate frame and setting the - base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. - - :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent - loops in the update. - """ - - if already_moved_objects is None: - already_moved_objects = [] - - for child in self.attachments: - - if child in already_moved_objects: - continue - - attachment = self.attachments[child] - if not attachment.bidirectional: - self.update_attachment_with_object(child) - 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_attached_objects_poses(already_moved_objects + [self]) - - def set_position(self, position: Union[Pose, Point], base=False) -> None: - """ - Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position - instead of the origin in the center of the Object. The given position can either be a Pose, - in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. - - :param position: Target position as xyz. - :param base: If the bottom of the Object should be placed or the origin in the center. - """ - pose = Pose() - if isinstance(position, Pose): - target_position = position.position - pose.frame = position.frame - elif isinstance(position, Point): - target_position = position - elif isinstance(position, list): - target_position = position - else: - raise TypeError("The given position has to be a Pose, Point or a list of xyz.") - - pose.position = target_position - pose.orientation = self.get_orientation() - self.set_pose(pose, base=base) - - def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: - """ - Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only - the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. - - :param orientation: Target orientation given as a list of xyzw. - """ - pose = Pose() - if isinstance(orientation, Pose): - target_orientation = orientation.orientation - pose.frame = orientation.frame - else: - target_orientation = orientation - - pose.pose.position = self.get_position() - pose.pose.orientation = target_orientation - self.set_pose(pose) - - def get_joint_id(self, name: str) -> int: - """ - Returns the unique id for a joint name. As used by the world/simulator. - - :param name: The joint name - :return: The unique id - """ - return self.joint_name_to_id[name] - - def get_root_link_description(self) -> LinkDescription: - """ - Returns the root link of the URDF of this object. - :return: The root link as defined in the URDF of this object. - """ - root_link_name = self.description.get_root() - for link_description in self.description.links: - if link_description.name == root_link_name: - return link_description - - @property - def root_link(self) -> Link: - """ - Returns the root link of this object. - :return: The root link of this object. - """ - return self.links[self.description.get_root()] - - @property - def root_link_name(self) -> str: - """ - Returns the name of the root link of this object. - :return: The name of the root link of this object. - """ - return self.description.get_root() - - def get_root_link_id(self) -> int: - """ - Returns the unique id of the root link of this object. - :return: The unique id of the root link of this object. - """ - return self.get_link_id(self.description.get_root()) - - def get_link_id(self, link_name: str) -> int: - """ - Returns a unique id for a link name. - :param link_name: The name of the link. - :return: The unique id of the link. - """ - assert link_name is not None - return self.link_name_to_id[link_name] - - def get_link_by_id(self, link_id: int) -> Link: - """ - Returns the link for a given unique link id - :param link_id: The unique id of the link. - :return: The link object. - """ - return self.links[self.link_id_to_name[link_id]] - - def get_joint_by_id(self, joint_id: int) -> str: - """ - Returns the joint name for a unique world id - - :param joint_id: The world id of for joint - :return: The joint name - """ - return dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys()))[joint_id] - - 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_positions = [0] * len(joint_names) - self.set_joint_positions(dict(zip(joint_names, joint_positions))) - - def set_joint_positions(self, joint_poses: dict) -> 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: - """ - for joint_name, joint_position in joint_poses.items(): - self.joints[joint_name].position = joint_position - # 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: - """ - 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: - return self.joints[joint_name].position - - def get_joint_damping(self, joint_name: str) -> float: - return self.joints[joint_name].damping - - def get_joint_upper_limit(self, joint_name: str) -> float: - return self.joints[joint_name].upper_limit - - def get_joint_lower_limit(self, joint_name: str) -> float: - return self.joints[joint_name].lower_limit - - def get_joint_axis(self, joint_name: str) -> Point: - return self.joints[joint_name].axis - - def get_joint_type(self, joint_name: str) -> JointType: - return self.joints[joint_name].type - - def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: - return self.joints[joint_name].limits - - def find_joint_above(self, link_name: str, joint_type: JointType) -> str: - """ - Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. - - :param link_name: AbstractLink name above which the joint should be found - :param joint_type: Joint type that should be searched for - :return: Name of the first joint which has the given type - """ - chain = self.description.get_chain(self.description.get_root(), link_name) - reversed_chain = reversed(chain) - container_joint = None - for element in reversed_chain: - if element in self.joint_name_to_id and self.get_joint_type(element) == joint_type: - container_joint = element - break - if not container_joint: - rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") - return container_joint - - 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. - - :return: A dictionary with all joints positions'. - """ - return {j.name: j.position for j in self.joints.values()} - - def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: - """ - Updates the transforms of all links of this object using time 'transform_time' or the current ros time. - """ - for link in self.links.values(): - link.update_transform(transform_time) - - def contact_points(self) -> List: - """ - Returns a list of contact points of this Object with other Objects. - - :return: A list of all contact points with other objects - """ - return self.world.get_object_contact_points(self) - - def contact_points_simulated(self) -> List: - """ - Returns a list of all contact points between this Object and other Objects after stepping the simulation once. - - :return: A list of contact points between this Object and other Objects - """ - state_id = self.world.save_state() - self.world.step() - contact_points = self.contact_points() - self.world.restore_state(state_id) - return contact_points - - def set_color(self, rgba_color: Color) -> None: - """ - Changes the color of this object, the color has to be given as a list - of RGBA values. - - :param rgba_color: The color as Color object with RGBA values between 0 and 1 - """ - # Check if there is only one link, this is the case for primitive - # forms or if loaded from an .stl or .obj file - if self.links != {}: - for link in self.links.values(): - link.color = rgba_color - else: - self.root_link.color = rgba_color - - def get_color(self) -> Union[Color, Dict[str, Color]]: - """ - This method returns the rgba_color of this object. The return is either: - - 1. A Color object with RGBA values, this is the case if the object only has one link (this - happens for example if the object is spawned from a .obj or .stl file) - 2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object. - Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which - the object is spawned. - - :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and - the rgba_color as value. - """ - link_to_color_dict = self.links_colors - - if len(link_to_color_dict) == 1: - return list(link_to_color_dict.values())[0] - else: - return link_to_color_dict - - @property - def links_colors(self) -> Dict[str, Color]: - """ - The color of each link as a dictionary with link names as keys and RGBA colors as values. - """ - return self.world.get_colors_of_object_links(self) - - def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: - """ - :return: The axis aligned bounding box of this object. - """ - return self.world.get_object_axis_aligned_bounding_box(self) - - def get_base_origin(self) -> Pose: - """ - :return: the origin of the base/bottom of this object. - """ - aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() - base_width = np.absolute(aabb.min_x - aabb.max_x) - base_length = np.absolute(aabb.min_y - aabb.max_y) - return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], - self.get_pose().orientation_as_list()) - - -def filter_contact_points(contact_points, exclude_ids) -> List: - """ - Returns a list of contact points where Objects that are in the 'exclude_ids' list are removed. - - :param contact_points: A list of contact points - :param exclude_ids: A list of unique ids of Objects that should be removed from the list - :return: A list containing 'contact_points' without Objects that are in 'exclude_ids' - """ - return list(filter(lambda cp: cp[2] not in exclude_ids, contact_points)) - - -class EntityDescription(ABC): - - @property - @abstractmethod - def origin(self) -> Pose: - """ - Returns the origin of this entity. - """ - pass - - @property - @abstractmethod - def name(self) -> str: - """ - Returns the name of this entity. - """ - pass - - -class LinkDescription(EntityDescription): - """ - A class that represents a link description of an object. - """ - - def __init__(self, parsed_link_description: Any): - self.parsed_description = parsed_link_description - - @property - @abstractmethod - def geometry(self) -> Shape: - """ - Returns the geometry type of the URDF collision element of this link. - """ - pass - - -class JointDescription(EntityDescription): - """ - A class that represents a joint description of a URDF joint. - """ - - def __init__(self, parsed_joint_description: Any): - self.parsed_description = parsed_joint_description - - @property - @abstractmethod - def type(self) -> JointType: - """ - :return: The type of this joint. - """ - pass - - @property - @abstractmethod - def axis(self) -> Point: - """ - :return: The axis of this joint, for example the rotation axis for a revolute joint. - """ - pass - - @property - @abstractmethod - def has_limits(self) -> bool: - """ - Checks if this joint has limits. - :return: True if the joint has limits, False otherwise. - """ - pass - - @property - def limits(self) -> Tuple[float, float]: - """ - :return: The lower and upper limits of this joint. - """ - lower, upper = self.lower_limit, self.upper_limit - if lower > upper: - lower, upper = upper, lower - return lower, upper - - @property - @abstractmethod - def lower_limit(self) -> Union[float, None]: - """ - :return: The lower limit of this joint, or None if the joint has no limits. - """ - pass - - @property - @abstractmethod - def upper_limit(self) -> Union[float, None]: - """ - :return: The upper limit of this joint, or None if the joint has no limits. - """ - pass - - @property - @abstractmethod - def parent_link_name(self) -> str: - """ - :return: The name of the parent link of this joint. - """ - pass - - @property - @abstractmethod - def child_link_name(self) -> str: - """ - :return: The name of the child link of this joint. - """ - pass - - @property - def damping(self) -> float: - """ - :return: The damping of this joint. - """ - raise NotImplementedError - - @property - def friction(self) -> float: - """ - :return: The friction of this joint. - """ - raise NotImplementedError - - -class ObjectEntity(WorldEntity): - """ - An abstract base class that represents a physical part/entity of an Object. - This can be a link or a joint of an Object. - """ - - def __init__(self, _id: int, obj: Object): - WorldEntity.__init__(self, _id, obj.world) - self.object: Object = obj - - @property - @abstractmethod - def pose(self) -> Pose: - """ - :return: The pose of this entity relative to the world frame. - """ - pass - - @property - def transform(self) -> Transform: - """ - Returns the transform of this entity. - - :return: The transform of this entity. - """ - return self.pose.to_transform(self.tf_frame) - - @property - @abstractmethod - def tf_frame(self) -> str: - """ - Returns the tf frame of this entity. - - :return: The tf frame of this entity. - """ - pass - - @property - def object_id(self) -> int: - """ - :return: the id of the object to which this entity belongs. - """ - return self.object.id - - def __repr__(self): - return self.__class__.__qualname__ + f"(" + ', \n'.join( - [f"{key}={value}" for key, value in self.__dict__.items()]) + ")" - - def __str__(self): - return self.__repr__() - - -class AbstractConstraint: - """ - Represents an abstract constraint concept, this could be used to create joints for example or any kind of constraint - between two links in the world. - """ - - def __init__(self, - parent_link: Link, - child_link: Link, - _type: JointType, - parent_to_constraint: Transform, - child_to_constraint: Transform): - self.parent_link: Link = parent_link - self.child_link: Link = child_link - self.type: JointType = _type - self.parent_to_constraint = parent_to_constraint - self.child_to_constraint = child_to_constraint - self._parent_to_child = None - - @property - def parent_to_child_transform(self) -> Union[Transform, None]: - if self._parent_to_child is None: - if self.parent_to_constraint is not None and self.child_to_constraint is not None: - self._parent_to_child = self.parent_to_constraint * self.child_to_constraint.invert() - return self._parent_to_child - - @parent_to_child_transform.setter - def parent_to_child_transform(self, transform: Transform) -> None: - self._parent_to_child = transform - - @property - def parent_object_id(self) -> int: - """ - Returns the id of the parent object of the constraint. - - :return: The id of the parent object of the constraint - """ - return self.parent_link.object_id - - @property - def child_object_id(self) -> int: - """ - Returns the id of the child object of the constraint. - - :return: The id of the child object of the constraint - """ - return self.child_link.object_id - - @property - def parent_link_id(self) -> int: - """ - Returns the id of the parent link of the constraint. - - :return: The id of the parent link of the constraint - """ - return self.parent_link.id - - @property - def child_link_id(self) -> int: - """ - Returns the id of the child link of the constraint. - - :return: The id of the child link of the constraint - """ - return self.child_link.id - - @property - def position_wrt_parent_as_list(self) -> List[float]: - """ - Returns the constraint frame pose with respect to the parent origin as a list. - - :return: The constraint frame pose with respect to the parent origin as a list - """ - return self.pose_wrt_parent.position_as_list() - - @property - def orientation_wrt_parent_as_list(self) -> List[float]: - """ - Returns the constraint frame orientation with respect to the parent origin as a list. - - :return: The constraint frame orientation with respect to the parent origin as a list - """ - return self.pose_wrt_parent.orientation_as_list() - - @property - def pose_wrt_parent(self) -> Pose: - """ - Returns the joint frame pose with respect to the parent origin. - - :return: The joint frame pose with respect to the parent origin - """ - return self.parent_to_constraint.to_pose() - - @property - def position_wrt_child_as_list(self) -> List[float]: - """ - Returns the constraint frame pose with respect to the child origin as a list. - - :return: The constraint frame pose with respect to the child origin as a list - """ - return self.pose_wrt_child.position_as_list() - - @property - def orientation_wrt_child_as_list(self) -> List[float]: - """ - Returns the constraint frame orientation with respect to the child origin as a list. - - :return: The constraint frame orientation with respect to the child origin as a list - """ - return self.pose_wrt_child.orientation_as_list() - - @property - def pose_wrt_child(self) -> Pose: - """ - Returns the joint frame pose with respect to the child origin. - - :return: The joint frame pose with respect to the child origin - """ - return self.child_to_constraint.to_pose() - - -class Constraint(AbstractConstraint): - """ - Represents a constraint between two links in the World. - """ - - def __init__(self, - parent_link: Link, - child_link: Link, - _type: JointType, - axis_in_child_frame: Point, - constraint_to_parent: Transform, - child_to_constraint: Transform): - parent_to_constraint = constraint_to_parent.invert() - AbstractConstraint.__init__(self, parent_link, child_link, _type, parent_to_constraint, child_to_constraint) - self.axis: Point = axis_in_child_frame - - @property - def axis_as_list(self) -> List[float]: - """ - Returns the axis of this constraint as a list. - - :return: The axis of this constraint as a list of xyz - """ - return [self.axis.x, self.axis.y, self.axis.z] - - -class Joint(ObjectEntity, JointDescription, ABC): - """ - Represents a joint of an Object in the World. - """ - - def __init__(self, _id: int, - joint_description: JointDescription, - obj: Object): - ObjectEntity.__init__(self, _id, obj) - JointDescription.__init__(self, joint_description.parsed_description) - self._update_position() - - @property - def tf_frame(self) -> str: - """ - The tf frame of a joint is the tf frame of the child link. - """ - return self.child_link.tf_frame - - @property - def pose(self) -> Pose: - """ - Returns the pose of this joint. The pose is the pose of the child link of this joint. - - :return: The pose of this joint. - """ - return self.child_link.pose - - def _update_position(self) -> None: - """ - Updates the current position of the joint from the physics simulator. - """ - self._current_position = self.world.get_joint_position(self.object, self.name) - - @property - def parent_link(self) -> Link: - """ - Returns the parent link of this joint. - :return: The parent link as a AbstractLink object. - """ - return self.object.links[self.parent_link_name] - - @property - def child_link(self) -> Link: - """ - Returns the child link of this joint. - :return: The child link as a AbstractLink object. - """ - return self.object.links[self.child_link_name] - - @property - def position(self) -> float: - return self._current_position - - def reset_position(self, position: float) -> None: - self.world.reset_joint_position(self.object, self.name, position) - self._update_position() - - def get_object_id(self) -> int: - """ - Returns the id of the object to which this joint belongs. - :return: The integer id of the object to which this joint belongs. - """ - return self.object.id - - @position.setter - def position(self, joint_position: float) -> None: - """ - Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, as stated - in the URDF, an error will be printed. However, the joint will be set either way. - - :param joint_position: The target pose for this joint - """ - # TODO Limits for rotational (infinitie) joints are 0 and 1, they should be considered seperatly - if self.has_limits: - low_lim, up_lim = self.limits - if not low_lim <= joint_position <= up_lim: - logging.error( - f"The joint position has to be within the limits of the joint. The joint limits for {self.name}" - f" are {low_lim} and {up_lim}") - logging.error(f"The given joint position was: {joint_position}") - # Temporarily disabled because kdl outputs values exciting joint limits - # return - self.reset_position(joint_position) - - def enable_force_torque_sensor(self) -> None: - self.world.enable_joint_force_torque_sensor(self.object, self.id) - - def disable_force_torque_sensor(self) -> None: - self.world.disable_joint_force_torque_sensor(self.object, self.id) - - def get_reaction_force_torque(self) -> List[float]: - return self.world.get_joint_reaction_force_torque(self.object, self.id) - - def get_applied_motor_torque(self) -> float: - return self.world.get_applied_joint_motor_torque(self.object, self.id) - - def restore_state(self, state_id: int) -> None: - self.current_state = self.saved_states[state_id] - - @property - def current_state(self) -> JointState: - return JointState(self.position) - - @current_state.setter - def current_state(self, joint_state: JointState) -> None: - self.position = joint_state.position - - -class Link(ObjectEntity, LinkDescription, ABC): - """ - Represents a link of an Object in the World. - """ - - def __init__(self, _id: int, link_description: LinkDescription, obj: Object): - ObjectEntity.__init__(self, _id, obj) - LinkDescription.__init__(self, link_description.parsed_description) - self.local_transformer: LocalTransformer = LocalTransformer() - self.constraint_ids: Dict[Link, int] = {} - self._update_pose() - - @property - def current_state(self) -> LinkState: - return LinkState(self.constraint_ids.copy()) - - @current_state.setter - 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) -> 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. - :return: The unique id of the constraint. - """ - constraint_id = self.world.add_fixed_constraint(self, - child_link, - child_link.get_transform_from_link(self)) - self.constraint_ids[child_link] = constraint_id - child_link.constraint_ids[self] = constraint_id - return constraint_id - - def remove_constraint_with_link(self, child_link: Link) -> None: - """ - Removes the constraint between this link and the given link. - :param child_link: The child link of the constraint that should be removed. - """ - self.world.remove_constraint(self.constraint_ids[child_link]) - del self.constraint_ids[child_link] - del child_link.constraint_ids[self] - - @property - def is_root(self) -> bool: - """ - Returns whether this link is the root link of the object. - :return: True if this link is the root link, False otherwise. - """ - return self.object.get_root_link_id() == self.id - - def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: - """ - Updates the transformation of this link at the given time. - :param transform_time: The time at which the transformation should be updated. - """ - self.local_transformer.update_transforms([self.transform], transform_time) - - def get_transform_to_link(self, link: Link) -> Transform: - """ - Returns the transformation from this link to the given link. - :param link: The link to which the transformation should be returned. - :return: A Transform object with the transformation from this link to the given link. - """ - return link.get_transform_from_link(self) - - def get_transform_from_link(self, link: Link) -> Transform: - """ - Returns the transformation from the given link to this link. - :param link: The link from which the transformation should be returned. - :return: A Transform object with the transformation from the given link to this link. - """ - return self.get_pose_wrt_link(link).to_transform(self.tf_frame) - - def get_pose_wrt_link(self, link: Link) -> Pose: - """ - Returns the pose of this link with respect to the given link. - :param link: The link with respect to which the pose should be returned. - :return: A Pose object with the pose of this link with respect to the given link. - """ - return self.local_transformer.transform_pose(self.pose, link.tf_frame) - - def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: - """ - Returns the axis aligned bounding box of this link. - :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. - """ - return self.world.get_link_axis_aligned_bounding_box(self) - - @property - def position(self) -> Point: - """ - The getter for the position of the link relative to the world frame. - :return: A Point object containing the position of the link relative to the world frame. - """ - return self.pose.position - - @property - def position_as_list(self) -> List[float]: - """ - The getter for the position of the link relative to the world frame as a list. - :return: A list containing the position of the link relative to the world frame. - """ - return self.pose.position_as_list() - - @property - def orientation(self) -> Quaternion: - """ - The getter for the orientation of the link relative to the world frame. - :return: A Quaternion object containing the orientation of the link relative to the world frame. - """ - return self.pose.orientation - - @property - def orientation_as_list(self) -> List[float]: - """ - The getter for the orientation of the link relative to the world frame as a list. - :return: A list containing the orientation of the link relative to the world frame. - """ - return self.pose.orientation_as_list() - - def _update_pose(self) -> None: - """ - Updates the current pose of this link from the world. - """ - self._current_pose = self.world.get_link_pose(self) - - @property - def pose(self) -> Pose: - """ - The pose of the link relative to the world frame. - :return: A Pose object containing the pose of the link relative to the world frame. - """ - return self._current_pose - - @property - def pose_as_list(self) -> List[List[float]]: - """ - The pose of the link relative to the world frame as a list. - :return: A list containing the position and orientation of the link relative to the world frame. - """ - return self.pose.to_list() - - def get_origin_transform(self) -> Transform: - """ - Returns the transformation between the link frame and the origin frame of this link. - """ - return self.origin.to_transform(self.tf_frame) - - @property - def color(self) -> Color: - """ - The getter for the rgba_color of this link. - :return: A Color object containing the rgba_color of this link. - """ - return self.world.get_link_color(self) - - @color.setter - def color(self, color: List[float]) -> None: - """ - The setter for the color of this link, could be rgb or rgba. - :param color: The color as a list of floats, either rgb or rgba. - """ - self.world.set_link_color(self, Color.from_list(color)) - - @property - def origin_transform(self) -> Transform: - """ - :return: The transform from world to origin of entity. - """ - return self.origin.to_transform(self.tf_frame) - - @property - def tf_frame(self) -> str: - """ - The name of the tf frame of this link. - """ - return f"{self.object.tf_frame}/{self.name}" - - -class RootLink(Link, ABC): - """ - Represents the root link of an Object in the World. - It differs from the normal AbstractLink class in that the pose ande the tf_frame is the same as that of the object. - """ - - def __init__(self, obj: Object): - super().__init__(obj.get_root_link_id(), obj.get_root_link_description(), obj) - - @property - def tf_frame(self) -> str: - """ - Returns the tf frame of the root link, which is the same as the tf frame of the object. - """ - return self.object.tf_frame - - def _update_pose(self) -> None: - self._current_pose = self.object.get_pose() - - -class ObjectDescription(EntityDescription): - MESH_EXTENSIONS: Tuple[str] = (".obj", ".stl") - - class Link(Link, ABC): - ... - - class RootLink(RootLink, ABC): - ... - - class Joint(Joint, ABC): - ... - - def __init__(self, path: Optional[str] = None): - if path: - self.update_description_from_file(path) - else: - self._parsed_description = None - - def update_description_from_file(self, path: str) -> None: - """ - Updates the description of this object from the file at the given path. - :param path: The path of the file to update from. - """ - self._parsed_description = self.load_description(path) - - @property - def parsed_description(self) -> Any: - """ - Return the object parsed from the description file. - """ - return self._parsed_description - - @parsed_description.setter - def parsed_description(self, parsed_description: Any): - """ - Return the object parsed from the description file. - :param parsed_description: The parsed description object. - """ - self._parsed_description = parsed_description - - @abstractmethod - def load_description(self, path: str) -> Any: - """ - Loads the description from the file at the given path. - :param path: The path to the source file, if only a filename is provided then the resources directories will be - searched. - """ - pass - - def generate_description_from_file(self, path: str, extension: str) -> str: - """ - Generates and preprocesses the description from the file at the given path and returns the preprocessed - description as a string. - :param path: The path of the file to preprocess. - :param extension: The file extension of the file to preprocess. - :return: The processed description string. - """ - - if extension in self.MESH_EXTENSIONS: - description_string = self.generate_from_mesh_file(path) - elif extension == self.get_file_extension(): - description_string = self.generate_from_description_file(path) - else: - # Using the description from the parameter server - description_string = self.generate_from_parameter_server(path) - - return description_string - - def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: str) -> str: - """ - Returns the file name of the description file. - :param path_object: The path object of the description file or the mesh file. - :param extension: The file extension of the description file or the mesh file. - :param object_name: The name of the object. - :return: The file name of the description file. - """ - if extension in self.MESH_EXTENSIONS: - file_name = path_object.stem + self.get_file_extension() - elif extension == self.get_file_extension(): - file_name = path_object.name - else: - file_name = object_name + self.get_file_extension() - - return file_name - - @classmethod - @abstractmethod - def generate_from_mesh_file(cls, path: str) -> str: - """ - Generates a description file from one of the mesh types defined in the MESH_EXTENSIONS and - returns the path of the generated file. - :param path: The path to the .obj file. - :return: The path of the generated description file. - """ - pass - - @classmethod - @abstractmethod - def generate_from_description_file(cls, path: str) -> str: - """ - Preprocesses the given file and returns the preprocessed description string. - :param path: The path of the file to preprocess. - :return: The preprocessed description string. - """ - pass - - @classmethod - @abstractmethod - def generate_from_parameter_server(cls, name: str) -> str: - """ - Preprocesses the description from the ROS parameter server and returns the preprocessed description string. - :param name: The name of the description on the parameter server. - :return: The preprocessed description string. - """ - pass - - @property - @abstractmethod - def links(self) -> List[LinkDescription]: - """ - :return: A list of links descriptions of this object. - """ - pass - - @abstractmethod - def get_link_by_name(self, link_name: str) -> LinkDescription: - """ - :return: The link description with the given name. - """ - pass - - @property - @abstractmethod - def joints(self) -> List[JointDescription]: - """ - :return: A list of joints descriptions of this object. - """ - pass - - @abstractmethod - def get_joint_by_name(self, joint_name: str) -> JointDescription: - """ - :return: The joint description with the given name. - """ - pass - - @abstractmethod - def get_root(self) -> str: - """ - :return: the name of the root link of this object. - """ - pass - - @abstractmethod - def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: - """ - :return: the chain of links from 'start_link_name' to 'end_link_name'. - """ - pass - - @staticmethod - @abstractmethod - def get_file_extension() -> str: - """ - :return: The file extension of the description file. - """ - pass - - -class Attachment(AbstractConstraint): - def __init__(self, - parent_link: Link, - child_link: Link, - bidirectional: Optional[bool] = False, - parent_to_child_transform: Optional[Transform] = None, - constraint_id: Optional[int] = None): - """ - Creates an attachment between the parent object link and the child object link. - This could be a bidirectional attachment, meaning that both objects will move when one moves. - :param parent_link: The parent object link. - :param child_link: The child object link. - :param bidirectional: If true, both objects will move when one moves. - :param parent_to_child_transform: The transform from the parent link to the child object link. - :param constraint_id: The id of the constraint in the simulator. - """ - super().__init__(parent_link, child_link, JointType.FIXED, parent_to_child_transform, - Transform(frame=child_link.tf_frame)) - self.id = constraint_id - self.bidirectional: bool = bidirectional - self._loose: bool = False - - if self.parent_to_child_transform is None: - self.update_transform() - - if self.id is None: - self.add_fixed_constraint() - - def update_transform_and_constraint(self) -> None: - """ - Updates the transform and constraint of this attachment. - """ - self.update_transform() - self.update_constraint() - - def update_transform(self) -> None: - """ - Updates the transform of this attachment by calculating the transform from the parent link to the child link. - """ - self.parent_to_child_transform = self.calculate_transform() - - def update_constraint(self) -> None: - """ - Updates the constraint of this attachment by removing the old constraint if one exists and adding a new one. - """ - self.remove_constraint_if_exists() - self.add_fixed_constraint() - - 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) - - def calculate_transform(self) -> Transform: - """ - Calculates the transform from the parent link to the child link. - """ - return self.parent_link.get_transform_to_link(self.child_link) - - def remove_constraint_if_exists(self) -> None: - """ - Removes the constraint between the parent and the child links if one exists. - """ - if self.child_link in self.parent_link.constraint_ids: - self.parent_link.remove_constraint_with_link(self.child_link) - - def get_inverse(self) -> Attachment: - """ - :return: A new Attachment object with the parent and child links swapped. - """ - attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, - constraint_id=self.id) - attachment.loose = not self._loose - return attachment - - @property - def loose(self) -> bool: - """ - If true, then the child object will not move when parent moves. - """ - return self._loose - - @loose.setter - def loose(self, loose: bool) -> None: - """ - Sets the loose property of this attachment. - :param loose: If true, then the child object will not move when parent moves. - """ - self._loose = loose and not self.bidirectional - - @property - def is_reversed(self) -> bool: - """ - :return: True if the parent and child links are swapped. - """ - return self.loose - - def __del__(self) -> None: - """ - Removes the constraint between the parent and the child links if one exists when the attachment is deleted. - """ - self.remove_constraint_if_exists() - - -class CacheManager: - cache_dir: str = World.cache_dir - """ - The directory where the cached files are stored. - """ - - mesh_extensions: List[str] = [".obj", ".stl"] - """ - The file extensions of mesh files. - """ - - def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - object_description: ObjectDescription, object_name: str) -> str: - """ - Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. - """ - path_object = pathlib.Path(path) - extension = path_object.suffix - - path = self.look_for_file_in_data_dir(path, path_object) - - self.create_cache_dir_if_not_exists() - - # save correct path in case the file is already in the cache directory - cache_path = self.cache_dir + object_description.get_file_name(path_object, extension, object_name) - - # if file is not yet cached preprocess the description file and save it in the cache directory. - if not self.is_cached(path, object_description) or ignore_cached_files: - self.generate_description_and_write_to_cache(path, extension, cache_path, object_description) - - return cache_path - - def generate_description_and_write_to_cache(self, path: str, extension: str, cache_path: str, - object_description: ObjectDescription) -> None: - """ - Generates the description from the file at the given path and writes it to the cache directory. - :param path: The path of the file to preprocess. - :param extension: The file extension of the file to preprocess. - :param cache_path: The path of the file in the cache directory. - :param object_description: The object description of the file. - """ - description_string = object_description.generate_description_from_file(path, extension) - self.write_to_cache(description_string, cache_path) - - @staticmethod - def write_to_cache(description_string: str, cache_path: str) -> None: - """ - Writes the description string to the cache directory. - :param description_string: The description string to write to the cache directory. - :param cache_path: The path of the file in the cache directory. - """ - with open(cache_path, "w") as file: - file.write(description_string) - - @staticmethod - def look_for_file_in_data_dir(path: str, path_object: pathlib.Path) -> str: - """ - Looks for a file in the data directory of the World. If the file is not found in the data directory, this method - raises a FileNotFoundError. - :param path: The path of the file to look for. - :param path_object: The pathlib object of the file to look for. - """ - if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): - for data_dir in World.data_directory: - for file in os.listdir(data_dir): - if file == path: - return data_dir + f"/{path}" - if path: - break - - if not path: - raise FileNotFoundError( - f"File {path_object.name} could not be found in the resource directory {World.data_directory}") - - return path - - def create_cache_dir_if_not_exists(self): - """ - Creates the cache directory if it does not exist. - """ - if not pathlib.Path(self.cache_dir).exists(): - os.mkdir(self.cache_dir) - - def is_cached(self, path: str, object_description: ObjectDescription) -> bool: - """ - Checks if the file in the given path is already cached or if - there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from - the parameter server is used. - - :param path: The path of the file to check. - :param object_description: The object description of the file. - :return: True if there already exists a cached file, False in any other case. - """ - return True if self.check_with_extension(path) else self.check_without_extension(path, object_description) - - def check_with_extension(self, path: str) -> bool: - """ - Checks if the file in the given ath exists in the cache directory including file extension. - :param path: The path of the file to check. - """ - file_name = pathlib.Path(path).name - full_path = pathlib.Path(self.cache_dir + file_name) - return full_path.exists() - - def check_without_extension(self, path: str, object_description: ObjectDescription) -> bool: - """ - Checks if the file in the given path exists in the cache directory without file extension, - the extension is added after the file name manually in this case. - :param path: The path of the file to check. - :param object_description: The object description of the file. - """ - file_stem = pathlib.Path(path).stem - full_path = pathlib.Path(self.cache_dir + file_stem + object_description.get_file_extension()) - return full_path.exists() diff --git a/src/pycram/world_constraints.py b/src/pycram/world_constraints.py new file mode 100644 index 000000000..f301f0ff9 --- /dev/null +++ b/src/pycram/world_constraints.py @@ -0,0 +1,257 @@ +from geometry_msgs.msg import Point +from typing_extensions import Union, List, Optional + +from pycram.enums import JointType +from pycram.pose import Transform, Pose + + +class AbstractConstraint: + """ + Represents an abstract constraint concept, this could be used to create joints for example or any kind of constraint + between two links in the world. + """ + + def __init__(self, + parent_link: 'Link', + child_link: 'Link', + _type: JointType, + parent_to_constraint: Transform, + child_to_constraint: Transform): + self.parent_link: 'Link' = parent_link + self.child_link: 'Link' = child_link + self.type: JointType = _type + self.parent_to_constraint = parent_to_constraint + self.child_to_constraint = child_to_constraint + self._parent_to_child = None + + @property + def parent_to_child_transform(self) -> Union[Transform, None]: + if self._parent_to_child is None: + if self.parent_to_constraint is not None and self.child_to_constraint is not None: + self._parent_to_child = self.parent_to_constraint * self.child_to_constraint.invert() + return self._parent_to_child + + @parent_to_child_transform.setter + def parent_to_child_transform(self, transform: Transform) -> None: + self._parent_to_child = transform + + @property + def parent_object_id(self) -> int: + """ + Returns the id of the parent object of the constraint. + + :return: The id of the parent object of the constraint + """ + return self.parent_link.object_id + + @property + def child_object_id(self) -> int: + """ + Returns the id of the child object of the constraint. + + :return: The id of the child object of the constraint + """ + return self.child_link.object_id + + @property + def parent_link_id(self) -> int: + """ + Returns the id of the parent link of the constraint. + + :return: The id of the parent link of the constraint + """ + return self.parent_link.id + + @property + def child_link_id(self) -> int: + """ + Returns the id of the child link of the constraint. + + :return: The id of the child link of the constraint + """ + return self.child_link.id + + @property + def position_wrt_parent_as_list(self) -> List[float]: + """ + Returns the constraint frame pose with respect to the parent origin as a list. + + :return: The constraint frame pose with respect to the parent origin as a list + """ + return self.pose_wrt_parent.position_as_list() + + @property + def orientation_wrt_parent_as_list(self) -> List[float]: + """ + Returns the constraint frame orientation with respect to the parent origin as a list. + + :return: The constraint frame orientation with respect to the parent origin as a list + """ + return self.pose_wrt_parent.orientation_as_list() + + @property + def pose_wrt_parent(self) -> Pose: + """ + Returns the joint frame pose with respect to the parent origin. + + :return: The joint frame pose with respect to the parent origin + """ + return self.parent_to_constraint.to_pose() + + @property + def position_wrt_child_as_list(self) -> List[float]: + """ + Returns the constraint frame pose with respect to the child origin as a list. + + :return: The constraint frame pose with respect to the child origin as a list + """ + return self.pose_wrt_child.position_as_list() + + @property + def orientation_wrt_child_as_list(self) -> List[float]: + """ + Returns the constraint frame orientation with respect to the child origin as a list. + + :return: The constraint frame orientation with respect to the child origin as a list + """ + return self.pose_wrt_child.orientation_as_list() + + @property + def pose_wrt_child(self) -> Pose: + """ + Returns the joint frame pose with respect to the child origin. + + :return: The joint frame pose with respect to the child origin + """ + return self.child_to_constraint.to_pose() + + +class Constraint(AbstractConstraint): + """ + Represents a constraint between two links in the World. + """ + + def __init__(self, + parent_link: 'Link', + child_link: 'Link', + _type: JointType, + axis_in_child_frame: Point, + constraint_to_parent: Transform, + child_to_constraint: Transform): + parent_to_constraint = constraint_to_parent.invert() + AbstractConstraint.__init__(self, parent_link, child_link, _type, parent_to_constraint, child_to_constraint) + self.axis: Point = axis_in_child_frame + + @property + def axis_as_list(self) -> List[float]: + """ + Returns the axis of this constraint as a list. + + :return: The axis of this constraint as a list of xyz + """ + return [self.axis.x, self.axis.y, self.axis.z] + + +class Attachment(AbstractConstraint): + def __init__(self, + parent_link: 'Link', + child_link: 'Link', + bidirectional: Optional[bool] = False, + parent_to_child_transform: Optional[Transform] = None, + constraint_id: Optional[int] = None): + """ + Creates an attachment between the parent object link and the child object link. + This could be a bidirectional attachment, meaning that both objects will move when one moves. + :param parent_link: The parent object link. + :param child_link: The child object link. + :param bidirectional: If true, both objects will move when one moves. + :param parent_to_child_transform: The transform from the parent link to the child object link. + :param constraint_id: The id of the constraint in the simulator. + """ + super().__init__(parent_link, child_link, JointType.FIXED, parent_to_child_transform, + Transform(frame=child_link.tf_frame)) + self.id = constraint_id + self.bidirectional: bool = bidirectional + self._loose: bool = False + + if self.parent_to_child_transform is None: + self.update_transform() + + if self.id is None: + self.add_fixed_constraint() + + def update_transform_and_constraint(self) -> None: + """ + Updates the transform and constraint of this attachment. + """ + self.update_transform() + self.update_constraint() + + def update_transform(self) -> None: + """ + Updates the transform of this attachment by calculating the transform from the parent link to the child link. + """ + self.parent_to_child_transform = self.calculate_transform() + + def update_constraint(self) -> None: + """ + Updates the constraint of this attachment by removing the old constraint if one exists and adding a new one. + """ + self.remove_constraint_if_exists() + self.add_fixed_constraint() + + 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) + + def calculate_transform(self) -> Transform: + """ + Calculates the transform from the parent link to the child link. + """ + return self.parent_link.get_transform_to_link(self.child_link) + + def remove_constraint_if_exists(self) -> None: + """ + Removes the constraint between the parent and the child links if one exists. + """ + if self.child_link in self.parent_link.constraint_ids: + self.parent_link.remove_constraint_with_link(self.child_link) + + def get_inverse(self) -> 'Attachment': + """ + :return: A new Attachment object with the parent and child links swapped. + """ + attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, + constraint_id=self.id) + attachment.loose = not self._loose + return attachment + + @property + def loose(self) -> bool: + """ + If true, then the child object will not move when parent moves. + """ + return self._loose + + @loose.setter + def loose(self, loose: bool) -> None: + """ + Sets the loose property of this attachment. + :param loose: If true, then the child object will not move when parent moves. + """ + self._loose = loose and not self.bidirectional + + @property + def is_reversed(self) -> bool: + """ + :return: True if the parent and child links are swapped. + """ + return self.loose + + def __del__(self) -> None: + """ + Removes the constraint between the parent and the child links if one exists when the attachment is deleted. + """ + self.remove_constraint_if_exists() diff --git a/src/pycram/world_object.py b/src/pycram/world_object.py new file mode 100644 index 000000000..5e2eabf69 --- /dev/null +++ b/src/pycram/world_object.py @@ -0,0 +1,822 @@ +import logging +import os + +import numpy as np +import rospy +from geometry_msgs.msg import Point, Quaternion +from typing_extensions import Type, Optional, Dict, Tuple, List, Union + +from pycram.enums import ObjectType, JointType +from pycram.local_transformer import LocalTransformer +from pycram.pose import Pose +from pycram.robot_descriptions import robot_description +from pycram.world import WorldEntity, World +from pycram.world_constraints import Attachment +from pycram.world_dataclasses import Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox + + +class Object(WorldEntity): + """ + Represents a spawned Object in the World. + """ + + prospection_world_prefix: str = "prospection/" + """ + The ObjectDescription of the object, this contains the name and type of the object as well as the path to the source + file. + """ + + def __init__(self, name: str, obj_type: ObjectType, path: str, + description: Type['ObjectDescription'], + pose: Optional[Pose] = None, + world: Optional[World] = None, + color: Optional[Color] = Color(), + ignore_cached_files: Optional[bool] = False): + """ + The constructor loads the description file into the given World, if no World is specified the + :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. + The rgba_color parameter is only used when loading .stl or .obj files, + for URDFs :func:`~Object.set_color` can be used. + + :param name: The name of the object + :param obj_type: The type of the object as an ObjectType enum. + :param path: The path to the source file, if only a filename is provided then the resources directories will be + searched. + :param description: The ObjectDescription of the object, this contains the joints and links of the object. + :param pose: The pose at which the Object should be spawned + :param world: The World in which the object should be spawned, + if no world is specified the :py:attr:`~World.current_world` will be used. + :param color: The rgba_color with which the object should be spawned. + :param ignore_cached_files: If true the file will be spawned while ignoring cached files. + """ + + super().__init__(-1, world) + + if pose is None: + pose = Pose() + + self.name: str = name + self.obj_type: ObjectType = obj_type + self.color: Color = color + self.description = description() + self.cache_manager = self.world.cache_manager + + self.local_transformer = LocalTransformer() + self.original_pose = self.local_transformer.transform_pose(pose, "map") + self._current_pose = self.original_pose + + self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) + + self.description.update_description_from_file(self.path) + + self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") + + f"{self.name}_{self.id}") + + self._update_object_description_from_file(self.path) + + if self.description.name == robot_description.name: + self.world.set_robot_if_not_set(self) + + self._init_joint_name_and_id_map() + self._init_link_name_and_id_map() + + self._init_links_and_update_transforms() + self._init_joints() + + self.attachments: Dict[Object, Attachment] = {} + + if not self.world.is_prospection_world: + self._add_to_world_sync_obj_queue() + + self.world.objects.append(self) + + def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, str]: + """ + Loads an object to the given World with the given position and orientation. The rgba_color will only be + used when an .obj or .stl file is given. + If a .obj or .stl file is given, before spawning, an urdf file with the .obj or .stl as mesh will be created + and this URDf file will be loaded instead. + When spawning a URDf file a new file will be created in the cache directory, if there exists none. + This new file will have resolved mesh file paths, meaning there will be no references + to ROS packges instead there will be absolute file paths. + + :param ignore_cached_files: Whether to ignore files in the cache directory. + :return: The unique id of the object and the path of the file that was loaded. + """ + + path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) + + try: + obj_id = self.world.load_description_and_get_object_id(path, Pose(self.get_position_as_list(), + self.get_orientation_as_list())) + return obj_id, path + except Exception as e: + logging.error( + "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" + " the name of an URDF string on the parameter server.") + os.remove(path) + raise (e) + + def _update_object_description_from_file(self, path: str) -> None: + """ + Updates the object description from the given file path. + :param path: The path to the file from which the object description should be updated. + """ + self.description.update_description_from_file(path) + + def _init_joint_name_and_id_map(self) -> None: + """ + Creates a dictionary which maps the joint names to their unique ids and vice versa. + """ + n_joints = len(self.joint_names) + self.joint_name_to_id = dict(zip(self.joint_names, range(n_joints))) + self.joint_id_to_name = dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys())) + + def _init_link_name_and_id_map(self) -> None: + """ + Creates a dictionary which maps the link names to their unique ids and vice versa. + """ + n_links = len(self.link_names) + self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) + self.link_name_to_id[self.description.get_root()] = -1 + self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) + + def _init_links_and_update_transforms(self) -> None: + """ + Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the + corresponding link objects. + """ + self.links = {} + for link_name, link_id in self.link_name_to_id.items(): + link_description = self.description.get_link_by_name(link_name) + if link_name == self.description.get_root(): + self.links[link_name] = self.description.RootLink(self) + else: + self.links[link_name] = self.description.Link(link_id, link_description, self) + + self.update_link_transforms() + + def _init_joints(self): + """ + Initialize the joint objects from the URDF file and creates a dictionary which mas the joint names to the + corresponding joint objects + """ + 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) + + def _add_to_world_sync_obj_queue(self) -> None: + """ + Adds this object to the objects queue of the WorldSync object of the World. + """ + self.world.world_sync.add_obj_queue.put( + [self.name, self.obj_type, self.path, type(self.description), self.get_position_as_list(), + self.get_orientation_as_list(), + self.world.prospection_world, self.color, self]) + + @property + def link_names(self) -> List[str]: + """ + :return: The name of each link as a list. + """ + return self.world.get_object_link_names(self) + + @property + def number_of_links(self) -> int: + """ + :return: The number of links of this object. + """ + return len(self.description.links) + + @property + def joint_names(self) -> List[str]: + """ + :return: The name of each joint as a list. + """ + return self.world.get_object_joint_names(self) + + @property + def number_of_joints(self) -> int: + """ + :return: The number of joints of this object. + """ + return len(self.description.joints) + + @property + def base_origin_shift(self) -> np.ndarray: + """ + The shift between the base of the object and the origin of the object. + :return: A numpy array with the shift between the base of the object and the origin of the object. + """ + return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) + + def __repr__(self): + skip_attr = ["links", "joints", "description", "attachments"] + return self.__class__.__qualname__ + f"(" + ', \n'.join( + [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" + + def remove(self) -> None: + """ + Removes this object from the World it currently resides in. + For the object to be removed it has to be detached from all objects it + is currently attached to. After this is done a call to world remove object is done + to remove this Object from the simulation/world. + """ + self.world.remove_object(self) + + def reset(self, remove_saved_states=True) -> None: + """ + Resets the Object to the state it was first spawned in. + All attached objects will be detached, all joints will be set to the + default position of 0 and the object will be set to the position and + orientation in which it was spawned. + :param remove_saved_states: If True the saved states will be removed. + """ + self.detach_all() + self.reset_all_joints_positions() + self.set_pose(self.original_pose) + if remove_saved_states: + self.remove_saved_states() + + def attach(self, + child_object: 'Object', + parent_link: Optional[str] = None, + child_link: Optional[str] = None, + bidirectional: Optional[bool] = True) -> None: + """ + Attaches another object to this object. This is done by + saving the transformation between the given link, if there is one, and + the base pose of the other object. Additionally, the name of the link, to + which the object is attached, will be saved. + Furthermore, a simulator constraint will be created so the attachment + also works while simulation. + Loose attachments means that the attachment will only be one-directional. For example, if this object moves the + other, attached, object will also move but not the other way around. + + :param child_object: The other object that should be attached. + :param parent_link: The link name of this object. + :param child_link: The link name of the other object. + :param bidirectional: If the attachment should be a loose attachment. + """ + parent_link = self.links[parent_link] if parent_link else self.root_link + child_link = child_object.links[child_link] if child_link else child_object.root_link + + attachment = Attachment(parent_link, child_link, bidirectional) + + self.attachments[child_object] = attachment + child_object.attachments[self] = attachment.get_inverse() + + self.world.attachment_event(self, [self, child_object]) + + def detach(self, child_object: 'Object') -> None: + """ + Detaches another object from this object. This is done by + deleting the attachment from the attachments dictionary of both objects + and deleting the constraint of the simulator. + Afterward the detachment event of the corresponding World will be fired. + + :param child_object: The object which should be detached + """ + del self.attachments[child_object] + del child_object.attachments[self] + + self.world.detachment_event(self, [self, child_object]) + + def detach_all(self) -> None: + """ + Detach all objects attached to this object. + """ + attachments = self.attachments.copy() + for att in attachments.keys(): + self.detach(att) + + def update_attachment_with_object(self, child_object: 'Object'): + self.attachments[child_object].update_transform_and_constraint() + + def get_position(self) -> Point: + """ + Returns the position of this Object as a list of xyz. + + :return: The current position of this object + """ + return self.get_pose().position + + def get_orientation(self) -> Pose.orientation: + """ + Returns the orientation of this object as a list of xyzw, representing a quaternion. + + :return: A list of xyzw + """ + return self.get_pose().orientation + + def get_position_as_list(self) -> List[float]: + """ + Returns the position of this Object as a list of xyz. + + :return: The current position of this object + """ + return self.get_pose().position_as_list() + + def get_orientation_as_list(self) -> List[float]: + """ + Returns the orientation of this object as a list of xyzw, representing a quaternion. + + :return: A list of xyzw + """ + return self.get_pose().orientation_as_list() + + def get_pose(self) -> Pose: + """ + Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. + + :return: The current pose of this object + """ + return self._current_pose + + def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Optional[bool] = True) -> None: + """ + Sets the Pose of the object. + + :param pose: New Pose for the object + :param base: If True places the object base instead of origin at the specified position and orientation + :param set_attachments: Whether to set the poses of the attached objects to this object or not. + """ + pose_in_map = self.local_transformer.transform_pose(pose, "map") + if base: + pose_in_map.position = np.array(pose_in_map.position) + self.base_origin_shift + + self.reset_base_pose(pose_in_map) + + if set_attachments: + self._set_attached_objects_poses() + + def reset_base_pose(self, pose: Pose): + self.world.reset_object_base_pose(self, pose) + self.update_pose() + + def update_pose(self): + """ + Updates the current pose of this object from the world, and updates the poses of all links. + """ + self._current_pose = self.world.get_object_pose(self) + self._update_all_links_poses() + self.update_link_transforms() + + def _update_all_joints_positions(self): + """ + Updates the posisitons of all joints by getting them from the simulator. + """ + for joint in self.joints.values(): + joint._update_position() + + def _update_all_links_poses(self): + """ + Updates the poses of all links by getting them from the simulator. + """ + for link in self.links.values(): + link._update_pose() + + def move_base_to_origin_pos(self) -> None: + """ + Move the object such that its base will be at the current origin position. + This is useful when placing objects on surfaces where you want the object base in contact with the surface. + """ + self.set_pose(self.get_pose(), base=True) + + def save_state(self, state_id) -> None: + """ + Saves the state of this object by saving the state of all links and attachments. + :param state_id: The unique id of the state. + """ + self.save_links_states(state_id) + self.save_joints_states(state_id) + super().save_state(state_id) + + def save_links_states(self, state_id: int) -> None: + """ + Saves the state of all links of this object. + :param state_id: The unique id of the state. + """ + for link in self.links.values(): + link.save_state(state_id) + + def save_joints_states(self, state_id: int) -> None: + """ + Saves the state of all joints of this object. + :param state_id: The unique id of the state. + """ + for joint in self.joints.values(): + joint.save_state(state_id) + + @property + def current_state(self) -> ObjectState: + return ObjectState(self.attachments.copy(), self.link_states, self.joint_states) + + @current_state.setter + def current_state(self, state: ObjectState) -> None: + self.attachments = state.attachments + self.link_states = state.link_states + self.joint_states = state.joint_states + + @property + def link_states(self) -> Dict[int, LinkState]: + """ + Returns the current state of all links of this object. + :return: A dictionary with the link id as key and the current state of the link as value. + """ + return {link.id: link.current_state for link in self.links.values()} + + @link_states.setter + def link_states(self, link_states: Dict[int, LinkState]) -> None: + """ + Sets the current state of all links of this object. + :param link_states: A dictionary with the link id as key and the current state of the link as value. + """ + for link in self.links.values(): + link.current_state = link_states[link.id] + + @property + def joint_states(self) -> Dict[int, JointState]: + """ + Returns the current state of all joints of this object. + :return: A dictionary with the joint id as key and the current state of the joint as value. + """ + return {joint.id: joint.current_state for joint in self.joints.values()} + + @joint_states.setter + def joint_states(self, joint_states: Dict[int, JointState]) -> None: + """ + Sets the current state of all joints of this object. + :param joint_states: A dictionary with the joint id as key and the current state of the joint as value. + """ + for joint in self.joints.values(): + joint.current_state = joint_states[joint.id] + + def restore_state(self, state_id: int) -> None: + """ + Restores the state of this object by restoring the state of all links and attachments. + :param state_id: The unique id of the state. + """ + self.restore_attachments(state_id) + self.restore_links_states(state_id) + self.restore_joints_states(state_id) + + def restore_attachments(self, state_id: int) -> None: + """ + Restores the attachments of this object from a saved state using the given state id. + :param state_id: The unique id of the state. + """ + self.attachments = self.saved_states[state_id].attachments + + def restore_links_states(self, state_id: int) -> None: + """ + Restores the states of all links of this object from a saved state using the given state id. + :param state_id: The unique id of the state. + """ + for link in self.links.values(): + link.restore_state(state_id) + + def restore_joints_states(self, state_id: int) -> None: + """ + Restores the states of all joints of this object from a saved state using the given state id. + :param state_id: The unique id of the state. + """ + for joint in self.joints.values(): + joint.restore_state(state_id) + + def remove_saved_states(self) -> None: + """ + Removes all saved states of this object. + """ + super().remove_saved_states() + self.remove_links_saved_states() + self.remove_joints_saved_states() + + def remove_links_saved_states(self) -> None: + """ + Removes all saved states of the links of this object. + """ + for link in self.links.values(): + link.remove_saved_states() + + def remove_joints_saved_states(self) -> None: + """ + Removes all saved states of the joints of this object. + """ + for joint in self.joints.values(): + joint.remove_saved_states() + + def _set_attached_objects_poses(self, already_moved_objects: Optional[List['Object']] = None) -> None: + """ + Updates the positions of all attached objects. This is done + by calculating the new pose in world coordinate frame and setting the + base pose of the attached objects to this new pose. + After this the _set_attached_objects method of all attached objects + will be called. + + :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent + loops in the update. + """ + + if already_moved_objects is None: + already_moved_objects = [] + + for child in self.attachments: + + if child in already_moved_objects: + continue + + attachment = self.attachments[child] + if not attachment.bidirectional: + self.update_attachment_with_object(child) + 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_attached_objects_poses(already_moved_objects + [self]) + + def set_position(self, position: Union[Pose, Point], base=False) -> None: + """ + Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position + instead of the origin in the center of the Object. The given position can either be a Pose, + in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. + + :param position: Target position as xyz. + :param base: If the bottom of the Object should be placed or the origin in the center. + """ + pose = Pose() + if isinstance(position, Pose): + target_position = position.position + pose.frame = position.frame + elif isinstance(position, Point): + target_position = position + elif isinstance(position, list): + target_position = position + else: + raise TypeError("The given position has to be a Pose, Point or a list of xyz.") + + pose.position = target_position + pose.orientation = self.get_orientation() + self.set_pose(pose, base=base) + + def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: + """ + Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only + the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. + + :param orientation: Target orientation given as a list of xyzw. + """ + pose = Pose() + if isinstance(orientation, Pose): + target_orientation = orientation.orientation + pose.frame = orientation.frame + else: + target_orientation = orientation + + pose.pose.position = self.get_position() + pose.pose.orientation = target_orientation + self.set_pose(pose) + + def get_joint_id(self, name: str) -> int: + """ + Returns the unique id for a joint name. As used by the world/simulator. + + :param name: The joint name + :return: The unique id + """ + return self.joint_name_to_id[name] + + def get_root_link_description(self) -> 'LinkDescription': + """ + Returns the root link of the URDF of this object. + :return: The root link as defined in the URDF of this object. + """ + root_link_name = self.description.get_root() + for link_description in self.description.links: + if link_description.name == root_link_name: + return link_description + + @property + def root_link(self) -> 'Link': + """ + Returns the root link of this object. + :return: The root link of this object. + """ + return self.links[self.description.get_root()] + + @property + def root_link_name(self) -> str: + """ + Returns the name of the root link of this object. + :return: The name of the root link of this object. + """ + return self.description.get_root() + + def get_root_link_id(self) -> int: + """ + Returns the unique id of the root link of this object. + :return: The unique id of the root link of this object. + """ + return self.get_link_id(self.description.get_root()) + + def get_link_id(self, link_name: str) -> int: + """ + Returns a unique id for a link name. + :param link_name: The name of the link. + :return: The unique id of the link. + """ + return self.link_name_to_id[link_name] + + def get_link_by_id(self, link_id: int) -> 'Link': + """ + Returns the link for a given unique link id + :param link_id: The unique id of the link. + :return: The link object. + """ + return self.links[self.link_id_to_name[link_id]] + + def get_joint_by_id(self, joint_id: int) -> str: + """ + Returns the joint name for a unique world id + + :param joint_id: The world id of for joint + :return: The joint name + """ + return dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys()))[joint_id] + + 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_positions = [0] * len(joint_names) + self.set_joint_positions(dict(zip(joint_names, joint_positions))) + + def set_joint_positions(self, joint_poses: dict) -> 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: + """ + for joint_name, joint_position in joint_poses.items(): + self.joints[joint_name].position = joint_position + # 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: + """ + 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: + return self.joints[joint_name].position + + def get_joint_damping(self, joint_name: str) -> float: + return self.joints[joint_name].damping + + def get_joint_upper_limit(self, joint_name: str) -> float: + return self.joints[joint_name].upper_limit + + def get_joint_lower_limit(self, joint_name: str) -> float: + return self.joints[joint_name].lower_limit + + def get_joint_axis(self, joint_name: str) -> Point: + return self.joints[joint_name].axis + + def get_joint_type(self, joint_name: str) -> JointType: + return self.joints[joint_name].type + + def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: + return self.joints[joint_name].limits + + def find_joint_above(self, link_name: str, joint_type: JointType) -> str: + """ + Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. + + :param link_name: AbstractLink name above which the joint should be found + :param joint_type: Joint type that should be searched for + :return: Name of the first joint which has the given type + """ + chain = self.description.get_chain(self.description.get_root(), link_name) + reversed_chain = reversed(chain) + container_joint = None + for element in reversed_chain: + if element in self.joint_name_to_id and self.get_joint_type(element) == joint_type: + container_joint = element + break + if not container_joint: + rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") + return container_joint + + 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. + + :return: A dictionary with all joints positions'. + """ + return {j.name: j.position for j in self.joints.values()} + + def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: + """ + Updates the transforms of all links of this object using time 'transform_time' or the current ros time. + """ + for link in self.links.values(): + link.update_transform(transform_time) + + def contact_points(self) -> List: + """ + Returns a list of contact points of this Object with other Objects. + + :return: A list of all contact points with other objects + """ + return self.world.get_object_contact_points(self) + + def contact_points_simulated(self) -> List: + """ + Returns a list of all contact points between this Object and other Objects after stepping the simulation once. + + :return: A list of contact points between this Object and other Objects + """ + state_id = self.world.save_state() + self.world.step() + contact_points = self.contact_points() + self.world.restore_state(state_id) + return contact_points + + def set_color(self, rgba_color: Color) -> None: + """ + Changes the color of this object, the color has to be given as a list + of RGBA values. + + :param rgba_color: The color as Color object with RGBA values between 0 and 1 + """ + # Check if there is only one link, this is the case for primitive + # forms or if loaded from an .stl or .obj file + if self.links != {}: + for link in self.links.values(): + link.color = rgba_color + else: + self.root_link.color = rgba_color + + def get_color(self) -> Union[Color, Dict[str, Color]]: + """ + This method returns the rgba_color of this object. The return is either: + + 1. A Color object with RGBA values, this is the case if the object only has one link (this + happens for example if the object is spawned from a .obj or .stl file) + 2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object. + Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which + the object is spawned. + + :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and + the rgba_color as value. + """ + link_to_color_dict = self.links_colors + + if len(link_to_color_dict) == 1: + return list(link_to_color_dict.values())[0] + else: + return link_to_color_dict + + @property + def links_colors(self) -> Dict[str, Color]: + """ + The color of each link as a dictionary with link names as keys and RGBA colors as values. + """ + return self.world.get_colors_of_object_links(self) + + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis aligned bounding box of this object. + """ + return self.world.get_object_axis_aligned_bounding_box(self) + + def get_base_origin(self) -> Pose: + """ + :return: the origin of the base/bottom of this object. + """ + aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() + base_width = np.absolute(aabb.min_x - aabb.max_x) + base_length = np.absolute(aabb.min_y - aabb.max_y) + return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], + self.get_pose().orientation_as_list()) + + def __copy__(self) -> 'Object': + """ + Returns a copy of this object. The copy will have the same name, type, path, description, pose, world and color. + :return: A copy of this object. + """ + return Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), + self.world.prospection_world, self.color) diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 9c8f4f764..56da2945f 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -6,8 +6,8 @@ from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from .pose import Pose, Transform from .robot_descriptions import robot_description -from .world import Object, UseProspectionWorld -from .world import World +from .world_object import Object +from .world import World, UseProspectionWorld def stable(obj: Object) -> bool: diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 9a8f5a808..fa97f3d0a 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -1,6 +1,7 @@ import unittest -from pycram.bullet_world import BulletWorld, Object +from pycram.bullet_world import BulletWorld +from pycram.world_object import Object from pycram.pose import Pose from pycram.robot_descriptions import robot_description from pycram.process_module import ProcessModule diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 9542edf98..47ad23064 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -5,7 +5,9 @@ from bullet_world_testcase import BulletWorldTestCase from pycram.pose import Pose -from pycram.world import Object, ObjectType +from pycram.robot_descriptions import robot_description +from pycram.enums import ObjectType +from pycram.world_object import Object import xml.etree.ElementTree as ET from pycram.urdf_interface import ObjectDescription @@ -56,11 +58,26 @@ def test_get_object_contact_points(self): def test_step_simulation(self): # TODO: kitchen explodes when stepping simulation, fix this + time.sleep(2) self.world.remove_object(self.kitchen) self.milk.set_position(Pose([0, 0, 2])) self.world.simulate(1) self.assertTrue(self.milk.get_position().z < 2) + def test_set_real_time_simulation(self): + # self.world.remove_object(self.kitchen) + self.milk.set_position(Pose([100, 0, 2])) + curr_time = time.time() + self.world.simulate(0.5, real_time=True) + time_elapsed = time.time() - curr_time + self.assertAlmostEqual(time_elapsed, 0.5, delta=0.1) + + def test_create_vis_axis(self): + self.world.add_vis_axis(self.robot.links[robot_description.get_camera_frame()].pose, 1) + self.assertTrue(len(self.world.vis_axis) == 1) + self.world.remove_vis_axis() + self.assertTrue(len(self.world.vis_axis) == 0) + class XMLTester(unittest.TestCase): diff --git a/test/test_database_merger.py b/test/test_database_merger.py index cbd26b731..6ed3faaf2 100644 --- a/test/test_database_merger.py +++ b/test/test_database_merger.py @@ -11,7 +11,8 @@ from pycram.enums import Arms, ObjectType, WorldMode from pycram.task import with_tree import pycram.task -from pycram.bullet_world import Object +from pycram.bullet_world import BulletWorld +from pycram.world_object import Object from pycram.designators.object_designator import * from dataclasses import dataclass, field diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 6df2a71bc..31f4cc28d 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -3,7 +3,7 @@ import sqlalchemy import sqlalchemy.orm import pycram.plan_failures -from pycram.world import Object +from pycram.world_object import Object from pycram import task from pycram.world import World from pycram.designators import action_designator, object_designator diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index 4ffad3d4c..b03319785 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -6,7 +6,7 @@ import pycram.plan_failures from pycram.bullet_world import BulletWorld -from pycram.world import Object +from pycram.world_object import Object from pycram.designators import action_designator, object_designator from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot diff --git a/test/test_language.py b/test/test_language.py index e4739802a..94cff57a3 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -1,4 +1,5 @@ import threading +import time import unittest from pycram.designators.action_designator import * from pycram.enums import ObjectType, State diff --git a/test/test_links.py b/test/test_links.py index 1205b4e23..a7f37016c 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -1,4 +1,7 @@ +import time + from bullet_world_testcase import BulletWorldTestCase +from pycram.world_dataclasses import Color class TestLinks(BulletWorldTestCase): @@ -25,3 +28,9 @@ def test_transform_link(self): robot_to_milk = robot_link.get_transform_to_link(milk_link) self.assertAlmostEqual(robot_to_milk, milk_to_robot.invert()) + def test_set_color(self): + link = self.robot.links['base_link'] + link.color = Color(1, 0, 0, 1) + self.assertEqual(link.color.get_rgba(), [1, 0, 0, 1]) + + From bfc5134b49ea577e87fd1c2d9b0470f006fc7e17 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 15 Feb 2024 14:45:41 +0100 Subject: [PATCH 53/69] [Abstract World] Added more tests for bullet world, Tests are running. --- src/pycram/bullet_world.py | 63 +++++++++++++++--------------- test/bullet_world_testcase.py | 32 +++++++++++++++- test/test_bullet_world.py | 72 ++++++++++++++++++++++++++++++----- 3 files changed, 123 insertions(+), 44 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 5cb332084..eeef44a64 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -3,20 +3,21 @@ import threading import time -from typing_extensions import List, Optional, Dict import numpy as np import pybullet as p import rosgraph import rospy +from geometry_msgs.msg import Point +from typing_extensions import List, Optional, Dict -from .enums import ObjectType, WorldMode +from .enums import ObjectType, WorldMode, JointType from .pose import Pose +from .urdf_interface import ObjectDescription from .world import World -from .world_object import Object from .world_constraints import Constraint from .world_dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape -from .urdf_interface import ObjectDescription +from .world_object import Object Link = ObjectDescription.Link RootLink = ObjectDescription.RootLink @@ -56,7 +57,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo # Needed to let the other thread start the simulation, before Objects are spawned. time.sleep(0.1) - self.vis_axis: List[Object] = [] + self.vis_axis: List[int] = [] # Some default settings self.set_gravity([0, 0, -9.8]) @@ -212,19 +213,15 @@ def add_vis_axis(self, pose: Pose, box_vis_shape = BoxVisualShape(Color(0, 0, 1, 0.8), [0.01, 0.01, length], [0.01, 0.01, length]) vis_z = self.create_visual_shape(box_vis_shape) - obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=[vis_x, vis_y, vis_z], - basePosition=position, baseOrientation=orientation, - linkPositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkMasses=[1.0, 1.0, 1.0], linkOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkInertialFramePositions=[[0, 0, 0], [0, 0, 0], [0, 0, 0]], - linkInertialFrameOrientations=[[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 0, 1]], - linkParentIndices=[0, 0, 0], - linkJointTypes=[p.JOINT_FIXED, p.JOINT_FIXED, p.JOINT_FIXED], - linkJointAxis=[[1, 0, 0], [0, 1, 0], [0, 0, 1]], - linkCollisionShapeIndices=[-1, -1, -1], - physicsClientId=self.id) + multibody = MultiBody(base_visual_shape_index=-1, base_pose=pose_in_map, + link_visual_shape_indices=[vis_x, vis_y, vis_z], + link_poses=[Pose(), Pose(), Pose()], link_masses=[1.0, 1.0, 1.0], + link_inertial_frame_poses=[Pose(), Pose(), Pose()], link_parent_indices=[0, 0, 0], + link_joint_types=[JointType.FIXED.value, JointType.FIXED.value, JointType.FIXED.value], + link_joint_axis=[Point(1, 0, 0), Point(0, 1, 0), Point(0, 0, 1)], + link_collision_shape_indices=[-1, -1, -1]) - self.vis_axis.append(obj) + self.vis_axis.append(self.create_multi_body(multibody)) def remove_vis_axis(self) -> None: """ @@ -250,21 +247,21 @@ def create_visual_shape(self, visual_shape: VisualShape) -> int: physicsClientId=self.id, **visual_shape.shape_data()) def create_multi_body(self, multi_body: MultiBody) -> int: - return p.createMultiBody(baseVisualShapeIndex=-MultiBody.base_visual_shape_index, - linkVisualShapeIndices=MultiBody.link_visual_shape_indices, - basePosition=MultiBody.base_pose.position_as_list(), - baseOrientation=MultiBody.base_pose.orientation_as_list(), - linkPositions=[pose.position_as_list() for pose in MultiBody.link_poses], - linkMasses=MultiBody.link_masses, - linkOrientations=[pose.orientation_as_list() for pose in MultiBody.link_poses], + return p.createMultiBody(baseVisualShapeIndex=-multi_body.base_visual_shape_index, + linkVisualShapeIndices=multi_body.link_visual_shape_indices, + basePosition=multi_body.base_pose.position_as_list(), + baseOrientation=multi_body.base_pose.orientation_as_list(), + linkPositions=[pose.position_as_list() for pose in multi_body.link_poses], + linkMasses=multi_body.link_masses, + linkOrientations=[pose.orientation_as_list() for pose in multi_body.link_poses], linkInertialFramePositions=[pose.position_as_list() - for pose in MultiBody.link_inertial_frame_poses], + for pose in multi_body.link_inertial_frame_poses], linkInertialFrameOrientations=[pose.orientation_as_list() - for pose in MultiBody.link_inertial_frame_poses], - linkParentIndices=MultiBody.link_parent_indices, - linkJointTypes=MultiBody.link_joint_types, - linkJointAxis=[[point.x, point.y, point.z] for point in MultiBody.link_joint_axis], - linkCollisionShapeIndices=MultiBody.link_collision_shape_indices) + for pose in multi_body.link_inertial_frame_poses], + linkParentIndices=multi_body.link_parent_indices, + linkJointTypes=multi_body.link_joint_types, + linkJointAxis=[[point.x, point.y, point.z] for point in multi_body.link_joint_axis], + linkCollisionShapeIndices=multi_body.link_collision_shape_indices) def get_images_for_target(self, target_pose: Pose, @@ -299,7 +296,7 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ return p.addUserDebugText(text, position, color.get_rgb(), physicsClientId=self.id, **args) def remove_text(self, text_id: Optional[int] = None) -> None: - if text_id: + if text_id is not None: p.removeUserDebugItem(text_id, physicsClientId=self.id) else: p.removeAllUserDebugItems(physicsClientId=self.id) @@ -416,9 +413,9 @@ def run(self): # camera movement when the left mouse button is pressed if mouse_state[0] == 3: - speed_x = abs(old_mouse_x - mouse_x) if (abs(old_mouse_x - mouse_x)) < max_speed\ + speed_x = abs(old_mouse_x - mouse_x) if (abs(old_mouse_x - mouse_x)) < max_speed \ else max_speed - speed_y = abs(old_mouse_y - mouse_y) if (abs(old_mouse_y - mouse_y)) < max_speed\ + speed_y = abs(old_mouse_y - mouse_y) if (abs(old_mouse_y - mouse_y)) < max_speed \ else max_speed # max angle of 89.5 and -89.5 to make sure the camera does not flip (is annoying) diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index fa97f3d0a..99d99f0f8 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -1,3 +1,4 @@ +import time import unittest from pycram.bullet_world import BulletWorld @@ -16,7 +17,7 @@ class BulletWorldTestCase(unittest.TestCase): @classmethod def setUpClass(cls): - cls.world = BulletWorld(WorldMode.DIRECT) + cls.world = BulletWorld(mode=WorldMode.DIRECT) cls.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + cls.extension, ObjectDescription) @@ -33,6 +34,7 @@ def setUp(self): # Tests in here would not be properly executed in the CI def tearDown(self): + time.sleep(0.1) self.world.reset_world() @classmethod @@ -40,4 +42,32 @@ def tearDownClass(cls): cls.world.exit() +class BulletWorldGUITestCase(unittest.TestCase): + world: BulletWorld + extension: str = ObjectDescription.get_file_extension() + + @classmethod + def setUpClass(cls): + cls.world = BulletWorld(mode=WorldMode.GUI) + cls.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + cls.robot = Object(robot_description.name, ObjectType.ROBOT, + robot_description.name + cls.extension, ObjectDescription) + cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension, ObjectDescription) + cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", + ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) + ProcessModule.execution_delay = False + + def setUp(self): + self.world.reset_world() + + # DO NOT WRITE TESTS HERE!!! + # Test related to the BulletWorld should be written in test_bullet_world.py + # Tests in here would not be properly executed in the CI + + def tearDown(self): + pass + + @classmethod + def tearDownClass(cls): + cls.world.exit() diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 47ad23064..1027b373a 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -1,15 +1,16 @@ import time import unittest +import xml.etree.ElementTree as ET import rospkg -from bullet_world_testcase import BulletWorldTestCase +from bullet_world_testcase import BulletWorldTestCase, BulletWorldGUITestCase +from pycram.enums import ObjectType, WorldMode from pycram.pose import Pose from pycram.robot_descriptions import robot_description -from pycram.enums import ObjectType -from pycram.world_object import Object -import xml.etree.ElementTree as ET from pycram.urdf_interface import ObjectDescription +from pycram.world_dataclasses import Color +from pycram.world_object import Object fix_missing_inertial = ObjectDescription.fix_missing_inertial @@ -41,12 +42,13 @@ def test_save_and_restore_state(self): self.assertTrue(cid == self.robot.attachments[self.milk].id) def test_remove_object(self): - time.sleep(2) + # time.sleep(2) milk_id = self.milk.id self.assertTrue(milk_id in [obj.id for obj in self.world.objects]) self.world.remove_object(self.milk) self.assertTrue(milk_id not in [obj.id for obj in self.world.objects]) - BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", + ObjectDescription, pose=Pose([1.3, 1, 0.9])) def test_get_joint_position(self): self.assertEqual(self.robot.get_joint_position("head_pan_joint"), 0.0) @@ -56,24 +58,73 @@ def test_get_object_contact_points(self): self.milk.set_position(self.robot.get_position()) self.assertTrue(len(self.robot.contact_points()) > 0) + +class BulletWorldTestRemove(BulletWorldTestCase): def test_step_simulation(self): # TODO: kitchen explodes when stepping simulation, fix this time.sleep(2) self.world.remove_object(self.kitchen) + time.sleep(2) self.milk.set_position(Pose([0, 0, 2])) self.world.simulate(1) self.assertTrue(self.milk.get_position().z < 2) + # self.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + self.extension, ObjectDescription, + # world=self.world) + # time.sleep(2) def test_set_real_time_simulation(self): - # self.world.remove_object(self.kitchen) self.milk.set_position(Pose([100, 0, 2])) curr_time = time.time() self.world.simulate(0.5, real_time=True) time_elapsed = time.time() - curr_time - self.assertAlmostEqual(time_elapsed, 0.5, delta=0.1) + self.assertAlmostEqual(time_elapsed, 0.5, delta=0.2) - def test_create_vis_axis(self): - self.world.add_vis_axis(self.robot.links[robot_description.get_camera_frame()].pose, 1) + +class BulletWorldTestVis(BulletWorldTestCase): + def test_add_vis_axis(self): + self.world.add_vis_axis(self.robot.links[robot_description.get_camera_frame()].pose) + self.assertTrue(len(self.world.vis_axis) == 1) + self.world.remove_vis_axis() + self.assertTrue(len(self.world.vis_axis) == 0) + + def test_add_text(self): + link: ObjectDescription.Link = self.robot.links[robot_description.get_camera_frame()] + text_id = self.world.add_text("test", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, + Color(1, 0, 0, 1), 3, link.object_id, link.id) + if self.world.mode == WorldMode.GUI: + time.sleep(4) + + def test_remove_text(self): + link: ObjectDescription.Link = self.robot.links[robot_description.get_camera_frame()] + text_id_1 = self.world.add_text("test 1", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, + Color(1, 0, 0, 1), 0, link.object_id, link.id) + text_id = self.world.add_text("test 2", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, + Color(0, 1, 0, 1), 0, link.object_id, link.id) + + if self.world.mode == WorldMode.GUI: + time.sleep(2) + self.world.remove_text(text_id_1) + if self.world.mode == WorldMode.GUI: + time.sleep(3) + + def test_remove_all_text(self): + link: ObjectDescription.Link = self.robot.links[robot_description.get_camera_frame()] + text_id_1 = self.world.add_text("test 1", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, + Color(1, 0, 0, 1), 0, link.object_id, link.id) + text_id = self.world.add_text("test 2", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, + Color(0, 1, 0, 1), 0, link.object_id, link.id) + if self.world.mode == WorldMode.GUI: + time.sleep(2) + self.world.remove_text() + if self.world.mode == WorldMode.GUI: + time.sleep(3) + + +@unittest.skip("Not working in CI") +class BulletWorldTestGUI(BulletWorldGUITestCase): + def test_add_vis_axis(self): + time.sleep(10) + self.world.add_vis_axis(self.robot.links[robot_description.get_camera_frame()].pose) self.assertTrue(len(self.world.vis_axis) == 1) self.world.remove_vis_axis() self.assertTrue(len(self.world.vis_axis) == 0) @@ -92,3 +143,4 @@ def test_inertial(self): resulting_tree = ET.ElementTree(ET.fromstring(result)) for element in resulting_tree.iter("link"): self.assertTrue(len([*element.iter("inertial")]) > 0) + From b9f5d86813525b7b3d0189a60bc655ea106d2e35 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 15 Feb 2024 19:35:49 +0100 Subject: [PATCH 54/69] [Abstract World] Added more tests for bullet world, Tests are running. --- src/pycram/bullet_world_reasoning.py | 0 src/pycram/description.py | 21 ++++ src/pycram/designators/object_designator.py | 2 +- .../process_modules/hsr_process_modules.py | 2 +- .../process_modules/pr2_process_modules.py | 4 +- src/pycram/world.py | 68 +++-------- src/pycram/world_constraints.py | 12 ++ src/pycram/world_dataclasses.py | 2 +- src/pycram/world_object.py | 26 +++-- test/bullet_world_testcase.py | 2 +- test/test_bullet_world.py | 107 ++++++++++++++++-- test/test_costmaps.py | 10 +- 12 files changed, 176 insertions(+), 80 deletions(-) delete mode 100644 src/pycram/bullet_world_reasoning.py diff --git a/src/pycram/bullet_world_reasoning.py b/src/pycram/bullet_world_reasoning.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/pycram/description.py b/src/pycram/description.py index 8c86a7ea9..3ac66c521 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -370,6 +370,15 @@ def tf_frame(self) -> str: """ return f"{self.object.tf_frame}/{self.name}" + def __eq__(self, other): + return self.id == other.id and self.object == other.object and self.name == other.name + + def __copy__(self): + return Link(self.id, self, self.object) + + def __hash__(self): + return hash((self.id, self.object, self.name)) + class RootLink(Link, ABC): """ @@ -390,6 +399,9 @@ def tf_frame(self) -> str: def _update_pose(self) -> None: self._current_pose = self.object.get_pose() + def __copy__(self): + return RootLink(self.object) + class Joint(ObjectEntity, JointDescription, ABC): """ @@ -499,6 +511,15 @@ def current_state(self) -> JointState: def current_state(self, joint_state: JointState) -> None: self.position = joint_state.position + def __copy__(self): + return Joint(self.id, self, self.object) + + def __eq__(self, other): + return self.id == other.id and self.object == other.object and self.name == other.name + + def __hash__(self): + return hash((self.id, self.object, self.name)) + class ObjectDescription(EntityDescription): MESH_EXTENSIONS: Tuple[str] = (".obj", ".stl") diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 23dacdbdf..34975ce1d 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -170,6 +170,6 @@ def __iter__(self): """ object_candidates = query(self) for obj_desig in object_candidates: - for world_obj in World.get_objects_by_type(obj_desig.obj_type): + for world_obj in World.get_object_by_type(obj_desig.obj_type): obj_desig.world_object = world_obj yield obj_desig diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index f0f4d7d1b..0a9936147 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -171,7 +171,7 @@ def _execute(self, desig): cam_frame_name = solution['cam_frame'] front_facing_axis = solution['front_facing_axis'] - objects = World.current_world.get_objects_by_type(object_type) + objects = World.current_world.get_object_by_type(object_type) for obj in objects: if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis, 0.5): return obj diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index aa2c3bc53..1240f7467 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -184,7 +184,7 @@ def _execute(self, desig: DetectingMotion.Motion): # should be [0, 0, 1] front_facing_axis = robot_description.front_facing_axis - objects = World.current_world.get_objects_by_type(object_type) + objects = World.current_world.get_object_by_type(object_type) for obj in objects: if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis): return obj @@ -346,7 +346,7 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: obj_pose.orientation = [0, 0, 0, 1] obj_pose.position.x += 0.05 - world_obj = World.current_world.get_objects_by_type(designator.object_type) + world_obj = World.current_world.get_object_by_type(designator.object_type) if world_obj: world_obj[0].set_pose(obj_pose) return world_obj[0] diff --git a/src/pycram/world.py b/src/pycram/world.py index 463ddf62f..5db837f7e 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -170,6 +170,8 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._init_events() + self._current_state: Optional[WorldState] = None + def _init_events(self): """ Initializes dynamic events that can be used to react to changes in the World. @@ -244,17 +246,16 @@ def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: """ pass - # TODO: This is not used anywhere, should it be removed? - def get_objects_by_name(self, name: str) -> List['Object']: + def get_object_by_name(self, name: str) -> List['Object']: """ Returns a list of all Objects in this World with the same name as the given one. :param name: The name of the returned Objects. :return: A list of all Objects with the name 'name'. """ - return list(filter(lambda obj: obj.name == name, self.objects)) + return list(filter(lambda obj: obj.name == name, self.objects))[0] - def get_objects_by_type(self, obj_type: ObjectType) -> List['Object']: + def get_object_by_type(self, obj_type: ObjectType) -> List['Object']: """ Returns a list of all Objects which have the type 'obj_type'. @@ -301,7 +302,7 @@ def remove_object(self, obj: 'Object') -> None: self.remove_object_from_simulator(obj) - if World.robot == self: + if World.robot == obj: World.robot = None def add_fixed_constraint(self, parent_link: 'Link', child_link: 'Link', @@ -529,30 +530,6 @@ def get_link_axis_aligned_bounding_box(self, link: 'Link') -> AxisAlignedBoundin """ pass - def get_attachment_event(self) -> Event: - """ - Returns the event reference that is fired if an attachment occurs. - - :return: The reference to the attachment event - """ - return self.attachment_event - - def get_detachment_event(self) -> Event: - """ - Returns the event reference that is fired if a detachment occurs. - - :return: The event reference for the detachment event. - """ - return self.detachment_event - - def get_manipulation_event(self) -> Event: - """ - Returns the event reference that is fired if any manipulation occurs. - - :return: The event reference for the manipulation event. - """ - return self.manipulation_event - @abstractmethod def set_realtime(self, real_time: bool) -> None: """ @@ -666,6 +643,8 @@ def save_state(self, state_id: Optional[int] = None) -> int: @property def current_state(self) -> WorldState: + if self._current_state is None: + self._current_state = WorldState(self.save_physics_simulator_state(), self.object_states) return self._current_state @current_state.setter @@ -674,20 +653,20 @@ def current_state(self, state: WorldState) -> None: self.object_states = state.object_states @property - def object_states(self) -> Dict[int, ObjectState]: + def object_states(self) -> Dict[str, ObjectState]: """ Returns the states of all objects in the World. :return: A dictionary with the object id as key and the object state as value. """ - return {obj.id: obj.current_state for obj in self.objects} + return {obj.name: obj.current_state for obj in self.objects} @object_states.setter - def object_states(self, states: Dict[int, ObjectState]) -> None: + def object_states(self, states: Dict[str, ObjectState]) -> None: """ Sets the states of all objects in the World. """ - for obj_id, obj_state in states.items(): - self.get_object_by_id(obj_id).current_state = obj_state + for obj_name, obj_state in states.items(): + self.get_object_by_name(obj_name).current_state = obj_state def save_objects_state(self, state_id: int) -> None: """ @@ -697,18 +676,6 @@ def save_objects_state(self, state_id: int) -> None: for obj in self.objects: obj.save_state(state_id) - def restore_state(self, state_id) -> None: - """ - Restores the state of the World according to the given state using the unique state id. This includes the state - of the physics simulator and the state of all objects. - However, restore can not respawn objects if there are objects that were deleted between creation of the state - and restoring, they will be skipped. - - :param state_id: The unique id representing the state. - """ - self.restore_physics_simulator_state(state_id) - self.restore_objects_states(state_id) - @abstractmethod def save_physics_simulator_state(self) -> int: """ @@ -893,13 +860,12 @@ def create_multi_body_from_visual_shapes(self, visual_shape_ids: List[int], pose :param pose: The pose of the origin of the multi body relative to the world frame. :return: The unique id of the created multi body. """ - # Dummy parameter since these are needed to spawn visual shapes as a - # multibody. + # Dummy parameter since these are needed to spawn visual shapes as a multibody. num_of_shapes = len(visual_shape_ids) link_poses = [Pose() for _ in range(num_of_shapes)] link_masses = [1.0 for _ in range(num_of_shapes)] link_parent = [0 for _ in range(num_of_shapes)] - link_joints = [JointType.FIXED for _ in range(num_of_shapes)] + link_joints = [JointType.FIXED.value for _ in range(num_of_shapes)] link_collision = [-1 for _ in range(num_of_shapes)] link_joint_axis = [Point(1, 0, 0) for _ in range(num_of_shapes)] @@ -1158,12 +1124,14 @@ def check_for_pause(self) -> None: while self.pause_sync: time.sleep(0.1) - def check_for_equal(self) -> None: + def check_for_equal(self) -> bool: """ Checks if both Worlds have the same state, meaning all objects are in the same position. This is currently not used, but might be used in the future if synchronization issues worsen. + :return: True if both Worlds have the same state, False otherwise. """ eql = True for obj, prospection_obj in self.object_mapping.items(): eql = eql and obj.get_pose() == prospection_obj.get_pose() self.equal_states = eql + return eql diff --git a/src/pycram/world_constraints.py b/src/pycram/world_constraints.py index f301f0ff9..52d8cdd59 100644 --- a/src/pycram/world_constraints.py +++ b/src/pycram/world_constraints.py @@ -255,3 +255,15 @@ def __del__(self) -> None: Removes the constraint between the parent and the child links if one exists when the attachment is deleted. """ self.remove_constraint_if_exists() + + def __copy__(self): + return Attachment(self.parent_link, self.child_link, self.bidirectional, self.parent_to_child_transform, + self.id) + + def __eq__(self, other): + return (self.parent_link == other.parent_link and self.child_link == other.child_link and + self.bidirectional == other.bidirectional and + self.parent_to_child_transform == other.parent_to_child_transform) + + def __hash__(self): + return hash((self.parent_link, self.child_link, self.bidirectional, self.parent_to_child_transform)) diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index e7b0a8692..98b15178a 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -327,7 +327,7 @@ class State(ABC): @dataclass class WorldState(State): simulator_state_id: int - object_states: Dict[int, 'ObjectState'] + object_states: Dict[str, 'ObjectState'] @dataclass diff --git a/src/pycram/world_object.py b/src/pycram/world_object.py index 5e2eabf69..c398a1828 100644 --- a/src/pycram/world_object.py +++ b/src/pycram/world_object.py @@ -411,7 +411,7 @@ def save_joints_states(self, state_id: int) -> None: @property def current_state(self) -> ObjectState: - return ObjectState(self.attachments.copy(), self.link_states, self.joint_states) + return ObjectState(self.attachments.copy(), self.link_states.copy(), self.joint_states.copy()) @current_state.setter def current_state(self, state: ObjectState) -> None: @@ -453,15 +453,6 @@ def joint_states(self, joint_states: Dict[int, JointState]) -> None: for joint in self.joints.values(): joint.current_state = joint_states[joint.id] - def restore_state(self, state_id: int) -> None: - """ - Restores the state of this object by restoring the state of all links and attachments. - :param state_id: The unique id of the state. - """ - self.restore_attachments(state_id) - self.restore_links_states(state_id) - self.restore_joints_states(state_id) - def restore_attachments(self, state_id: int) -> None: """ Restores the attachments of this object from a saved state using the given state id. @@ -818,5 +809,16 @@ def __copy__(self) -> 'Object': Returns a copy of this object. The copy will have the same name, type, path, description, pose, world and color. :return: A copy of this object. """ - return Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), - self.world.prospection_world, self.color) + obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), + self.world.prospection_world, self.color) + obj.current_state = self.current_state + return obj + + def __eq__(self, other): + if not isinstance(other, Object): + return False + return (self.id == other.id and self.world == other.world and self.name == other.name + and self.obj_type == other.obj_type) + + def __hash__(self): + return hash((self.name, self.obj_type, self.id, self.world.id)) diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 99d99f0f8..4dc7ae2e7 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -34,7 +34,7 @@ def setUp(self): # Tests in here would not be properly executed in the CI def tearDown(self): - time.sleep(0.1) + time.sleep(0.2) self.world.reset_world() @classmethod diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 1027b373a..658138975 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -29,7 +29,10 @@ def test_robot_orientation(self): def test_save_and_restore_state(self): self.robot.attach(self.milk) + self.milk.attach(self.cereal) + all_object_attachments = {obj: obj.attachments.copy() for obj in self.world.objects} state_id = self.world.save_state() + self.milk.detach(self.cereal) robot_link = self.robot.root_link milk_link = self.milk.root_link cid = robot_link.constraint_ids[milk_link] @@ -38,8 +41,11 @@ def test_save_and_restore_state(self): self.world.restore_state(state_id) cid = robot_link.constraint_ids[milk_link] self.assertTrue(milk_link in robot_link.constraint_ids) - self.assertTrue(self.milk in self.robot.attachments) self.assertTrue(cid == self.robot.attachments[self.milk].id) + for obj in self.world.objects: + self.assertTrue(len(obj.attachments) == len(all_object_attachments[obj])) + for att in obj.attachments: + self.assertTrue(att in all_object_attachments[obj]) def test_remove_object(self): # time.sleep(2) @@ -50,6 +56,14 @@ def test_remove_object(self): BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + def test_remove_robot(self): + robot_id = self.robot.id + self.assertTrue(robot_id in [obj.id for obj in self.world.objects]) + self.world.remove_object(self.robot) + self.assertTrue(robot_id not in [obj.id for obj in self.world.objects]) + BulletWorldTest.robot = Object(robot_description.name, ObjectType.ROBOT, + robot_description.name + self.extension, ObjectDescription) + def test_get_joint_position(self): self.assertEqual(self.robot.get_joint_position("head_pan_joint"), 0.0) @@ -58,19 +72,29 @@ def test_get_object_contact_points(self): self.milk.set_position(self.robot.get_position()) self.assertTrue(len(self.robot.contact_points()) > 0) + def test_enable_joint_force_torque_sensor(self): + self.world.enable_joint_force_torque_sensor(self.robot, self.robot.get_joint_id("head_pan_joint")) + force_torque = self.world.get_joint_reaction_force_torque(self.robot, self.robot.get_joint_id("head_pan_joint")) + # TODO: useless because even if the sensor is disabled, the force_torque is still 0 + for ft in force_torque: + self.assertTrue(ft == 0.0) + + def test_disable_joint_force_torque_sensor(self): + self.world.enable_joint_force_torque_sensor(self.robot, self.robot.get_joint_id("head_pan_joint")) + self.world.disable_joint_force_torque_sensor(self.robot, self.robot.get_joint_id("head_pan_joint")) + force_torque = self.world.get_joint_reaction_force_torque(self.robot, self.robot.get_joint_id("head_pan_joint")) + for ft in force_torque: + self.assertTrue(ft == 0.0) + + def test_get_applied_joint_motor_torque(self): + self.world.get_applied_joint_motor_torque(self.robot, self.robot.get_joint_id("head_pan_joint")) -class BulletWorldTestRemove(BulletWorldTestCase): def test_step_simulation(self): # TODO: kitchen explodes when stepping simulation, fix this - time.sleep(2) - self.world.remove_object(self.kitchen) - time.sleep(2) + self.kitchen.set_position([100, 100, 0]) self.milk.set_position(Pose([0, 0, 2])) self.world.simulate(1) self.assertTrue(self.milk.get_position().z < 2) - # self.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + self.extension, ObjectDescription, - # world=self.world) - # time.sleep(2) def test_set_real_time_simulation(self): self.milk.set_position(Pose([100, 0, 2])) @@ -79,8 +103,72 @@ def test_set_real_time_simulation(self): time_elapsed = time.time() - curr_time self.assertAlmostEqual(time_elapsed, 0.5, delta=0.2) + def test_collision_callback(self): + self.kitchen.set_position([100, 100, 0]) + self.collision_called = False + self.no_collision_called = False + + def collision_callback(): + self.collision_called = True + + def no_collision_callback(): + self.no_collision_called = True + + self.world.register_two_objects_collision_callbacks(self.milk, self.cereal, + collision_callback, no_collision_callback) + + self.world.simulate(1) + self.assertTrue(self.no_collision_called) + self.assertFalse(self.collision_called) + + self.collision_called = False + self.no_collision_called = False + + new_milk_position = self.cereal.get_position() + new_milk_position.z += 0.5 + self.milk.set_position(new_milk_position) + + self.world.simulate(4) + self.assertTrue(self.collision_called) + + def test_equal_world_states(self): + time.sleep(2) + self.robot.set_pose(Pose([1, 0, 0], [0, 0, 0, 1])) + self.assertFalse(self.world.world_sync.check_for_equal()) + self.world.prospection_world.object_states = self.world.current_state.object_states + time.sleep(0.1) + self.assertTrue(self.world.world_sync.check_for_equal()) + + def test_add_resource_path(self): + self.world.add_resource_path("test") + self.assertTrue("test" in self.world.data_directory) + + def test_no_prospection_object_found_for_given_object(self): + milk_2 = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + time.sleep(0.1) + try: + self.world.prospection_world.remove_object(milk_2) + self.assertTrue(milk_2 in self.world.objects) + self.world.get_prospection_object_for_object(milk_2) + self.assertFalse(True) + except ValueError as e: + self.assertTrue(True) + self.world.remove_object(milk_2) + + def test_no_object_found_for_given_prospection_object(self): + milk_2 = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + time.sleep(0.1) + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + self.assertTrue(self.world.get_object_for_prospection_object(prospection_milk) == milk_2) + try: + self.world.remove_object(milk_2) + self.world.get_object_for_prospection_object(prospection_milk) + time.sleep(0.1) + self.assertFalse(True) + except ValueError as e: + self.assertTrue(True) + time.sleep(0.1) -class BulletWorldTestVis(BulletWorldTestCase): def test_add_vis_axis(self): self.world.add_vis_axis(self.robot.links[robot_description.get_camera_frame()].pose) self.assertTrue(len(self.world.vis_axis) == 1) @@ -143,4 +231,3 @@ def test_inertial(self): resulting_tree = ET.ElementTree(ET.fromstring(result)) for element in resulting_tree.iter("link"): self.assertTrue(len([*element.iter("inertial")]) > 0) - diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 457bbd577..ba8c855ff 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -1,7 +1,8 @@ +import numpy as np + from bullet_world_testcase import BulletWorldTestCase from pycram.costmaps import OccupancyCostmap from pycram.pose import Pose -import numpy as np class TestCostmapsCase(BulletWorldTestCase): @@ -16,9 +17,14 @@ def test_attachment_exclusion(self): self.robot.set_pose(Pose([0, 0, 0])) self.milk.set_pose(Pose([0.5, 0, 1])) self.cereal.set_pose(Pose([50, 50, 0])) - o = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02, origin=Pose([0, 0, 0], [0, 0, 0, 1])) + o = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02, + origin=Pose([0, 0, 0], [0, 0, 0, 1])) self.assertEquals(np.sum(o.map[115:135, 90:110]), 0) self.robot.attach(self.milk) self.assertTrue(np.sum(o.map[80:90, 90:110]) != 0) + def test_visualize(self): + o = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02, + origin=Pose([0, 0, 0], [0, 0, 0, 1])) + o.visualize() From 1458e03a4657b8c6595687f997109f35b264b2a0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 15 Feb 2024 20:54:54 +0100 Subject: [PATCH 55/69] [Abstract World] Added more tests for bullet world, Tests are running. --- src/pycram/world.py | 8 -------- test/test_bullet_world.py | 6 +++--- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 5db837f7e..9015347d6 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -701,14 +701,6 @@ def restore_physics_simulator_state(self, state_id: int) -> None: """ pass - def restore_objects_states(self, state_id: int) -> None: - """ - Restores the state of all objects in the World according to the given state using the unique state id. - :param state_id: The unique id representing the state. - """ - for obj in self.objects: - obj.restore_state(state_id) - def get_images_for_target(self, target_pose: Pose, cam_pose: Pose, diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 658138975..ff910d877 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -147,13 +147,13 @@ def test_no_prospection_object_found_for_given_object(self): milk_2 = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) time.sleep(0.1) try: - self.world.prospection_world.remove_object(milk_2) - self.assertTrue(milk_2 in self.world.objects) + prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) + self.world.remove_object(milk_2) + time.sleep(0.1) self.world.get_prospection_object_for_object(milk_2) self.assertFalse(True) except ValueError as e: self.assertTrue(True) - self.world.remove_object(milk_2) def test_no_object_found_for_given_prospection_object(self): milk_2 = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) From c49ccc0050648364656d854537dfe7a55c7d2f05 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 16 Feb 2024 17:57:08 +0100 Subject: [PATCH 56/69] [Abstract World] Added more tests for bullet world, Tests are running. Made 'WorldSync' sync objects by equating their current state/ --- src/pycram/bullet_world.py | 6 +- src/pycram/description.py | 22 ++-- src/pycram/designators/location_designator.py | 2 +- src/pycram/pose.py | 4 +- .../process_modules/pr2_process_modules.py | 4 +- src/pycram/world.py | 16 +-- src/pycram/world_dataclasses.py | 1 + src/pycram/world_object.py | 113 +++++------------- test/bullet_world_testcase.py | 2 +- test/test_bullet_world.py | 9 +- test/test_description.py | 25 ++++ test/test_object.py | 110 ++++++++++++++++- 12 files changed, 193 insertions(+), 121 deletions(-) create mode 100644 test/test_description.py diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index eeef44a64..f4fc8c300 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -63,8 +63,8 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self.set_gravity([0, 0, -9.8]) if not is_prospection_world: - plane = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, ObjectDescription, - world=self) + _ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, ObjectDescription, + world=self) def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: return p.loadURDF(path, @@ -202,8 +202,6 @@ def add_vis_axis(self, pose: Pose, pose_in_map = self.local_transformer.transform_pose(pose, "map") - position, orientation = pose_in_map.to_list() - box_vis_shape = BoxVisualShape(Color(1, 0, 0, 0.8), [length, 0.01, 0.01], [length, 0.01, 0.01]) vis_x = self.create_visual_shape(box_vis_shape) diff --git a/src/pycram/description.py b/src/pycram/description.py index 3ac66c521..f6bee8ff9 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -184,13 +184,6 @@ def object_id(self) -> int: """ return self.object.id - def __repr__(self): - return self.__class__.__qualname__ + f"(" + ', \n'.join( - [f"{key}={value}" for key, value in self.__dict__.items()]) + ")" - - def __str__(self): - return self.__repr__() - class Link(ObjectEntity, LinkDescription, ABC): """ @@ -509,7 +502,8 @@ def current_state(self) -> JointState: @current_state.setter def current_state(self, joint_state: JointState) -> None: - self.position = joint_state.position + if self._current_position != joint_state.position: + self.position = joint_state.position def __copy__(self): return Joint(self.id, self, self.object) @@ -578,14 +572,22 @@ def generate_description_from_file(self, path: str, extension: str) -> str: :param extension: The file extension of the file to preprocess. :return: The processed description string. """ + description_string = None if extension in self.MESH_EXTENSIONS: description_string = self.generate_from_mesh_file(path) elif extension == self.get_file_extension(): description_string = self.generate_from_description_file(path) else: - # Using the description from the parameter server - description_string = self.generate_from_parameter_server(path) + try: + # Using the description from the parameter server + description_string = self.generate_from_parameter_server(path) + except KeyError: + logging.warning(f"Couldn't find dile data in the ROS parameter server") + if description_string is None: + logging.error(f"Could not find file with path {path} in the resources directory nor" + f" in the ros parameter server.") + raise FileNotFoundError return description_string diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 4b2a0a931..2a087f07f 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -257,7 +257,7 @@ def __iter__(self) -> Location: test_robot = World.current_world.get_prospection_object_for_object(self.robot) # Find a Joint of type prismatic which is above the handle in the URDF tree - container_joint = self.handle.world_object.find_joint_above(self.handle.name, JointType.PRISMATIC) + container_joint = self.handle.world_object.find_joint_above_link(self.handle.name, JointType.PRISMATIC) init_pose = link_pose_for_joint_config(self.handle.world_object, { container_joint: self.handle.world_object.get_joint_limits(container_joint)[0]}, diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 0873883aa..85f85f08a 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -100,8 +100,8 @@ def position(self, value) -> None: """ if (not type(value) == list and not type(value) == tuple and not type(value) == GeoPose and not type(value) == Point): - rospy.logwarn("Position can only be a list or geometry_msgs/Pose") - return + rospy.logerr("Position can only be a list or geometry_msgs/Pose") + raise TypeError("Position can only be a list/tuple or geometry_msgs/Pose") if type(value) == list or type(value) == tuple and len(value) == 3: self.pose.position.x = value[0] self.pose.position.y = value[1] diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 1240f7467..ff5da6a0b 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -244,7 +244,7 @@ class Pr2Open(ProcessModule): def _execute(self, desig: OpeningMotion.Motion): part_of_object = desig.object_part.world_object - container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) + container_joint = part_of_object.find_joint_above_link(desig.object_part.name, JointType.PRISMATIC) goal_pose = btr.link_pose_for_joint_config(part_of_object, { container_joint: part_of_object.get_joint_limits(container_joint)[1] - 0.05}, desig.object_part.name) @@ -264,7 +264,7 @@ class Pr2Close(ProcessModule): def _execute(self, desig: ClosingMotion.Motion): part_of_object = desig.object_part.world_object - container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) + container_joint = part_of_object.find_joint_above_link(desig.object_part.name, JointType.PRISMATIC) goal_pose = btr.link_pose_for_joint_config(part_of_object, { container_joint: part_of_object.get_joint_limits(container_joint)[0]}, desig.object_part.name) diff --git a/src/pycram/world.py b/src/pycram/world.py index 9015347d6..4805c0306 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -1076,10 +1076,8 @@ def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): # self.equal_states = False for i in range(self.add_obj_queue.qsize()): obj = self.add_obj_queue.get() - # [0:name, 1:type, 2:path, 3:description, 4:position, 5:orientation, - # 6:self.world.prospection_world, 7:rgba_color, 8:world object] # Maps the World object to the prospection world object - self.object_mapping[obj[8]] = copy(obj[8]) + self.object_mapping[obj] = copy(obj) self.add_obj_queue.task_done() for i in range(self.remove_obj_queue.qsize()): obj = self.remove_obj_queue.get() @@ -1090,17 +1088,7 @@ def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): self.remove_obj_queue.task_done() for world_obj, prospection_obj in self.object_mapping.items(): - b_pose = world_obj.get_pose() - s_pose = prospection_obj.get_pose() - if b_pose.dist(s_pose) != 0.0: - prospection_obj.set_pose(world_obj.get_pose()) - - # Manage joint positions - if len(world_obj.joint_name_to_id) > 2: - for joint_name in world_obj.joint_name_to_id.keys(): - if prospection_obj.get_joint_position(joint_name) != world_obj.get_joint_position(joint_name): - prospection_obj.set_joint_positions(world_obj.get_positions_of_all_joints()) - break + prospection_obj.current_state = world_obj.current_state self.check_for_pause() # self.check_for_equal() diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 98b15178a..3f0900964 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -332,6 +332,7 @@ class WorldState(State): @dataclass class ObjectState(State): + pose: Pose attachments: Dict['Object', 'Attachment'] link_states: Dict[int, 'LinkState'] joint_states: Dict[int, 'JointState'] diff --git a/src/pycram/world_object.py b/src/pycram/world_object.py index c398a1828..1eee6c20d 100644 --- a/src/pycram/world_object.py +++ b/src/pycram/world_object.py @@ -72,8 +72,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") + f"{self.name}_{self.id}") - self._update_object_description_from_file(self.path) - if self.description.name == robot_description.name: self.world.set_robot_if_not_set(self) @@ -104,7 +102,11 @@ def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, :return: The unique id of the object and the path of the file that was loaded. """ - path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) + try: + path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) + except FileNotFoundError as e: + logging.error("Could not generate description from file.") + raise e try: obj_id = self.world.load_description_and_get_object_id(path, Pose(self.get_position_as_list(), @@ -115,14 +117,7 @@ def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" " the name of an URDF string on the parameter server.") os.remove(path) - raise (e) - - def _update_object_description_from_file(self, path: str) -> None: - """ - Updates the object description from the given file path. - :param path: The path to the file from which the object description should be updated. - """ - self.description.update_description_from_file(path) + raise e def _init_joint_name_and_id_map(self) -> None: """ @@ -170,10 +165,7 @@ def _add_to_world_sync_obj_queue(self) -> None: """ Adds this object to the objects queue of the WorldSync object of the World. """ - self.world.world_sync.add_obj_queue.put( - [self.name, self.obj_type, self.path, type(self.description), self.get_position_as_list(), - self.get_orientation_as_list(), - self.world.prospection_world, self.color, self]) + self.world.world_sync.add_obj_queue.put(self) @property def link_names(self) -> List[str]: @@ -182,13 +174,6 @@ def link_names(self) -> List[str]: """ return self.world.get_object_link_names(self) - @property - def number_of_links(self) -> int: - """ - :return: The number of links of this object. - """ - return len(self.description.links) - @property def joint_names(self) -> List[str]: """ @@ -196,20 +181,13 @@ def joint_names(self) -> List[str]: """ return self.world.get_object_joint_names(self) - @property - def number_of_joints(self) -> int: - """ - :return: The number of joints of this object. - """ - return len(self.description.joints) - @property def base_origin_shift(self) -> np.ndarray: """ The shift between the base of the object and the origin of the object. :return: A numpy array with the shift between the base of the object and the origin of the object. """ - return np.array(self.get_pose().position_as_list()) - np.array(self.get_base_origin().position_as_list()) + return np.array(self.get_position_as_list()) - np.array(self.get_base_position_as_list()) def __repr__(self): skip_attr = ["links", "joints", "description", "attachments"] @@ -318,6 +296,14 @@ def get_position_as_list(self) -> List[float]: """ return self.get_pose().position_as_list() + def get_base_position_as_list(self) -> List[float]: + """ + Returns the position of this Object as a list of xyz. + + :return: The current position of this object + """ + return self.get_base_origin().position_as_list() + def get_orientation_as_list(self) -> List[float]: """ Returns the orientation of this object as a list of xyzw, representing a quaternion. @@ -344,7 +330,7 @@ def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Op """ pose_in_map = self.local_transformer.transform_pose(pose, "map") if base: - pose_in_map.position = np.array(pose_in_map.position) + self.base_origin_shift + pose_in_map.position = (np.array(pose_in_map.position_as_list()) + self.base_origin_shift).tolist() self.reset_base_pose(pose_in_map) @@ -363,13 +349,6 @@ def update_pose(self): self._update_all_links_poses() self.update_link_transforms() - def _update_all_joints_positions(self): - """ - Updates the posisitons of all joints by getting them from the simulator. - """ - for joint in self.joints.values(): - joint._update_position() - def _update_all_links_poses(self): """ Updates the poses of all links by getting them from the simulator. @@ -377,7 +356,7 @@ def _update_all_links_poses(self): for link in self.links.values(): link._update_pose() - def move_base_to_origin_pos(self) -> None: + def move_base_to_origin_pose(self) -> None: """ Move the object such that its base will be at the current origin position. This is useful when placing objects on surfaces where you want the object base in contact with the surface. @@ -411,10 +390,12 @@ def save_joints_states(self, state_id: int) -> None: @property def current_state(self) -> ObjectState: - return ObjectState(self.attachments.copy(), self.link_states.copy(), self.joint_states.copy()) + return ObjectState(self.get_pose(), self.attachments.copy(), self.link_states.copy(), self.joint_states.copy()) @current_state.setter def current_state(self, state: ObjectState) -> None: + if self.get_pose().dist(state.pose) != 0.0: + self.set_pose(state.pose, base=False, set_attachments=False) self.attachments = state.attachments self.link_states = state.link_states self.joint_states = state.joint_states @@ -453,29 +434,6 @@ def joint_states(self, joint_states: Dict[int, JointState]) -> None: for joint in self.joints.values(): joint.current_state = joint_states[joint.id] - def restore_attachments(self, state_id: int) -> None: - """ - Restores the attachments of this object from a saved state using the given state id. - :param state_id: The unique id of the state. - """ - self.attachments = self.saved_states[state_id].attachments - - def restore_links_states(self, state_id: int) -> None: - """ - Restores the states of all links of this object from a saved state using the given state id. - :param state_id: The unique id of the state. - """ - for link in self.links.values(): - link.restore_state(state_id) - - def restore_joints_states(self, state_id: int) -> None: - """ - Restores the states of all joints of this object from a saved state using the given state id. - :param state_id: The unique id of the state. - """ - for joint in self.joints.values(): - joint.restore_state(state_id) - def remove_saved_states(self) -> None: """ Removes all saved states of this object. @@ -519,7 +477,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List['Obje continue attachment = self.attachments[child] - if not attachment.bidirectional: + if attachment.loose: self.update_attachment_with_object(child) child.update_attachment_with_object(self) @@ -552,7 +510,7 @@ def set_position(self, position: Union[Pose, Point], base=False) -> None: pose.orientation = self.get_orientation() self.set_pose(pose, base=base) - def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: + def set_orientation(self, orientation: Union[Pose, Quaternion, List, Tuple, np.ndarray]) -> None: """ Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. @@ -563,8 +521,13 @@ def set_orientation(self, orientation: Union[Pose, Quaternion]) -> None: if isinstance(orientation, Pose): target_orientation = orientation.orientation pose.frame = orientation.frame - else: + elif isinstance(orientation, Quaternion): target_orientation = orientation + elif (isinstance(orientation, list) or isinstance(orientation, np.ndarray) or isinstance(orientation, tuple)) \ + and len(orientation) == 4: + target_orientation = Quaternion(*orientation) + else: + raise TypeError("The given orientation has to be a Pose, Quaternion or one of list/tuple/ndarray of xyzw.") pose.pose.position = self.get_position() pose.pose.orientation = target_orientation @@ -584,9 +547,8 @@ def get_root_link_description(self) -> 'LinkDescription': Returns the root link of the URDF of this object. :return: The root link as defined in the URDF of this object. """ - root_link_name = self.description.get_root() for link_description in self.description.links: - if link_description.name == root_link_name: + if link_description.name == self.root_link_name: return link_description @property @@ -628,15 +590,6 @@ def get_link_by_id(self, link_id: int) -> 'Link': """ return self.links[self.link_id_to_name[link_id]] - def get_joint_by_id(self, joint_id: int) -> str: - """ - Returns the joint name for a unique world id - - :param joint_id: The world id of for joint - :return: The joint name - """ - return dict(zip(self.joint_name_to_id.values(), self.joint_name_to_id.keys()))[joint_id] - 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 @@ -692,7 +645,7 @@ def get_joint_type(self, joint_name: str) -> JointType: def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: return self.joints[joint_name].limits - def find_joint_above(self, link_name: str, joint_type: JointType) -> str: + def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: """ Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. @@ -798,11 +751,11 @@ def get_base_origin(self) -> Pose: """ :return: the origin of the base/bottom of this object. """ - aabb = self.get_link_by_id(-1).get_axis_aligned_bounding_box() + aabb = self.get_axis_aligned_bounding_box() base_width = np.absolute(aabb.min_x - aabb.max_x) base_length = np.absolute(aabb.min_y - aabb.max_y) return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], - self.get_pose().orientation_as_list()) + self.get_orientation_as_list()) def __copy__(self) -> 'Object': """ diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 4dc7ae2e7..bc0d2cc72 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -34,7 +34,7 @@ def setUp(self): # Tests in here would not be properly executed in the CI def tearDown(self): - time.sleep(0.2) + time.sleep(0.05) self.world.reset_world() @classmethod diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index ff910d877..e68397875 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -48,7 +48,6 @@ def test_save_and_restore_state(self): self.assertTrue(att in all_object_attachments[obj]) def test_remove_object(self): - # time.sleep(2) milk_id = self.milk.id self.assertTrue(milk_id in [obj.id for obj in self.world.objects]) self.world.remove_object(self.milk) @@ -136,7 +135,7 @@ def test_equal_world_states(self): self.robot.set_pose(Pose([1, 0, 0], [0, 0, 0, 1])) self.assertFalse(self.world.world_sync.check_for_equal()) self.world.prospection_world.object_states = self.world.current_state.object_states - time.sleep(0.1) + time.sleep(0.05) self.assertTrue(self.world.world_sync.check_for_equal()) def test_add_resource_path(self): @@ -145,7 +144,7 @@ def test_add_resource_path(self): def test_no_prospection_object_found_for_given_object(self): milk_2 = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) - time.sleep(0.1) + time.sleep(0.05) try: prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) self.world.remove_object(milk_2) @@ -157,7 +156,7 @@ def test_no_prospection_object_found_for_given_object(self): def test_no_object_found_for_given_prospection_object(self): milk_2 = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) - time.sleep(0.1) + time.sleep(0.05) prospection_milk = self.world.get_prospection_object_for_object(milk_2) self.assertTrue(self.world.get_object_for_prospection_object(prospection_milk) == milk_2) try: @@ -167,7 +166,7 @@ def test_no_object_found_for_given_prospection_object(self): self.assertFalse(True) except ValueError as e: self.assertTrue(True) - time.sleep(0.1) + time.sleep(0.05) def test_add_vis_axis(self): self.world.add_vis_axis(self.robot.links[robot_description.get_camera_frame()].pose) diff --git a/test/test_description.py b/test/test_description.py new file mode 100644 index 000000000..8f3afa201 --- /dev/null +++ b/test/test_description.py @@ -0,0 +1,25 @@ +from bullet_world_testcase import BulletWorldTestCase + + +class DescriptionTest(BulletWorldTestCase): + def test_is_root(self): + self.assertTrue(self.robot.root_link.is_root) + self.assertFalse(self.robot.links["base_link"].is_root) + + def test_orientation_as_list(self): + base_link = self.robot.links["base_link"] + self.assertEqual(base_link.orientation_as_list, base_link.pose.orientation_as_list()) + + def test_pose_as_list(self): + base_link = self.robot.links["base_link"] + self.assertEqual(base_link.pose_as_list, base_link.pose.to_list()) + + def test_joint_child_link(self): + self.assertEqual(self.robot.joints["base_footprint_joint"].child_link, self.robot.links["base_link"]) + self.assertEqual(self.robot.joints["base_footprint_joint"].parent_link, self.robot.root_link) + + def test_generate_description_from_mesh(self): + self.assertTrue(self.milk.description.generate_description_from_file("../resources/cached/milk.stl", ".stl")) + + def test_generate_description_from_description_file(self): + self.assertTrue(self.milk.description.generate_description_from_file("../resources/cached/milk.urdf", ".urdf")) diff --git a/test/test_object.py b/test/test_object.py index ff9361eb5..80a8f98c0 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -1,15 +1,89 @@ +import os + +import numpy as np + from bullet_world_testcase import BulletWorldTestCase -from pycram.enums import JointType + +from pycram.enums import JointType, ObjectType from pycram.pose import Pose -from geometry_msgs.msg import Point +from pycram.world_dataclasses import Color +from pycram.world_object import Object +from pycram.urdf_interface import ObjectDescription + +from geometry_msgs.msg import Point, Quaternion class TestObject(BulletWorldTestCase): + def test_wrong_object_description_path(self): + with self.assertRaises(FileNotFoundError): + milk = Object("milk2", ObjectType.MILK, "wrong_path.sk", ObjectDescription) + + def test_malformed_object_description(self): + malformed_file = "../resources/cached/malformed_description.urdf" + with open(malformed_file, "w") as file: + file.write("malformed") + with self.assertRaises(Exception): + Object("milk2", ObjectType.MILK, malformed_file, ObjectDescription) + + def test_move_base_to_origin_pose(self): + self.milk.set_position(Point(1, 2, 3), base=False) + self.milk.move_base_to_origin_pose() + self.assertEqual(self.milk.get_base_position_as_list(), [1, 2, 3]) + def test_set_position_as_point(self): self.milk.set_position(Point(1, 2, 3)) self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) + def test_uni_direction_attachment(self): + self.milk.attach(self.cereal, bidirectional=False) + + milk_position = self.milk.get_position_as_list() + cereal_position = self.cereal.get_position_as_list() + + def move_milk_and_assert_cereal_moved(): + milk_position[0] += 1 + cereal_position[0] += 1 + self.milk.set_position(milk_position) + + new_cereal_position = self.cereal.get_position_as_list() + self.assertEqual(new_cereal_position, cereal_position) + + def move_cereal_and_assert_milk_not_moved(): + cereal_position[0] += 1 + self.cereal.set_position(cereal_position) + + new_milk_position = self.milk.get_position_as_list() + self.assertEqual(new_milk_position, milk_position) + + # move twice to test updated attachment at the new cereal position + for _ in range(2): + move_milk_and_assert_cereal_moved() + move_cereal_and_assert_milk_not_moved() + + def test_setting_wrong_position_type(self): + with self.assertRaises(TypeError): + self.milk.set_position(np.array([1, 2, 3])) + + with self.assertRaises(TypeError): + self.milk.get_pose().position = np.array([1, 2, 3]) + + def test_set_orientation_as_list(self): + self.milk.set_orientation([1, 0, 0, 0]) + self.assertEqual(self.milk.get_orientation_as_list(), [1, 0, 0, 0]) + + def test_set_orientation_as_quaternion(self): + self.milk.set_orientation(Quaternion(*[1, 0, 0, 0])) + self.assertEqual(self.milk.get_orientation_as_list(), [1, 0, 0, 0]) + + def test_set_orientation_as_ndarray(self): + self.milk.set_orientation(np.array([1, 0, 0, 0])) + self.assertEqual(self.milk.get_orientation_as_list(), [1, 0, 0, 0]) + + def test_set_wrong_orientation_type(self): + with self.assertRaises(TypeError): + self.milk.set_orientation(1) + def test_set_position_as_pose(self): self.milk.set_position(Pose([1, 2, 3])) self.assertEqual(self.milk.get_position_as_list(), [1, 2, 3]) @@ -56,3 +130,35 @@ def test_restore_state(self): saved_state = link.saved_states[1] self.assertEqual(curr_state, saved_state) self.assertEqual(curr_state.constraint_ids, saved_state.constraint_ids) + + def test_get_link_by_id(self): + self.assertEqual(self.robot.get_link_by_id(-1), self.robot.root_link) + + def test_find_joint_above_link(self): + self.assertEqual(self.robot.find_joint_above_link("head_pan_link", JointType.REVOLUTE), "head_pan_joint") + + def test_wrong_joint_type_for_joint_above_link(self): + container_joint = self.robot.find_joint_above_link("head_pan_link", JointType.CONTINUOUS) + self.assertTrue(container_joint is None) + + def test_contact_points_simulated(self): + self.milk.set_position([0, 0, 100]) + contact_points = self.milk.contact_points_simulated() + self.assertFalse(contact_points) + self.milk.set_position(self.cereal.get_position_as_list(), base=True) + contact_points = self.milk.contact_points_simulated() + self.assertTrue(contact_points) + + def test_set_color(self): + self.milk.set_color(Color(1, 0, 0, 1)) + self.assertEqual(self.milk.get_color(), Color(1, 0, 0, 1)) + self.robot.set_color(Color(0, 1, 0, 1)) + for color in self.robot.get_color().values(): + self.assertEqual(color, Color(0, 1, 0, 1)) + + def test_object_equal(self): + milk2 = Object("milk2", ObjectType.MILK, "milk.stl", ObjectDescription) + self.assertNotEqual(self.milk, milk2) + self.assertEqual(self.milk, self.milk) + self.assertNotEqual(self.milk, self.cereal) + self.assertNotEqual(self.milk, self.world) From bba4c4fec79134afb3da4ce97fde600b31f45d6b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 16 Feb 2024 18:10:28 +0100 Subject: [PATCH 57/69] [Abstract World] Added more tests for bullet world, Tests are running. --- src/pycram/world_dataclasses.py | 61 --------------------------------- test/test_cache_manager.py | 17 +++++++++ 2 files changed, 17 insertions(+), 61 deletions(-) create mode 100644 test/test_cache_manager.py diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 3f0900964..54b04876a 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -76,67 +76,6 @@ def get_rgb(self) -> List[float]: return [self.R, self.G, self.B] -@dataclass -class Constraint: - """ - Dataclass for storing a constraint between two objects. - """ - parent_link: 'Link' - child_link: 'Link' - joint_type: JointType - joint_axis_in_child_link_frame: Point - joint_frame_pose_wrt_parent_origin: Pose - joint_frame_pose_wrt_child_origin: Pose - - def get_parent_object_id(self) -> int: - """ - Returns the id of the parent object of the constraint. - - :return: The id of the parent object of the constraint - """ - return self.parent_link.object.id - - def get_child_object_id(self) -> int: - """ - Returns the id of the child object of the constraint. - - :return: The id of the child object of the constraint - """ - return self.child_link.object.id - - def get_parent_link_id(self) -> int: - """ - Returns the id of the parent link of the constraint. - - :return: The id of the parent link of the constraint - """ - return self.parent_link.id - - def get_child_link_id(self) -> int: - """ - Returns the id of the child link of the constraint. - - :return: The id of the child link of the constraint - """ - return self.child_link.id - - def get_joint_axis_as_list(self) -> List[float]: - """ - Returns the joint axis of the constraint as a list. - - :return: The joint axis of the constraint as a list - """ - return get_point_as_list(self.joint_axis_in_child_link_frame) - - def get_joint_position_wrt_parent_as_list(self) -> List[float]: - """ - Returns the joint frame pose with respect to the parent origin as a list. - - :return: The joint frame pose with respect to the parent origin as a list - """ - return self.joint_frame_pose_wrt_parent_origin.position_as_list() - - @dataclass class AxisAlignedBoundingBox: """ diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py new file mode 100644 index 000000000..c69e1ea9f --- /dev/null +++ b/test/test_cache_manager.py @@ -0,0 +1,17 @@ +from bullet_world_testcase import BulletWorldTestCase +from pycram.enums import ObjectType +from pycram.urdf_interface import ObjectDescription +from pycram.world_object import Object + + +class TestCacheManager(BulletWorldTestCase): + + def test_generate_description_and_write_to_cache(self): + cache_manager = self.world.cache_manager + path = "../resources/apartment.urdf" + extension = ".urdf" + cache_path = self.world.cache_dir + "apartment.urdf" + apartment = Object("apartment", ObjectType.ENVIRONMENT, path, ObjectDescription) + cache_manager.generate_description_and_write_to_cache(path, extension, cache_path, + apartment.description) + self.assertTrue(cache_manager.is_cached(path, apartment.description)) From 9e39552e7e87b7acf7a2d0dccd438ee262cd35c6 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 20 Feb 2024 20:35:51 +0100 Subject: [PATCH 58/69] [Abstract World] Tried importing annotations from __future__ but Fails due to circular imports. --- demos/pycram_bullet_world_demo/demo.py | 4 +- doc/source/notebooks/bullet_world.ipynb | 21 +-- doc/source/notebooks/intro.ipynb | 2 +- examples/bullet_world.ipynb | 21 ++- examples/cram_plan_tutorial.ipynb | 6 +- examples/intro.ipynb | 2 +- examples/local_transformer.ipynb | 2 +- src/neem_interface_python | 2 +- src/pycram/costmaps.py | 4 +- src/pycram/description.py | 22 +-- src/pycram/designators/action_designator.py | 8 +- src/pycram/designators/object_designator.py | 2 +- src/pycram/external_interfaces/giskard.py | 7 +- src/pycram/external_interfaces/ik.py | 4 +- src/pycram/external_interfaces/robokudo.py | 2 +- src/pycram/helper.py | 2 +- src/pycram/pose_generator_and_validator.py | 4 +- .../process_modules/boxy_process_modules.py | 6 +- .../default_process_modules.py | 4 +- .../process_modules/donbot_process_modules.py | 6 +- .../process_modules/hsr_process_modules.py | 6 +- .../process_modules/pr2_process_modules.py | 10 +- src/pycram/ros/tf_broadcaster.py | 4 +- src/pycram/ros/viz_marker_publisher.py | 23 +-- src/pycram/world.py | 96 ++++++----- src/pycram/world_constraints.py | 24 +-- src/pycram/world_dataclasses.py | 41 +++-- src/pycram/world_object.py | 160 ++++++++++++++++-- src/pycram/world_reasoning.py | 4 +- test/test_action_designator.py | 4 +- test/test_bullet_world.py | 16 +- test/test_bullet_world_reasoning.py | 10 +- test/test_description.py | 4 +- test/test_links.py | 2 +- test/test_task_tree.py | 2 +- 35 files changed, 347 insertions(+), 190 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 24917d05d..f37015e4f 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -6,7 +6,7 @@ from pycram.pose import Pose from pycram.process_module import simulated_robot, with_simulated_robot from pycram.urdf_interface import ObjectDescription -from pycram.world import Object +from pycram.world_object import Object from pycram.world_dataclasses import Color extension = ObjectDescription.get_file_extension() @@ -70,7 +70,7 @@ def move_and_detect(obj_type): spoon.detach(apartment) # Detect and pickup the spoon - LookAtAction([apartment.links["handle_cab10_t"].pose]).resolve().perform() + LookAtAction([apartment.get_link_pose("handle_cab10_t")]).resolve().perform() spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform() diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb index c9a0def28..cc8874cec 100644 --- a/doc/source/notebooks/bullet_world.ipynb +++ b/doc/source/notebooks/bullet_world.ipynb @@ -141,9 +141,10 @@ }, "outputs": [], "source": [ - "from pycram.world import Object\n", + "from pycram.world_object import Object\n", + "from pycram.urdf_interface import ObjectDescription\n", "\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([0, 0, 1]))" + "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", ObjectDescription, pose=Pose([0, 0, 1]))" ] }, { @@ -292,7 +293,7 @@ }, "outputs": [], "source": [ - "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1, 0, 1]))" + "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", ObjectDescription, pose=Pose([1, 0, 1]))" ] }, { @@ -395,7 +396,7 @@ } ], "source": [ - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", + "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\", ObjectDescription)\n", "print(pr2.links)" ] }, @@ -452,11 +453,11 @@ } ], "source": [ - "print(f\"Position: \\n{pr2.links['torso_lift_link'].position}\")\n", + "print(f\"Position: \\n{pr2.get_link_position('torso_lift_link')}\")\n", "\n", - "print(f\"Orientation: \\n{pr2.links['torso_lift_link'].orientation}\")\n", + "print(f\"Orientation: \\n{pr2.get_link_orientation('torso_lift_link')}\")\n", "\n", - "print(f\"Pose: \\n{pr2.links['torso_lift_link'].pose}\")" + "print(f\"Pose: \\n{pr2.get_link_pose('torso_lift_link')}\")" ] }, { @@ -533,7 +534,7 @@ } ], "source": [ - "print(f\"Pr2 forearm color: {pr2.links['r_forearm_link'].color}\")" + "print(f\"Pr2 forearm color: {pr2.get_link_color('r_forearm_link')}\")" ] }, { @@ -548,7 +549,7 @@ }, "outputs": [], "source": [ - "pr2.links[\"r_forearm_link\"].color = [1, 0, 0, 1]" + "pr2.set_link_color(\"r_forearm_link\", [1, 0, 0, 1])" ] }, { @@ -580,7 +581,7 @@ } ], "source": [ - "pr2.get_aabb()" + "pr2.get_axis_aligned_bounding_box()" ] }, { diff --git a/doc/source/notebooks/intro.ipynb b/doc/source/notebooks/intro.ipynb index d38e92ecc..86a479e1e 100644 --- a/doc/source/notebooks/intro.ipynb +++ b/doc/source/notebooks/intro.ipynb @@ -489,7 +489,7 @@ ], "source": [ "milk.set_position(Pose([1,0,1]))\n", - "visible = btr.visible(milk, pr2.links[\"wide_stereo_optical_frame\"].pose)\n", + "visible = btr.visible(milk, pr2.get_link_pose(\"wide_stereo_optical_frame\"))\n", "print(f\"Milk visible: {visible}\")" ] }, diff --git a/examples/bullet_world.ipynb b/examples/bullet_world.ipynb index 6a7423953..33a3da4d7 100644 --- a/examples/bullet_world.ipynb +++ b/examples/bullet_world.ipynb @@ -355,6 +355,15 @@ "print(pr2.links)" ] }, + { + "cell_type": "code", + "outputs": [], + "source": [], + "metadata": { + "collapsed": false + }, + "id": "e896223230860878" + }, { "cell_type": "markdown", "id": "e157eb6f", @@ -408,11 +417,11 @@ } ], "source": [ - "print(f\"Position: \\n{pr2.links['torso_lift_link'].position}\")\n", + "print(f\"Position: \\n{pr2.get_link_position('torso_lift_link')}\")\n", "\n", - "print(f\"Orientation: \\n{pr2.links['torso_lift_link'].orientation}\")\n", + "print(f\"Orientation: \\n{pr2.get_link_orientation('torso_lift_link')}\")\n", "\n", - "print(f\"Pose: \\n{pr2.links['torso_lift_link'].pose}\")" + "print(f\"Pose: \\n{pr2.get_link_pose('torso_lift_link')}\")" ] }, { @@ -489,7 +498,7 @@ } ], "source": [ - "print(f\"Pr2 forearm color: {pr2.links['r_forearm_link'].color}\")" + "print(f\"Pr2 forearm color: {pr2.get_link_color('r_forearm_link')}\")" ] }, { @@ -504,7 +513,7 @@ }, "outputs": [], "source": [ - "pr2.links[\"r_forearm_link\"].color = [1, 0, 0]" + "pr2.set_link_color(\"r_forearm_link\", [1, 0, 0])" ] }, { @@ -536,7 +545,7 @@ } ], "source": [ - "pr2.get_aabb()" + "pr2.get_axis_aligned_bounding_box()" ] } ], diff --git a/examples/cram_plan_tutorial.ipynb b/examples/cram_plan_tutorial.ipynb index 2297b6cb5..1427b2915 100644 --- a/examples/cram_plan_tutorial.ipynb +++ b/examples/cram_plan_tutorial.ipynb @@ -81,7 +81,7 @@ "robot_desig = ObjectDesignatorDescription(names=['pr2']).resolve()\n", "apartment = Object(\"apartment\", \"environment\", \"/home/abassi/cram_ws/src/iai_maps/iai_apartment/urdf/apartment.urdf\", position=[-1.5, -2.5, 0])\n", "apartment_desig = ObjectDesignatorDescription(names=['apartment']).resolve()\n", - "table_top = apartment.links[\"cooktop\"].position\n", + "table_top = apartment.get_link_position(\"cooktop\")\n", "# milk = Object(\"milk\", \"milk\", \"milk.stl\", position=[table_top[0]-0.15, table_top[1], table_top[2]])\n", "# milk.set_position(position=milk.get_position(), base=True)\n", "# cereal = Object(\"cereal\", \"cereal\", \"breakfast_cereal.stl\", position=table_top)\n", @@ -213,8 +213,8 @@ "source": [ "import pybullet as p\n", "for link_name in apartment.links.keys():\n", - " world.add_vis_axis(apartment.links[link_name].pose)\n", - " p.addUserDebugText(link_name, apartment.links[link_name].position)" + " world.add_vis_axis(apartment.get_link_pose(link_name))\n", + " p.addUserDebugText(link_name, apartment.get_link_position(link_name))" ] }, { diff --git a/examples/intro.ipynb b/examples/intro.ipynb index 44d3ad652..ba33eab21 100644 --- a/examples/intro.ipynb +++ b/examples/intro.ipynb @@ -489,7 +489,7 @@ ], "source": [ "milk.set_position(Pose([1,0,1]))\n", - "visible = btr.visible(milk, pr2.links[\"wide_stereo_optical_frame\"].pose)\n", + "visible = btr.visible(milk, pr2.get_link_pose(\"wide_stereo_optical_frame\"))\n", "print(f\"Milk visible: {visible}\")" ] }, diff --git a/examples/local_transformer.ipynb b/examples/local_transformer.ipynb index 4ff40178e..f71431ec0 100644 --- a/examples/local_transformer.ipynb +++ b/examples/local_transformer.ipynb @@ -319,7 +319,7 @@ "source": [ "print(milk.tf_frame)\n", "\n", - "print(kitchen.links[\"kitchen_island_surface\"].tf_frame)" + "print(kitchen.get_link_tf_frame(\"kitchen_island_surface\"))" ] } ], diff --git a/src/neem_interface_python b/src/neem_interface_python index 05ad42c41..182daab69 160000 --- a/src/neem_interface_python +++ b/src/neem_interface_python @@ -1 +1 @@ -Subproject commit 05ad42c41042335dd2287d865f6a37c115ea8ffe +Subproject commit 182daab69cb2e4dab23fbf5c9b9d1146837d8102 diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 11ae3ab0f..01b9820db 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -680,9 +680,9 @@ def __init__(self, object, urdf_link_name, size=100, resolution=0.02, world=None """ self.world: World = world if world else World.current_world self.object: Object = object - self.link: Link = object.links[urdf_link_name] + self.link: Link = object.get_link(urdf_link_name) self.resolution: float = resolution - self.origin: Pose = object.links[urdf_link_name].pose + self.origin: Pose = object.get_link_pose(urdf_link_name) self.height: int = 0 self.width: int = 0 self.map: np.ndarray = [] diff --git a/src/pycram/description.py b/src/pycram/description.py index f6bee8ff9..93513b8aa 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -1,17 +1,19 @@ +from __future__ import annotations + +import logging import pathlib from abc import ABC, abstractmethod -import logging import rospy from geometry_msgs.msg import Point, Quaternion from typing_extensions import Tuple, Union, Any, List, Optional, Dict -from pycram.enums import Shape, JointType -from pycram.local_transformer import LocalTransformer -from pycram.pose import Pose, Transform -from pycram.world import WorldEntity -from pycram.world_object import Object -from pycram.world_dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState +from .enums import JointType +from .local_transformer import LocalTransformer +from .pose import Pose, Transform +from .world import WorldEntity +from .world_dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape +from .world_object import Object class EntityDescription(ABC): @@ -43,7 +45,7 @@ def __init__(self, parsed_link_description: Any): @property @abstractmethod - def geometry(self) -> Shape: + def geometry(self) -> VisualShape: """ Returns the geometry type of the URDF collision element of this link. """ @@ -436,7 +438,7 @@ def parent_link(self) -> Link: Returns the parent link of this joint. :return: The parent link as a AbstractLink object. """ - return self.object.links[self.parent_link_name] + return self.object.get_link(self.parent_link_name) @property def child_link(self) -> Link: @@ -444,7 +446,7 @@ def child_link(self) -> Link: Returns the child link of this joint. :return: The child link as a AbstractLink object. """ - return self.object.links[self.child_link_name] + return self.object.get_link(self.child_link_name) @property def position(self) -> float: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index beea5f255..d701b2bb0 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -326,7 +326,7 @@ def perform(self) -> None: # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" - gripper_frame = robot.links[robot_description.get_tool_frame(self.arm)].tf_frame + gripper_frame = robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm)) # First rotate the gripper, so the further calculations makes sense tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) tmp_for_rotate_pose.pose.position.x = 0 @@ -436,7 +436,7 @@ def perform(self) -> None: # Transformations such that the target position is the position of the object and not the tcp tool_name = robot_description.get_tool_frame(self.arm) tcp_to_object = local_tf.transform_pose(object_pose, - World.robot.links[tool_name].tf_frame) + World.robot.get_link_tf_frame(tool_name)) target_diff = self.target_location.to_transform("target").inverse_times( tcp_to_object.to_transform("object")).to_pose() @@ -445,7 +445,7 @@ def perform(self) -> None: World.robot.detach(self.object_designator.world_object) retract_pose = local_tf.transform_pose( target_diff, - World.robot.links[robot_description.get_tool_frame(self.arm)].tf_frame) + World.robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm))) retract_pose.position.x -= 0.07 MoveTCPMotion(retract_pose, self.arm).resolve().perform() @@ -887,7 +887,7 @@ def perform(self) -> Any: gripper_name = robot_description.get_tool_frame(self.arm) object_pose_in_gripper = lt.transform_pose(object_pose, - World.robot.links[gripper_name].tf_frame) + World.robot.get_link_tf_frame(gripper_name)) pre_grasp = object_pose_in_gripper.copy() pre_grasp.pose.position.x -= 0.1 diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 34975ce1d..bd24a86a3 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -97,7 +97,7 @@ def __iter__(self): for name in self.names: if name in self.part_of.world_object.link_name_to_id.keys(): yield self.Object(name, self.type, self.part_of.world_object, - self.part_of.world_object.links[name].pose) + self.part_of.world_object.get_link_pose(name)) class LocatedObject(ObjectDesignatorDescription): diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index d2c83e8e4..1d60c5a09 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -12,6 +12,7 @@ from ..pose import Pose from ..robot_descriptions import robot_description from ..world import World +from ..world_dataclasses import MeshVisualShape from ..world_object import Object from ..robot_description import ManipulatorDescription @@ -160,9 +161,9 @@ def spawn_object(object: Object) -> None: :param object: World object that should be spawned """ if len(object.link_name_to_id) == 1: - geometry = object.description.link_map[object.description.get_root()].collision.geometry - if isinstance(geometry, urdf_parser_py.urdf.Mesh): - filename = geometry.filename + geometry = object.get_link_geometry(object.root_link_name) + if isinstance(geometry, MeshVisualShape): + filename = geometry.file_name spawn_mesh(object.name + "_" + str(object.id), filename, object.get_pose()) else: spawn_urdf(object.name + "_" + str(object.id), object.path, object.get_pose()) diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 4d60d7d52..2f1b2c184 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -29,7 +29,7 @@ def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_ob :return: A moveit_msgs/PositionIKRequest message containing all the information from the parameter """ local_transformer = LocalTransformer() - target_pose = local_transformer.transform_pose(target_pose, robot_object.links[root_link].tf_frame) + target_pose = local_transformer.transform_pose(target_pose, robot_object.get_link_tf_frame(root_link)) robot_state = RobotState() joint_state = JointState() @@ -164,7 +164,7 @@ def request_ik(target_pose: Pose, robot: Object, joints: List[str], gripper: str # Get link after last joint in chain end_effector = robot_description.get_child(joints[-1]) - target_torso = local_transformer.transform_pose(target_pose, robot.links[base_link].tf_frame) + target_torso = local_transformer.transform_pose(target_pose, robot.get_link_tf_frame(base_link)) diff = calculate_wrist_tool_offset(end_effector, gripper, robot) target_diff = target_torso.to_transform("target").inverse_times(diff).to_pose() diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index 4ecc35085..0497c04f1 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -125,7 +125,7 @@ def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescripti for i in range(0, len(query_result.res[0].pose)): pose = Pose.from_pose_stamped(query_result.res[0].pose[i]) - pose.frame = World.current_world.robot.links[pose.frame].tf_frame # TODO: pose.frame is a link name? + pose.frame = World.current_world.robot.get_link_tf_frame(pose.frame) # TODO: pose.frame is a link name? source = query_result.res[0].poseSource[i] lt = LocalTransformer() diff --git a/src/pycram/helper.py b/src/pycram/helper.py index a0c9fb4ae..bb171a0b1 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -55,7 +55,7 @@ def _apply_ik(robot: WorldObject, joint_poses: List[float], joints: List[str]) - def calculate_wrist_tool_offset(wrist_frame: str, tool_frame: str, robot: WorldObject) -> Transform: - return robot.links[wrist_frame].get_transform_to_link(robot.links[tool_frame]) + return robot.get_transfrom_between_links(wrist_frame, tool_frame) def transform(pose: List[float], diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index a1bc00019..994ff234d 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -95,13 +95,13 @@ def visibility_validator(pose: Pose, robot_pose = robot.get_pose() if isinstance(object_or_pose, Object): robot.set_pose(pose) - camera_pose = robot.links[robot_description.get_camera_frame()].pose + camera_pose = robot.get_link_pose(robot_description.get_camera_frame()) robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) ray = world.ray_test(camera_pose.position_as_list(), object_or_pose.get_position_as_list()) res = ray == object_or_pose.id else: robot.set_pose(pose) - camera_pose = robot.links[robot_description.get_camera_frame()].pose + camera_pose = robot.get_link_pose(robot_description.get_camera_frame()) robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) # TODO: Check if this is correct ray = world.ray_test(camera_pose.position_as_list(), object_or_pose) diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index e5930b694..3b4fa4698 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -33,7 +33,7 @@ def _execute(self, desig: PlaceMotion.Motion): object_pose = obj.get_pose() local_tf = LocalTransformer() tcp_to_object = local_tf.transform_pose(object_pose, - robot.links[robot_description.get_tool_frame(arm)].tf_frame) + robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, arm) @@ -64,7 +64,7 @@ def _execute(self, desig): local_transformer = LocalTransformer() - pose_in_shoulder = local_transformer.transform_pose(target, robot.links["neck_shoulder_link"].tf_frame) + 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(robot_description.get_static_joint_chain("neck", "front")) @@ -75,7 +75,7 @@ def _execute(self, desig): if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x): robot.set_joint_positions(robot_description.get_static_joint_chain("neck", "neck_left")) - pose_in_shoulder = local_transformer.transform_pose(target, robot.links["neck_shoulder_link"].tf_frame) + pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link")) new_pan = np.arctan2(pose_in_shoulder.position.y, pose_in_shoulder.position.x) diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index ba763b7c7..19faebb29 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -30,8 +30,8 @@ def _execute(self, desig: LookingMotion.Motion): pan_joint = robot_description.chains["neck"].joints[0] tilt_joint = robot_description.chains["neck"].joints[1] - pose_in_pan = local_transformer.transform_pose(target, robot.links[pan_link].tf_frame) - pose_in_tilt = local_transformer.transform_pose(target, robot.links[tilt_link].tf_frame) + pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame(pan_link)) + pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame(tilt_link)) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 4b9979cd5..5833d0513 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -56,7 +56,7 @@ def _execute(self, desig): object_pose = obj.get_pose() local_tf = LocalTransformer() tcp_to_object = local_tf.transform_pose(object_pose, - robot.links[robot_description.get_tool_frame("left")].tf_frame) + robot.get_link_tf_frame(robot_description.get_tool_frame("left"))) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, "left") @@ -75,7 +75,7 @@ def _execute(self, desig): local_transformer = LocalTransformer() - pose_in_shoulder = local_transformer.transform_pose(target, robot.links["ur5_shoulder_link"].tf_frame) + 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(robot_description.get_static_joint_chain("left", "front")) @@ -86,7 +86,7 @@ def _execute(self, desig): if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x): robot.set_joint_positions(robot_description.get_static_joint_chain("left", "arm_left")) - pose_in_shoulder = local_transformer.transform_pose(target, robot.links["ur5_shoulder_link"].tf_frame) + pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link")) new_pan = np.arctan2(pose_in_shoulder.position.y, pose_in_shoulder.position.x) diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 0a9936147..0b4e11512 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -101,9 +101,9 @@ def _execute(self, desig): drawer_handle = solution['drawer_handle'] drawer_joint = solution['drawer_joint'] dis = solution['distance'] - calculate_and_apply_ik(robot, gripper, kitchen.links[drawer_handle].position) + calculate_and_apply_ik(robot, gripper, kitchen.get_link_position(drawer_handle)) time.sleep(0.2) - han_pose = kitchen.links[drawer_handle].position + han_pose = kitchen.get_link_position(drawer_handle) new_p = Point(han_pose[0] - dis, han_pose[1], han_pose[2]) calculate_and_apply_ik(robot, gripper, new_p) kitchen.set_joint_position(drawer_joint, 0.3) @@ -173,7 +173,7 @@ def _execute(self, desig): objects = World.current_world.get_object_by_type(object_type) for obj in objects: - if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis, 0.5): + if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis, 0.5): return obj diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index ff5da6a0b..34e367404 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -99,7 +99,7 @@ def _execute(self, desig: PlaceMotion.Motion): object_pose = obj.get_pose() local_tf = LocalTransformer() tool_name = robot_description.get_tool_frame(arm) - tcp_to_object = local_tf.transform_pose(object_pose, robot.links[tool_name].tf_frame) + tcp_to_object = local_tf.transform_pose(object_pose, robot.get_link_tf_frame(tool_name)) target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() _move_arm_tcp(target_diff, robot, arm) @@ -124,8 +124,8 @@ def get_pan_and_tilt_goals(self, desig: LookingMotion.Motion) -> Tuple[float, fl target = desig.target local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose(target, self.robot.links["head_pan_link"].tf_frame) - pose_in_tilt = local_transformer.transform_pose(target, self.robot.links["head_tilt_link"].tf_frame) + pose_in_pan = local_transformer.transform_pose(target, self.robot.get_link_tf_frame("head_pan_link")) + pose_in_tilt = local_transformer.transform_pose(target, self.robot.get_link_tf_frame("head_tilt_link")) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 @@ -186,7 +186,7 @@ def _execute(self, desig: DetectingMotion.Motion): objects = World.current_world.get_object_by_type(object_type) for obj in objects: - if btr.visible(obj, robot.links[cam_frame_name].pose, front_facing_axis): + if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): return obj @@ -342,7 +342,7 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: obj_pose = query_result["ClusterPoseBBAnnotator"] lt = LocalTransformer() - obj_pose = lt.transform_pose(obj_pose, World.robot.links["torso_lift_link"].tf_frame) + obj_pose = lt.transform_pose(obj_pose, World.robot.get_link_tf_frame("torso_lift_link")) obj_pose.orientation = [0, 0, 0, 1] obj_pose.position.x += 0.05 diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index e3be3bc6d..a727e75bb 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -55,9 +55,9 @@ def _update_objects(self) -> None: pose.header.stamp = rospy.Time.now() self._publish_pose(obj.tf_frame, pose) for link in obj.link_name_to_id.keys(): - link_pose = obj.links[link].pose + link_pose = obj.get_link_pose(link) link_pose.header.stamp = rospy.Time.now() - self._publish_pose(obj.links[link].tf_frame, link_pose) + self._publish_pose(obj.get_link_tf_frame(link), link_pose) def _update_static_odom(self) -> None: """ diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index d47279a90..8213b1b73 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -11,12 +11,14 @@ import urdf_parser_py from pycram.pose import Transform +from ..world_dataclasses import MeshVisualShape, CylinderVisualShape, BoxVisualShape, SphereVisualShape class VizMarkerPublisher: """ Publishes an Array of visualization marker which represent the situation in the World """ + def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): """ The Publisher creates an Array of Visualization marker with a Marker for each link of each Object in the @@ -59,7 +61,7 @@ def _make_marker_array(self) -> MarkerArray: if obj.name == "floor": continue for link in obj.link_name_to_id.keys(): - geom = obj.links[link].get_geometry() + geom = obj.get_link_geometry(link) if not geom: continue msg = Marker() @@ -68,9 +70,9 @@ def _make_marker_array(self) -> MarkerArray: msg.id = obj.link_name_to_id[link] msg.type = Marker.MESH_RESOURCE msg.action = Marker.ADD - link_pose = obj.links[link].transform - if obj.links[link].origin: - link_origin = obj.links[link].get_origin_transform() + link_pose = obj.get_link_transform(link) + if obj.get_link_origin(link): + link_origin = obj.get_link_origin_transform(link) else: link_origin = Transform() link_pose_with_origin = link_pose * link_origin @@ -81,19 +83,19 @@ def _make_marker_array(self) -> MarkerArray: msg.color = ColorRGBA(*color) msg.lifetime = rospy.Duration(1) - if type(geom) == urdf_parser_py.urdf.Mesh: + if isinstance(geom, MeshVisualShape): msg.type = Marker.MESH_RESOURCE - msg.mesh_resource = "file://" + geom.filename + msg.mesh_resource = "file://" + geom.file_name msg.scale = Vector3(1, 1, 1) msg.mesh_use_embedded_materials = True - elif type(geom) == urdf_parser_py.urdf.Cylinder: + elif isinstance(geom, CylinderVisualShape): msg.type = Marker.CYLINDER msg.scale = Vector3(geom.radius * 2, geom.radius * 2, geom.length) - elif type(geom) == urdf_parser_py.urdf.Box: + elif isinstance(geom, BoxVisualShape): msg.type = Marker.CUBE msg.scale = Vector3(*geom.size) - elif type(geom) == urdf_parser_py.urdf.Sphere: - msg.type == Marker.SPHERE + elif isinstance(geom, SphereVisualShape): + msg.type = Marker.SPHERE msg.scale = Vector3(geom.radius * 2, geom.radius * 2, geom.radius * 2) marker_array.markers.append(msg) @@ -105,4 +107,3 @@ def _stop_publishing(self) -> None: """ self.kill_event.set() self.thread.join() - diff --git a/src/pycram/world.py b/src/pycram/world.py index 4805c0306..67e7efc7a 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -24,6 +24,8 @@ MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, ObjectState, State, WorldState) +from .world_object import Object +from .description import Link class StateEntity: @@ -117,9 +119,9 @@ class World(StateEntity, ABC): used at the moment. """ - robot: Optional['Object'] = None + robot: Optional[Object] = None """ - Global reference to the spawned 'Object'that represents the robot. The robot is identified by checking the name in + Global reference to the spawned Object that represents the robot. The robot is identified by checking the name in the URDF with the name of the URDF on the parameter server. """ @@ -157,7 +159,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.local_transformer = LocalTransformer() self._update_local_transformer_worlds() - self.objects: List['Object'] = [] + self.objects: List[Object] = [] # List of all Objects in the World self.id: int = -1 @@ -166,7 +168,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.mode: WorldMode = mode # The mode of the simulation, can be "GUI" or "DIRECT" - self.coll_callbacks: Dict[Tuple['Object', 'Object'], CollisionCallbacks] = {} + self.coll_callbacks: Dict[Tuple[Object, Object], CollisionCallbacks] = {} self._init_events() @@ -218,7 +220,7 @@ def _sync_prospection_world(self): self.world_sync.start() def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - obj: 'Object') -> str: + obj: Object) -> str: """ Updates the cache directory with the given object. @@ -246,7 +248,7 @@ def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: """ pass - def get_object_by_name(self, name: str) -> List['Object']: + def get_object_by_name(self, name: str) -> List[Object]: """ Returns a list of all Objects in this World with the same name as the given one. @@ -255,7 +257,7 @@ def get_object_by_name(self, name: str) -> List['Object']: """ return list(filter(lambda obj: obj.name == name, self.objects))[0] - def get_object_by_type(self, obj_type: ObjectType) -> List['Object']: + def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: """ Returns a list of all Objects which have the type 'obj_type'. @@ -264,29 +266,29 @@ def get_object_by_type(self, obj_type: ObjectType) -> List['Object']: """ return list(filter(lambda obj: obj.obj_type == obj_type, self.objects)) - def get_object_by_id(self, obj_id: int) -> 'Object': + def get_object_by_id(self, obj_id: int) -> Object: """ - Returns the single 'Object'that has the unique id. + Returns the single Object that has the unique id. - :param obj_id: The unique id for which the 'Object'should be returned. - :return: The 'Object'with the id 'id'. + :param obj_id: The unique id for which the Object should be returned. + :return: The Object with the id 'id'. """ return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] @abstractmethod - def remove_object_from_simulator(self, obj: 'Object') -> None: + def remove_object_from_simulator(self, obj: Object) -> None: """ Removes an object from the physics simulator. :param obj: The object to be removed. """ pass - def remove_object(self, obj: 'Object') -> None: + def remove_object(self, obj: Object) -> None: """ Removes this object from the current world. For the object to be removed it has to be detached from all objects it is currently attached to. After this is done a call to world remove object is done - to remove this 'Object'from the simulation/world. + to remove this Object from the simulation/world. :param obj: The object to be removed. """ @@ -305,7 +307,7 @@ def remove_object(self, obj: 'Object') -> None: if World.robot == obj: World.robot = None - def add_fixed_constraint(self, parent_link: 'Link', child_link: 'Link', + def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: """ Creates a fixed joint constraint between the given parent and child links, @@ -347,7 +349,7 @@ def remove_constraint(self, constraint_id) -> None: pass @abstractmethod - def get_joint_position(self, obj: 'Object', joint_name: str) -> float: + def get_joint_position(self, obj: Object, joint_name: str) -> float: """ Get the position of a joint of an articulated object @@ -358,7 +360,7 @@ def get_joint_position(self, obj: 'Object', joint_name: str) -> float: pass @abstractmethod - def get_object_joint_names(self, obj: 'Object') -> List[str]: + def get_object_joint_names(self, obj: Object) -> List[str]: """ Returns the names of all joints of this object. @@ -368,7 +370,7 @@ def get_object_joint_names(self, obj: 'Object') -> List[str]: pass @abstractmethod - def get_link_pose(self, link: 'Link') -> Pose: + def get_link_pose(self, link: Link) -> Pose: """ Get the pose of a link of an articulated object with respect to the world frame. @@ -378,7 +380,7 @@ def get_link_pose(self, link: 'Link') -> Pose: pass @abstractmethod - def get_object_link_names(self, obj: 'Object') -> List[str]: + def get_object_link_names(self, obj: Object) -> List[str]: """ Returns the names of all links of this object. :param obj: The object. @@ -419,7 +421,7 @@ def update_all_objects_poses(self) -> None: obj.update_pose() @abstractmethod - def get_object_pose(self, obj: 'Object') -> Pose: + def get_object_pose(self, obj: Object) -> Pose: """ Get the pose of an object in the world frame from the current object pose in the simulator. """ @@ -433,9 +435,9 @@ def perform_collision_detection(self) -> None: pass @abstractmethod - def get_object_contact_points(self, obj: 'Object') -> List: + def get_object_contact_points(self, obj: Object) -> List: """ - Returns a list of contact points of this 'Object'with all other Objects. + Returns a list of contact points of this Object with all other Objects. :param obj: The object. :return: A list of all contact points with other objects @@ -443,7 +445,7 @@ def get_object_contact_points(self, obj: 'Object') -> List: pass @abstractmethod - def get_contact_points_between_two_objects(self, obj1: 'Object', obj2: 'Object') -> List: + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: """ Returns a list of contact points between obj1 and obj2. @@ -454,7 +456,7 @@ def get_contact_points_between_two_objects(self, obj1: 'Object', obj2: 'Object') pass @abstractmethod - def reset_joint_position(self, obj: 'Object', joint_name: str, joint_pose: float) -> None: + def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: """ Reset the joint position instantly without physics simulation @@ -465,7 +467,7 @@ def reset_joint_position(self, obj: 'Object', joint_name: str, joint_pose: float pass @abstractmethod - def reset_object_base_pose(self, obj: 'Object', pose: Pose): + def reset_object_base_pose(self, obj: Object, pose: Pose): """ 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. @@ -483,7 +485,7 @@ def step(self): pass @abstractmethod - def set_link_color(self, link: 'Link', rgba_color: Color): + def set_link_color(self, link: Link, rgba_color: Color): """ Changes the rgba_color of a link of this object, the rgba_color has to be given as Color object. @@ -493,7 +495,7 @@ def set_link_color(self, link: 'Link', rgba_color: Color): pass @abstractmethod - def get_link_color(self, link: 'Link') -> Color: + def get_link_color(self, link: Link) -> Color: """ This method returns the rgba_color of this link. :param link: The link for which the rgba_color should be returned. @@ -502,7 +504,7 @@ def get_link_color(self, link: 'Link') -> Color: pass @abstractmethod - def get_colors_of_object_links(self, obj: 'Object') -> Dict[str, Color]: + def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: """ Get the RGBA colors of each link in the object as a dictionary from link name to rgba_color. @@ -512,7 +514,7 @@ def get_colors_of_object_links(self, obj: 'Object') -> Dict[str, Color]: pass @abstractmethod - def get_object_axis_aligned_bounding_box(self, obj: 'Object') -> AxisAlignedBoundingBox: + def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. @@ -523,7 +525,7 @@ def get_object_axis_aligned_bounding_box(self, obj: 'Object') -> AxisAlignedBoun pass @abstractmethod - def get_link_axis_aligned_bounding_box(self, link: 'Link') -> AxisAlignedBoundingBox: + def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: """ Returns the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. @@ -550,21 +552,21 @@ def set_gravity(self, gravity_vector: List[float]) -> None: """ pass - def set_robot_if_not_set(self, robot: 'Object') -> None: + def set_robot_if_not_set(self, robot: Object) -> None: """ Sets the robot if it is not set yet. - :param robot: The 'Object'reference to the 'Object'representing the robot. + :param robot: The Object reference to the Object representing the robot. """ if not self.robot_is_set(): self.set_robot(robot) @staticmethod - def set_robot(robot: Union['Object', None]) -> None: + def set_robot(robot: Union[Object, None]) -> None: """ - Sets the global variable for the robot 'Object' This should be set on spawning the robot. + Sets the global variable for the robot Object This should be set on spawning the robot. - :param robot: The 'Object'reference to the 'Object'representing the robot. + :param robot: The Object reference to the Object representing the robot. """ World.robot = robot @@ -710,7 +712,7 @@ def get_images_for_target(self, 1. An RGB image 2. A depth image - 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible 'Object' + 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object :param target_pose: The pose to which the camera should point. :param cam_pose: The pose of the camera. @@ -720,8 +722,8 @@ def get_images_for_target(self, pass def register_two_objects_collision_callbacks(self, - object_a: 'Object', - object_b: 'Object', + object_a: Object, + object_b: Object, on_collision_callback: Callable, on_collision_removal_callback: Optional[Callable] = None) -> None: """ @@ -740,16 +742,16 @@ def register_two_objects_collision_callbacks(self, def add_resource_path(cls, path: str) -> None: """ Adds a resource path in which the World will search for files. This resource directory is searched if an - 'Object'is spawned only with a filename. + Object is spawned only with a filename. :param path: A path in the filesystem in which to search for files. """ cls.data_directory.append(path) - def get_prospection_object_for_object(self, obj: 'Object') -> 'Object': + def get_prospection_object_for_object(self, obj: Object) -> Object: """ Returns the corresponding object from the prospection world for a given object in the main world. - If the given 'Object'is already in the prospection world, it is returned. + If the given Object is already in the prospection world, it is returned. :param obj: The object for which the corresponding object in the prospection World should be found. :return: The corresponding object in the prospection world. @@ -766,7 +768,7 @@ def get_prospection_object_for_object(self, obj: 'Object') -> 'Object': f" the object isn't anymore in the main (graphical) World" f" or if the given object is already a prospection object. ") - def get_object_for_prospection_object(self, prospection_object: 'Object') -> 'Object': + def get_object_for_prospection_object(self, prospection_object: Object) -> Object: """ Returns the corresponding object from the main World for a given object in the prospection world. If the given object is not in the prospection @@ -960,7 +962,7 @@ def remove_text(self, text_id: Optional[int] = None) -> None: """ raise NotImplementedError - def enable_joint_force_torque_sensor(self, obj: 'Object', fts_joint_idx: int) -> None: + def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: """ You can enable a joint force/torque sensor in each joint. Once enabled, if you perform a simulation step, the get_joint_reaction_force_torque will report the joint reaction forces in @@ -972,7 +974,7 @@ def enable_joint_force_torque_sensor(self, obj: 'Object', fts_joint_idx: int) -> """ raise NotImplementedError - def disable_joint_force_torque_sensor(self, obj: 'Object', joint_id: int) -> None: + def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: """ Disables the force torque sensor of a joint. :param obj: The object in which the joint is located. @@ -980,7 +982,7 @@ def disable_joint_force_torque_sensor(self, obj: 'Object', joint_id: int) -> Non """ raise NotImplementedError - def get_joint_reaction_force_torque(self, obj: 'Object', joint_id: int) -> List[float]: + def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[float]: """ Returns the joint reaction forces and torques of the specified joint. :param obj: The object in which the joint is located. @@ -989,7 +991,7 @@ def get_joint_reaction_force_torque(self, obj: 'Object', joint_id: int) -> List[ """ raise NotImplementedError - def get_applied_joint_motor_torque(self, obj: 'Object', joint_id: int) -> float: + def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: """ Returns the applied torque by a joint motor. :param obj: The object in which the joint is located. @@ -1057,7 +1059,7 @@ def __init__(self, world: World, prospection_world: World): self.remove_obj_queue: Queue = Queue() self.pause_sync: bool = False # Maps world to prospection world objects - self.object_mapping: Dict['Object', 'Object'] = {} + self.object_mapping: Dict[Object, Object] = {} self.equal_states = False def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): diff --git a/src/pycram/world_constraints.py b/src/pycram/world_constraints.py index 52d8cdd59..de2ba2b62 100644 --- a/src/pycram/world_constraints.py +++ b/src/pycram/world_constraints.py @@ -1,8 +1,12 @@ +from __future__ import annotations + from geometry_msgs.msg import Point from typing_extensions import Union, List, Optional -from pycram.enums import JointType -from pycram.pose import Transform, Pose +from .enums import JointType +from .pose import Transform, Pose + +from .description import Link class AbstractConstraint: @@ -12,13 +16,13 @@ class AbstractConstraint: """ def __init__(self, - parent_link: 'Link', - child_link: 'Link', + parent_link: Link, + child_link: Link, _type: JointType, parent_to_constraint: Transform, child_to_constraint: Transform): - self.parent_link: 'Link' = parent_link - self.child_link: 'Link' = child_link + self.parent_link: Link = parent_link + self.child_link: Link = child_link self.type: JointType = _type self.parent_to_constraint = parent_to_constraint self.child_to_constraint = child_to_constraint @@ -132,8 +136,8 @@ class Constraint(AbstractConstraint): """ def __init__(self, - parent_link: 'Link', - child_link: 'Link', + parent_link: Link, + child_link: Link, _type: JointType, axis_in_child_frame: Point, constraint_to_parent: Transform, @@ -154,8 +158,8 @@ def axis_as_list(self) -> List[float]: class Attachment(AbstractConstraint): def __init__(self, - parent_link: 'Link', - child_link: 'Link', + parent_link: Link, + child_link: Link, bidirectional: Optional[bool] = False, parent_to_child_transform: Optional[Transform] = None, constraint_id: Optional[int] = None): diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 54b04876a..55f335f53 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -1,15 +1,20 @@ +from __future__ import annotations + from dataclasses import dataclass from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union from .enums import JointType, Shape from .pose import Pose, Point from abc import ABC, abstractmethod +from .description import Link +from .world_object import Object +from .world_constraints import Attachment def get_point_as_list(point: Point) -> List[float]: """ Returns the point as a list. - :param point: The point + :param point: The point. :return: The point as a list """ return [point.x, point.y, point.z] @@ -199,6 +204,10 @@ def shape_data(self) -> Dict[str, List[float]]: def visual_geometry_type(self) -> Shape: return Shape.BOX + @property + def size(self) -> List[float]: + return self.half_extents + @dataclass class SphereVisualShape(VisualShape): @@ -235,11 +244,11 @@ def visual_geometry_type(self) -> Shape: @dataclass class MeshVisualShape(VisualShape): - mesh_scale: List[float] - mesh_file_name: str + scale: List[float] + file_name: str def shape_data(self) -> Dict[str, Union[List[float], str]]: - return {"meshScale": self.mesh_scale, "meshFileName": self.mesh_file_name} + return {"meshScale": self.scale, "meshFileName": self.file_name} @property def visual_geometry_type(self) -> Shape: @@ -264,24 +273,24 @@ class State(ABC): @dataclass -class WorldState(State): - simulator_state_id: int - object_states: Dict[str, 'ObjectState'] +class LinkState(State): + constraint_ids: Dict[Link, int] @dataclass -class ObjectState(State): - pose: Pose - attachments: Dict['Object', 'Attachment'] - link_states: Dict[int, 'LinkState'] - joint_states: Dict[int, 'JointState'] +class JointState(State): + position: float @dataclass -class LinkState(State): - constraint_ids: Dict['Link', int] +class ObjectState(State): + pose: Pose + attachments: Dict[Object, Attachment] + link_states: Dict[int, LinkState] + joint_states: Dict[int, JointState] @dataclass -class JointState(State): - position: float +class WorldState(State): + simulator_state_id: int + object_states: Dict[str, ObjectState] diff --git a/src/pycram/world_object.py b/src/pycram/world_object.py index 1eee6c20d..791e71b90 100644 --- a/src/pycram/world_object.py +++ b/src/pycram/world_object.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import logging import os @@ -6,13 +8,16 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from pycram.enums import ObjectType, JointType -from pycram.local_transformer import LocalTransformer -from pycram.pose import Pose -from pycram.robot_descriptions import robot_description -from pycram.world import WorldEntity, World -from pycram.world_constraints import Attachment -from pycram.world_dataclasses import Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox +from .enums import ObjectType, JointType +from .local_transformer import LocalTransformer +from .pose import Pose, Transform +from .robot_descriptions import robot_description +from .world import WorldEntity, World +from .world_constraints import Attachment +from .world_dataclasses import Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape +from .description import ObjectDescription, LinkDescription + +Link = ObjectDescription.Link class Object(WorldEntity): @@ -27,7 +32,7 @@ class Object(WorldEntity): """ def __init__(self, name: str, obj_type: ObjectType, path: str, - description: Type['ObjectDescription'], + description: Type[ObjectDescription], pose: Optional[Pose] = None, world: Optional[World] = None, color: Optional[Color] = Color(), @@ -181,6 +186,126 @@ def joint_names(self) -> List[str]: """ return self.world.get_object_joint_names(self) + def get_link(self, link_name: str) -> Link: + """ + Returns the link object with the given name. + :param link_name: The name of the link. + :return: The link object. + """ + return self.links[link_name] + + def get_link_pose(self, link_name: str) -> Pose: + """ + Returns the pose of the link with the given name. + :param link_name: The name of the link. + :return: The pose of the link. + """ + return self.links[link_name].pose + + def get_link_position(self, link_name: str) -> Point: + """ + Returns the position of the link with the given name. + :param link_name: The name of the link. + :return: The position of the link. + """ + return self.links[link_name].position + + def get_link_position_as_list(self, link_name: str) -> List[float]: + """ + Returns the position of the link with the given name. + :param link_name: The name of the link. + :return: The position of the link. + """ + return self.links[link_name].position_as_list + + def get_link_orientation(self, link_name: str) -> Quaternion: + """ + Returns the orientation of the link with the given name. + :param link_name: The name of the link. + :return: The orientation of the link. + """ + return self.links[link_name].orientation + + def get_link_orientation_as_list(self, link_name: str) -> List[float]: + """ + Returns the orientation of the link with the given name. + :param link_name: The name of the link. + :return: The orientation of the link. + """ + return self.links[link_name].orientation_as_list + + def get_link_tf_frame(self, link_name: str) -> str: + """ + Returns the tf frame of the link with the given name. + :param link_name: The name of the link. + :return: The tf frame of the link. + """ + return self.links[link_name].tf_frame + + def get_link_axis_aligned_bounding_box(self, link_name: str) -> AxisAlignedBoundingBox: + """ + Returns the axis aligned bounding box of the link with the given name. + :param link_name: The name of the link. + :return: The axis aligned bounding box of the link. + """ + return self.links[link_name].get_axis_aligned_bounding_box() + + def get_transform_between_links(self, from_link: str, to_link: str) -> Transform: + """ + Returns the transform between two links. + :param from_link: The name of the link from which the transform should be calculated. + :param to_link: The name of the link to which the transform should be calculated. + """ + return self.links[from_link].get_transform_to_link(self.links[to_link]) + + def get_link_color(self, link_name: str) -> Color: + """ + Returns the color of the link with the given name. + :param link_name: The name of the link. + :return: The color of the link. + """ + return self.links[link_name].color + + def set_link_color(self, link_name: str, color: List[float]) -> None: + """ + Sets the color of the link with the given name. + :param link_name: The name of the link. + :param color: The new color of the link. + """ + self.links[link_name].color = Color.from_list(color) + + def get_link_geometry(self, link_name: str) -> VisualShape: + """ + Returns the geometry of the link with the given name. + :param link_name: The name of the link. + :return: The geometry of the link. + """ + return self.links[link_name].geometry + + def get_link_transform(self, link_name: str) -> Transform: + """ + Returns the transform of the link with the given name. + :param link_name: The name of the link. + :return: The transform of the link. + """ + return self.links[link_name].transform + + def get_link_origin(self, link_name: str) -> Pose: + """ + Returns the origin of the link with the given name. + :param link_name: The name of the link. + :return: The origin of the link as a 'Pose'. + """ + return self.links[link_name].origin + + def get_link_origin_transform(self, link_name: str) -> Transform: + """ + Returns the origin transform of the link with the given name. + :param link_name: The name of the link. + :return: The origin transform of the link. + """ + return self.links[link_name].origin_transform + @property def base_origin_shift(self) -> np.ndarray: """ @@ -218,7 +343,7 @@ def reset(self, remove_saved_states=True) -> None: self.remove_saved_states() def attach(self, - child_object: 'Object', + child_object: Object, parent_link: Optional[str] = None, child_link: Optional[str] = None, bidirectional: Optional[bool] = True) -> None: @@ -247,7 +372,7 @@ def attach(self, self.world.attachment_event(self, [self, child_object]) - def detach(self, child_object: 'Object') -> None: + def detach(self, child_object: Object) -> None: """ Detaches another object from this object. This is done by deleting the attachment from the attachments dictionary of both objects @@ -269,7 +394,7 @@ def detach_all(self) -> None: for att in attachments.keys(): self.detach(att) - def update_attachment_with_object(self, child_object: 'Object'): + def update_attachment_with_object(self, child_object: Object): self.attachments[child_object].update_transform_and_constraint() def get_position(self) -> Point: @@ -456,7 +581,7 @@ def remove_joints_saved_states(self) -> None: for joint in self.joints.values(): joint.remove_saved_states() - def _set_attached_objects_poses(self, already_moved_objects: Optional[List['Object']] = None) -> None: + def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: """ Updates the positions of all attached objects. This is done by calculating the new pose in world coordinate frame and setting the @@ -542,7 +667,7 @@ def get_joint_id(self, name: str) -> int: """ return self.joint_name_to_id[name] - def get_root_link_description(self) -> 'LinkDescription': + def get_root_link_description(self) -> LinkDescription: """ Returns the root link of the URDF of this object. :return: The root link as defined in the URDF of this object. @@ -552,7 +677,7 @@ def get_root_link_description(self) -> 'LinkDescription': return link_description @property - def root_link(self) -> 'Link': + def root_link(self) -> Link: """ Returns the root link of this object. :return: The root link of this object. @@ -582,7 +707,7 @@ def get_link_id(self, link_name: str) -> int: """ return self.link_name_to_id[link_name] - def get_link_by_id(self, link_id: int) -> 'Link': + def get_link_by_id(self, link_id: int) -> Link: """ Returns the link for a given unique link id :param link_id: The unique id of the link. @@ -645,6 +770,9 @@ def get_joint_type(self, joint_name: str) -> JointType: def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: return self.joints[joint_name].limits + def get_joint_child_link(self, joint_name: str) -> Link: + return self.joints[joint_name].child_link + def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: """ Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. @@ -757,7 +885,7 @@ def get_base_origin(self) -> Pose: return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z], self.get_orientation_as_list()) - def __copy__(self) -> 'Object': + def __copy__(self) -> Object: """ Returns a copy of this object. The copy will have the same name, type, path, description, pose, world and color. :return: A copy of this object. diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 56da2945f..2e9103084 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -202,7 +202,7 @@ def reachable( if not target_pose: return False - gripper_pose = prospection_robot.links[gripper_name].pose + gripper_pose = prospection_robot.get_link_pose(gripper_name) diff = target_pose.dist(gripper_pose) return diff < threshold @@ -273,4 +273,4 @@ def link_pose_for_joint_config( with UseProspectionWorld(): for joint, pose in joint_config.items(): prospection_object.set_joint_position(joint, pose) - return prospection_object.links[link_name].pose + return prospection_object.get_link_pose(link_name) diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 37a5f13d4..d972aa0c0 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -131,8 +131,8 @@ def test_grasping(self): with simulated_robot: description.resolve().perform() dist = np.linalg.norm( - np.array(self.robot.links[robot_description.get_tool_frame("right")].position_as_list) - - np.array(self.milk.get_pose().position_as_list())) + np.array(self.robot.get_link_position_as_list(robot_description.get_tool_frame("right"))) - + np.array(self.milk.get_position_as_list())) self.assertTrue(dist < 0.01) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index e68397875..2c1bf0682 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -23,9 +23,9 @@ def test_object_movement(self): def test_robot_orientation(self): self.robot.set_pose(Pose([0, 1, 1])) - head_position = self.robot.links['head_pan_link'].position.z + head_position = self.robot.get_link_position('head_pan_link').z self.robot.set_orientation(Pose(orientation=[0, 0, 1, 1])) - self.assertEqual(self.robot.links['head_pan_link'].position.z, head_position) + self.assertEqual(self.robot.get_link_position('head_pan_link').z, head_position) def test_save_and_restore_state(self): self.robot.attach(self.milk) @@ -169,20 +169,20 @@ def test_no_object_found_for_given_prospection_object(self): time.sleep(0.05) def test_add_vis_axis(self): - self.world.add_vis_axis(self.robot.links[robot_description.get_camera_frame()].pose) + self.world.add_vis_axis(self.robot.get_link_pose(robot_description.get_camera_frame())) self.assertTrue(len(self.world.vis_axis) == 1) self.world.remove_vis_axis() self.assertTrue(len(self.world.vis_axis) == 0) def test_add_text(self): - link: ObjectDescription.Link = self.robot.links[robot_description.get_camera_frame()] - text_id = self.world.add_text("test", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, + link: ObjectDescription.Link = self.robot.get_link(robot_description.get_camera_frame()) + text_id = self.world.add_text("test", link.position_as_list, link.orientation_as_list, 1, Color(1, 0, 0, 1), 3, link.object_id, link.id) if self.world.mode == WorldMode.GUI: time.sleep(4) def test_remove_text(self): - link: ObjectDescription.Link = self.robot.links[robot_description.get_camera_frame()] + link: ObjectDescription.Link = self.robot.get_link(robot_description.get_camera_frame()) text_id_1 = self.world.add_text("test 1", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, Color(1, 0, 0, 1), 0, link.object_id, link.id) text_id = self.world.add_text("test 2", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, @@ -195,7 +195,7 @@ def test_remove_text(self): time.sleep(3) def test_remove_all_text(self): - link: ObjectDescription.Link = self.robot.links[robot_description.get_camera_frame()] + link: ObjectDescription.Link = self.robot.get_link(robot_description.get_camera_frame()) text_id_1 = self.world.add_text("test 1", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, Color(1, 0, 0, 1), 0, link.object_id, link.id) text_id = self.world.add_text("test 2", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, @@ -211,7 +211,7 @@ def test_remove_all_text(self): class BulletWorldTestGUI(BulletWorldGUITestCase): def test_add_vis_axis(self): time.sleep(10) - self.world.add_vis_axis(self.robot.links[robot_description.get_camera_frame()].pose) + self.world.add_vis_axis(self.robot.get_link_pose(robot_description.get_camera_frame())) self.assertTrue(len(self.world.vis_axis) == 1) self.world.remove_vis_axis() self.assertTrue(len(self.world.vis_axis) == 0) diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index 2643eb40e..db81af34f 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -1,7 +1,7 @@ import time -from bullet_world_testcase import BulletWorldTestCase import pycram.world_reasoning as btr +from bullet_world_testcase import BulletWorldTestCase from pycram.pose import Pose from pycram.robot_descriptions import robot_description @@ -21,15 +21,15 @@ def test_visible(self): self.robot.set_pose(Pose()) time.sleep(1) camera_frame = robot_description.get_camera_frame() - self.world.add_vis_axis(self.robot.links[camera_frame].pose) - self.assertTrue(btr.visible(self.milk, self.robot.links[camera_frame].pose, + self.world.add_vis_axis(self.robot.get_link_pose(camera_frame)) + self.assertTrue(btr.visible(self.milk, self.robot.get_link_pose(camera_frame), robot_description.front_facing_axis)) def test_occluding(self): self.milk.set_pose(Pose([3, 0, 1.2])) self.robot.set_pose(Pose()) - self.assertTrue(btr.occluding(self.milk, self.robot.links[robot_description.get_camera_frame()].pose, - robot_description.front_facing_axis) != []) + self.assertTrue(btr.occluding(self.milk, self.robot.get_link_pose(robot_description.get_camera_frame()), + robot_description.front_facing_axis) != []) def test_reachable(self): self.robot.set_pose(Pose()) diff --git a/test/test_description.py b/test/test_description.py index 8f3afa201..0d427717c 100644 --- a/test/test_description.py +++ b/test/test_description.py @@ -15,8 +15,8 @@ def test_pose_as_list(self): self.assertEqual(base_link.pose_as_list, base_link.pose.to_list()) def test_joint_child_link(self): - self.assertEqual(self.robot.joints["base_footprint_joint"].child_link, self.robot.links["base_link"]) - self.assertEqual(self.robot.joints["base_footprint_joint"].parent_link, self.robot.root_link) + self.assertEqual(self.robot.get_joint_child_link("base_footprint_joint"), self.robot.get_link("base_link")) + self.assertEqual(self.robot.get_joint_parent_link("base_footprint_joint"), self.robot.root_link) def test_generate_description_from_mesh(self): self.assertTrue(self.milk.description.generate_description_from_file("../resources/cached/milk.stl", ".stl")) diff --git a/test/test_links.py b/test/test_links.py index a7f37016c..af26bb39e 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -29,7 +29,7 @@ def test_transform_link(self): self.assertAlmostEqual(robot_to_milk, milk_to_robot.invert()) def test_set_color(self): - link = self.robot.links['base_link'] + link = self.robot.get_link('base_link') link.color = Color(1, 0, 0, 1) self.assertEqual(link.color.get_rgba(), [1, 0, 0, 1]) diff --git a/test/test_task_tree.py b/test/test_task_tree.py index 1635974a0..36d994663 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -4,7 +4,7 @@ from pycram.task import with_tree import unittest import anytree -from bullet_world_testcase import BulletWorldTestCase +from bullet_world_testcase import BulletWorldTestCase import pycram.plan_failures from pycram.designators import object_designator, action_designator From 8462338eafb5c9f6044ff68ccd8a061cf320c6ef Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 23 Feb 2024 15:21:07 +0100 Subject: [PATCH 59/69] [Abstract World] Solved cyclic imports problem. --- src/pycram/description.py | 6 +++-- src/pycram/helper.py | 2 +- src/pycram/urdf_interface.py | 8 +++---- src/pycram/world.py | 8 ++++--- src/pycram/world_constraints.py | 5 +++-- src/pycram/world_dataclasses.py | 10 +++++---- src/pycram/world_object.py | 39 +++++++++++++++++++++++++++++++++ 7 files changed, 62 insertions(+), 16 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 93513b8aa..3d91601e2 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -6,14 +6,16 @@ import rospy from geometry_msgs.msg import Point, Quaternion -from typing_extensions import Tuple, Union, Any, List, Optional, Dict +from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING from .enums import JointType from .local_transformer import LocalTransformer from .pose import Pose, Transform from .world import WorldEntity from .world_dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape -from .world_object import Object + +if TYPE_CHECKING: + from .world_object import Object class EntityDescription(ABC): diff --git a/src/pycram/helper.py b/src/pycram/helper.py index bb171a0b1..3b07ce6e3 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -55,7 +55,7 @@ def _apply_ik(robot: WorldObject, joint_poses: List[float], joints: List[str]) - def calculate_wrist_tool_offset(wrist_frame: str, tool_frame: str, robot: WorldObject) -> Transform: - return robot.get_transfrom_between_links(wrist_frame, tool_frame) + return robot.get_transform_between_links(wrist_frame, tool_frame) def transform(pose: List[float], diff --git a/src/pycram/urdf_interface.py b/src/pycram/urdf_interface.py index 51ce8c3b3..e33ad8d09 100644 --- a/src/pycram/urdf_interface.py +++ b/src/pycram/urdf_interface.py @@ -10,11 +10,11 @@ from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, Sphere as URDF_Sphere, Mesh as URDF_Mesh) -from pycram.enums import JointType, Shape -from pycram.pose import Pose -from pycram.description import JointDescription as AbstractJointDescription, \ +from .enums import JointType, Shape +from .pose import Pose +from .description import JointDescription as AbstractJointDescription, \ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription -from pycram.world_dataclasses import Color +from .world_dataclasses import Color class LinkDescription(AbstractLinkDescription): diff --git a/src/pycram/world.py b/src/pycram/world.py index 67e7efc7a..96294fc08 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -11,7 +11,7 @@ import numpy as np import rospy from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict, Tuple, Callable +from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING from typing_extensions import Union from .cache_manager import CacheManager @@ -24,8 +24,10 @@ MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, ObjectState, State, WorldState) -from .world_object import Object -from .description import Link + +if TYPE_CHECKING: + from .world_object import Object + from .description import Link class StateEntity: diff --git a/src/pycram/world_constraints.py b/src/pycram/world_constraints.py index de2ba2b62..6dc75e33b 100644 --- a/src/pycram/world_constraints.py +++ b/src/pycram/world_constraints.py @@ -1,12 +1,13 @@ from __future__ import annotations from geometry_msgs.msg import Point -from typing_extensions import Union, List, Optional +from typing_extensions import Union, List, Optional, TYPE_CHECKING from .enums import JointType from .pose import Transform, Pose -from .description import Link +if TYPE_CHECKING: + from .description import Link class AbstractConstraint: diff --git a/src/pycram/world_dataclasses.py b/src/pycram/world_dataclasses.py index 55f335f53..7aea14338 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/world_dataclasses.py @@ -1,13 +1,15 @@ from __future__ import annotations from dataclasses import dataclass -from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union +from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING from .enums import JointType, Shape from .pose import Pose, Point from abc import ABC, abstractmethod -from .description import Link -from .world_object import Object -from .world_constraints import Attachment + +if TYPE_CHECKING: + from .description import Link + from .world_object import Object + from .world_constraints import Attachment def get_point_as_list(point: Point) -> List[float]: diff --git a/src/pycram/world_object.py b/src/pycram/world_object.py index 791e71b90..ba31d8dc2 100644 --- a/src/pycram/world_object.py +++ b/src/pycram/world_object.py @@ -750,29 +750,68 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: self._set_attached_objects_poses() def get_joint_position(self, joint_name: str) -> float: + """ + :param joint_name: The name of the joint + :return: The current position of the given joint + """ return self.joints[joint_name].position def get_joint_damping(self, joint_name: str) -> float: + """ + :param joint_name: The name of the joint + :return: The damping of the given joint + """ return self.joints[joint_name].damping def get_joint_upper_limit(self, joint_name: str) -> float: + """ + :param joint_name: The name of the joint + :return: The upper limit of the given joint + """ return self.joints[joint_name].upper_limit def get_joint_lower_limit(self, joint_name: str) -> float: + """ + :param joint_name: The name of the joint + :return: The lower limit of the given joint + """ return self.joints[joint_name].lower_limit def get_joint_axis(self, joint_name: str) -> Point: + """ + :param joint_name: The name of the joint + :return: The axis of the given joint + """ return self.joints[joint_name].axis def get_joint_type(self, joint_name: str) -> JointType: + """ + :param joint_name: The name of the joint + :return: The type of the given joint + """ return self.joints[joint_name].type def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: + """ + :param joint_name: The name of the joint + :return: The lower and upper limits of the given joint + """ return self.joints[joint_name].limits def get_joint_child_link(self, joint_name: str) -> Link: + """ + :param joint_name: The name of the joint + :return: The child link of the given joint + """ return self.joints[joint_name].child_link + def get_joint_parent_link(self, joint_name: str) -> Link: + """ + :param joint_name: The name of the joint + :return: The parent link of the given joint + """ + return self.joints[joint_name].parent_link + def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: """ Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. From 7b8afe664df3d92dc612d7c1c4b0911b7c6cd01a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 23 Feb 2024 17:45:04 +0100 Subject: [PATCH 60/69] [Abstract World] Resolved requested changes. --- src/pycram/cache_manager.py | 22 +++++++++++--- src/pycram/description.py | 45 ++++++++++++++++++---------- src/pycram/pose.py | 59 +++++++++++++++++++++---------------- src/pycram/world.py | 12 ++++++-- 4 files changed, 91 insertions(+), 47 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 24d3989e7..ae3ea3c68 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -2,27 +2,41 @@ import pathlib import re -from typing_extensions import List +from typing_extensions import List, TYPE_CHECKING + +if TYPE_CHECKING: + from .description import ObjectDescription class CacheManager: + """ + The CacheManager is responsible for caching object description files and managing the cache directory. + """ + mesh_extensions: List[str] = [".obj", ".stl"] """ The file extensions of mesh files. """ def __init__(self, cache_dir: str, data_directory: List[str]): + """ + Initializes the CacheManager. + :param cache_dir: The directory where the cached files are stored. + :param data_directory: The directory where all resource files are stored. + """ self.cache_dir = cache_dir - # The directory where the cached files are stored. - self.data_directory = data_directory - # The directory where all resource files are stored. def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, object_description: 'ObjectDescription', object_name: str) -> str: """ Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. + :param path: The path of the file to preprocess and save in the cache directory. + :param ignore_cached_files: If True, the file will be preprocessed and saved in the cache directory even if it + is already cached. + :param object_description: The object description of the file. + :param object_name: The name of the object. """ path_object = pathlib.Path(path) extension = path_object.suffix diff --git a/src/pycram/description.py b/src/pycram/description.py index 3d91601e2..7286a7e78 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -20,6 +20,10 @@ class EntityDescription(ABC): + """ + A class that represents a description of an entity. This can be a link, joint or object description. + """ + @property @abstractmethod def origin(self) -> Pose: @@ -49,14 +53,14 @@ def __init__(self, parsed_link_description: Any): @abstractmethod def geometry(self) -> VisualShape: """ - Returns the geometry type of the URDF collision element of this link. + Returns the geometry type of the collision element of this link. """ pass class JointDescription(EntityDescription): """ - A class that represents a joint description of a URDF joint. + A class that represents the description of a joint. """ def __init__(self, parsed_joint_description: Any): @@ -468,8 +472,8 @@ def get_object_id(self) -> int: @position.setter def position(self, joint_position: float) -> None: """ - Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, as stated - in the URDF, an error will be printed. However, the joint will be set either way. + Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, + an error will be printed. However, the joint will be set either way. :param joint_position: The target pose for this joint """ @@ -477,10 +481,10 @@ def position(self, joint_position: float) -> None: if self.has_limits: low_lim, up_lim = self.limits if not low_lim <= joint_position <= up_lim: - logging.error( + logging.warning( f"The joint position has to be within the limits of the joint. The joint limits for {self.name}" f" are {low_lim} and {up_lim}") - logging.error(f"The given joint position was: {joint_position}") + logging.warning(f"The given joint position was: {joint_position}") # Temporarily disabled because kdl outputs values exciting joint limits # return self.reset_position(joint_position) @@ -497,15 +501,16 @@ def get_reaction_force_torque(self) -> List[float]: def get_applied_motor_torque(self) -> float: return self.world.get_applied_joint_motor_torque(self.object, self.id) - def restore_state(self, state_id: int) -> None: - self.current_state = self.saved_states[state_id] - @property def current_state(self) -> JointState: return JointState(self.position) @current_state.setter def current_state(self, joint_state: JointState) -> None: + """ + Updates the current state of this joint from the given joint state if the position is different. + :param joint_state: The joint state to update from. + """ if self._current_position != joint_state.position: self.position = joint_state.position @@ -520,7 +525,15 @@ def __hash__(self): class ObjectDescription(EntityDescription): - MESH_EXTENSIONS: Tuple[str] = (".obj", ".stl") + + """ + A class that represents the description of an object. + """ + + mesh_extensions: Tuple[str] = (".obj", ".stl") + """ + The file extensions of the mesh files that can be used to generate a description file. + """ class Link(Link, ABC): ... @@ -532,6 +545,9 @@ class Joint(Joint, ABC): ... def __init__(self, path: Optional[str] = None): + """ + :param path: The path of the file to update the description data from. + """ if path: self.update_description_from_file(path) else: @@ -554,8 +570,7 @@ def parsed_description(self) -> Any: @parsed_description.setter def parsed_description(self, parsed_description: Any): """ - Return the object parsed from the description file. - :param parsed_description: The parsed description object. + :param parsed_description: The parsed description object (depends on the description file type). """ self._parsed_description = parsed_description @@ -578,7 +593,7 @@ def generate_description_from_file(self, path: str, extension: str) -> str: """ description_string = None - if extension in self.MESH_EXTENSIONS: + if extension in self.mesh_extensions: description_string = self.generate_from_mesh_file(path) elif extension == self.get_file_extension(): description_string = self.generate_from_description_file(path) @@ -603,7 +618,7 @@ def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: :param object_name: The name of the object. :return: The file name of the description file. """ - if extension in self.MESH_EXTENSIONS: + if extension in self.mesh_extensions: file_name = path_object.stem + self.get_file_extension() elif extension == self.get_file_extension(): file_name = path_object.name @@ -616,7 +631,7 @@ def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: @abstractmethod def generate_from_mesh_file(cls, path: str) -> str: """ - Generates a description file from one of the mesh types defined in the MESH_EXTENSIONS and + Generates a description file from one of the mesh types defined in the mesh_extensions and returns the path of the generated file. :param path: The path to the .obj file. :return: The path of the generated description file. diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 85f85f08a..74bda8b8d 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -14,6 +14,25 @@ from .orm.base import Pose as ORMPose, Position, Quaternion, ProcessMetaData +def get_normalized_quaternion(quaternion: np.ndarray) -> GeoQuaternion: + """ + Normalizes a given quaternion such that it has a magnitude of 1. + + :param quaternion: The quaternion that should be normalized + :return: The normalized quaternion + """ + mag = math.sqrt(sum(v**2 for v in quaternion)) + normed_rotation = quaternion / mag + + geo_quaternion = GeoQuaternion() + geo_quaternion.x = normed_rotation[0] + geo_quaternion.y = normed_rotation[1] + geo_quaternion.z = normed_rotation[2] + geo_quaternion.w = normed_rotation[3] + + return geo_quaternion + + class Pose(PoseStamped): """ Pose representation for PyCRAM, this class extends the PoseStamped ROS message from geometry_msgs. Thus making it @@ -98,11 +117,11 @@ def position(self, value) -> None: :param value: List or geometry_msgs/Pose message for the position """ - if (not type(value) == list and not type(value) == tuple and not type(value) == GeoPose - and not type(value) == Point): + if (not isinstance(value, list) and not isinstance(value, tuple) and not isinstance(value, GeoPose) + and not isinstance(value, Point)): rospy.logerr("Position can only be a list or geometry_msgs/Pose") raise TypeError("Position can only be a list/tuple or geometry_msgs/Pose") - if type(value) == list or type(value) == tuple and len(value) == 3: + if isinstance(value, list) or isinstance(value, tuple) and len(value) == 3: self.pose.position.x = value[0] self.pose.position.y = value[1] self.pose.position.z = value[2] @@ -125,22 +144,16 @@ def orientation(self, value) -> None: :param value: New orientation, either a list or geometry_msgs/Quaternion """ - if not type(value) == list and not type(value) == tuple and not type(value) == GeoQuaternion: + 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") return - if type(value) == list or type(value) == tuple and len(value) == 4: + if isinstance(value, list) or isinstance(value, tuple) and len(value) == 4: orientation = np.array(value) else: orientation = np.array([value.x, value.y, value.z, value.w]) # This is used instead of np.linalg.norm since numpy is too slow on small arrays - mag = math.sqrt(sum(v**2 for v in orientation)) - normed_orientation = orientation / mag - - self.pose.orientation.x = normed_orientation[0] - self.pose.orientation.y = normed_orientation[1] - self.pose.orientation.z = normed_orientation[2] - self.pose.orientation.w = normed_orientation[3] + self.pose.orientation = get_normalized_quaternion(orientation) def to_list(self) -> List[List[float]]: """ @@ -207,7 +220,7 @@ def __eq__(self, other: Pose) -> bool: :param other: Other pose which should be compared :return: True if both Poses have the same position, orientation and frame. False otherwise """ - if not type(other) == Pose: + if not isinstance(other, Pose): return False self_position = self.position_as_list() other_position = other.position_as_list() @@ -354,10 +367,10 @@ def translation(self, value) -> None: :param value: The new value for the translation, either a list or geometry_msgs/Vector3 """ - if not type(value) == list and not type(value) == Vector3: + if not isinstance(value, list) and not isinstance(value, Vector3): rospy.logwarn("Value of a translation can only be a list of a geometry_msgs/Vector3") return - if type(value) == list and len(value) == 3: + if isinstance(value, list) and len(value) == 3: self.transform.translation.x = value[0] self.transform.translation.y = value[1] self.transform.translation.z = value[2] @@ -379,22 +392,16 @@ def rotation(self, value): :param value: The new value for the rotation, either a list or geometry_msgs/Quaternion """ - if not type(value) == list and not type(value) == GeoQuaternion: + if not isinstance(value, list) and not isinstance(value, GeoQuaternion): rospy.logwarn("Value of the rotation can only be a list or a geometry.msgs/Quaternion") return - if type(value) == list and len(value) == 4: + if isinstance(value, list) and len(value) == 4: rotation = np.array(value) else: rotation = np.array([value.x, value.y, value.z, value.w]) # This is used instead of np.linalg.norm since numpy is too slow on small arrays - mag = math.sqrt(sum(v**2 for v in rotation)) - normed_rotation = rotation / mag - - self.transform.rotation.x = normed_rotation[0] - self.transform.rotation.y = normed_rotation[1] - self.transform.rotation.z = normed_rotation[2] - self.transform.rotation.w = normed_rotation[3] + self.transform.rotation = get_normalized_quaternion(rotation) def copy(self) -> Transform: """ @@ -453,7 +460,7 @@ def __mul__(self, other: Transform) -> Union[Transform, None]: :param other: The Transform which should be multiplied with this one. :return: The resulting Transform from the multiplication """ - if not type(other) == Transform: + if not isinstance(other, Transform): rospy.logerr(f"Can only multiply two Transforms") return self_trans = transformations.translation_matrix(self.translation_as_list()) @@ -487,7 +494,7 @@ def __eq__(self, other: Transform) -> bool: :param other: Other pose which should be compared :return: True if both Transforms have the same translation, rotation, frame and child frame. False otherwise """ - if not type(other) == Transform: + if not isinstance(other, Transform): return False self_position = self.translation_as_list() other_position = other.translation_as_list() diff --git a/src/pycram/world.py b/src/pycram/world.py index 96294fc08..cc1f6fde2 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -164,7 +164,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.objects: List[Object] = [] # List of all Objects in the World - self.id: int = -1 + self.id: Optional[int] = -1 # This is used to connect to the physics server (allows multiple clients) self.mode: WorldMode = mode @@ -1002,6 +1002,9 @@ def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: """ raise NotImplementedError + def __del__(self): + self.exit() + class UseProspectionWorld: """ @@ -1013,6 +1016,11 @@ class UseProspectionWorld: NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() """ + WAIT_TIME_FOR_ADDING_QUEUE = 20 + """ + The time in seconds to wait for the adding queue to be ready. + """ + def __init__(self): self.prev_world: Optional[World] = None # The previous world is saved to restore it after the with block is exited. @@ -1022,7 +1030,7 @@ def __enter__(self): This method is called when entering the with block, it will set the current world to the prospection world """ if not World.current_world.is_prospection_world: - time.sleep(20 * World.current_world.simulation_time_step) + time.sleep(self.WAIT_TIME_FOR_ADDING_QUEUE * World.current_world.simulation_time_step) # blocks until the adding queue is ready World.current_world.world_sync.add_obj_queue.join() From c85cb9115eb44b51992e85ac93f4be79bf02b90b Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 25 Feb 2024 17:43:02 +0100 Subject: [PATCH 61/69] [AbstractWorld] refactored joint position getting and setting to use joint class. --- src/pycram/bullet_world.py | 8 ++++---- src/pycram/description.py | 4 ++-- src/pycram/world.py | 14 ++++++-------- 3 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index f4fc8c300..f5083f6c4 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -92,8 +92,8 @@ def add_constraint(self, constraint: Constraint) -> int: def remove_constraint(self, constraint_id): p.removeConstraint(constraint_id, physicsClientId=self.id) - def get_joint_position(self, obj: Object, joint_name: str) -> float: - return p.getJointState(obj.id, obj.joint_name_to_id[joint_name], physicsClientId=self.id)[0] + def get_joint_position(self, joint: Joint) -> float: + return p.getJointState(joint.object_id, joint.id, physicsClientId=self.id)[0] def get_object_joint_names(self, obj: Object) -> List[str]: return [p.getJointInfo(obj.id, i, physicsClientId=self.id)[1].decode('utf-8') @@ -129,8 +129,8 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> self.perform_collision_detection() return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.id) - def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: - p.resetJointState(obj.id, obj.joint_name_to_id[joint_name], joint_pose, physicsClientId=self.id) + def reset_joint_position(self, joint: Joint, joint_position: str) -> 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(), diff --git a/src/pycram/description.py b/src/pycram/description.py index 7286a7e78..04b3dd667 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -436,7 +436,7 @@ def _update_position(self) -> None: """ Updates the current position of the joint from the physics simulator. """ - self._current_position = self.world.get_joint_position(self.object, self.name) + self._current_position = self.world.get_joint_position(self) @property def parent_link(self) -> Link: @@ -459,7 +459,7 @@ def position(self) -> float: return self._current_position def reset_position(self, position: float) -> None: - self.world.reset_joint_position(self.object, self.name, position) + self.world.reset_joint_position(self, position) self._update_position() def get_object_id(self) -> int: diff --git a/src/pycram/world.py b/src/pycram/world.py index cc1f6fde2..dc93a2c2f 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -27,7 +27,7 @@ if TYPE_CHECKING: from .world_object import Object - from .description import Link + from .description import Link, Joint class StateEntity: @@ -351,12 +351,11 @@ def remove_constraint(self, constraint_id) -> None: pass @abstractmethod - def get_joint_position(self, obj: Object, joint_name: str) -> float: + def get_joint_position(self, joint: Joint) -> float: """ Get the position of a joint of an articulated object - :param obj: The object. - :param joint_name: The name of the joint. + :param joint: The joint to get the position for. :return: The joint position as a float. """ pass @@ -458,13 +457,12 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> pass @abstractmethod - def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None: + def reset_joint_position(self, joint: Joint, joint_position: float) -> None: """ Reset the joint position instantly without physics simulation - :param obj: The object - :param joint_name: The name of the joint - :param joint_pose: The new joint pose + :param joint: The joint to reset the position for. + :param joint_position: The new joint pose. """ pass From 8b813538a756156408737c59c1892f4795542d13 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 27 Feb 2024 08:30:34 +0100 Subject: [PATCH 62/69] [AbstractWorld] allowing not object description for initializing objects. still in development. --- src/neem_interface_python | 2 +- src/pycram/bullet_world.py | 9 ++++++++- src/pycram/cache_manager.py | 16 ++++++---------- src/pycram/description.py | 8 +++++--- src/pycram/urdf_interface.py | 5 +++-- src/pycram/world.py | 9 +++++---- src/pycram/world_object.py | 26 +++++++++++++++----------- 7 files changed, 43 insertions(+), 32 deletions(-) diff --git a/src/neem_interface_python b/src/neem_interface_python index 182daab69..05ad42c41 160000 --- a/src/neem_interface_python +++ b/src/neem_interface_python @@ -1 +1 @@ -Subproject commit 182daab69cb2e4dab23fbf5c9b9d1146837d8102 +Subproject commit 05ad42c41042335dd2287d865f6a37c115ea8ffe diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index f5083f6c4..67536bcf1 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -66,7 +66,14 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo _ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, ObjectDescription, world=self) - def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: + def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + if pose is None: + pose = Pose() + return self._load_object_and_get_id(path, pose) + + def _load_object_and_get_id(self, path: str, pose: Pose) -> int: + if path is None: + raise ValueError("Path to the object file is required.") return p.loadURDF(path, basePosition=pose.position_as_list(), baseOrientation=pose.orientation_as_list(), physicsClientId=self.id) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index ae3ea3c68..835c07cef 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -50,20 +50,21 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, # if file is not yet cached preprocess the description file and save it in the cache directory. if not self.is_cached(path, object_description) or ignore_cached_files: - self.generate_description_and_write_to_cache(path, extension, cache_path, object_description) + self.generate_description_and_write_to_cache(path, object_name, extension, cache_path, object_description) return cache_path - def generate_description_and_write_to_cache(self, path: str, extension: str, cache_path: str, + def generate_description_and_write_to_cache(self, path: str, name: str, extension: str, cache_path: str, object_description: 'ObjectDescription') -> None: """ Generates the description from the file at the given path and writes it to the cache directory. :param path: The path of the file to preprocess. + :param name: The name of the object. :param extension: The file extension of the file to preprocess. :param cache_path: The path of the file in the cache directory. :param object_description: The object description of the file. """ - description_string = object_description.generate_description_from_file(path, extension) + description_string = object_description.generate_description_from_file(path, name, extension) self.write_to_cache(description_string, cache_path) @staticmethod @@ -88,14 +89,9 @@ def look_for_file_in_data_dir(self, path: str, path_object: pathlib.Path) -> str for file in os.listdir(data_dir): if file == path: return data_dir + f"/{path}" - if path: - break - if not path: - raise FileNotFoundError( - f"File {path_object.name} could not be found in the resource directory {self.data_directory}") - - return path + raise FileNotFoundError( + f"File {path_object.name} could not be found in the resource directory {self.data_directory}") def create_cache_dir_if_not_exists(self): """ diff --git a/src/pycram/description.py b/src/pycram/description.py index 04b3dd667..e0b9ecc1e 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -583,18 +583,19 @@ def load_description(self, path: str) -> Any: """ pass - def generate_description_from_file(self, path: str, extension: str) -> str: + def generate_description_from_file(self, path: str, name: str, extension: str) -> str: """ Generates and preprocesses the description from the file at the given path and returns the preprocessed description as a string. :param path: The path of the file to preprocess. + :param name: The name of the object. :param extension: The file extension of the file to preprocess. :return: The processed description string. """ description_string = None if extension in self.mesh_extensions: - description_string = self.generate_from_mesh_file(path) + description_string = self.generate_from_mesh_file(path, name) elif extension == self.get_file_extension(): description_string = self.generate_from_description_file(path) else: @@ -629,11 +630,12 @@ def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: @classmethod @abstractmethod - def generate_from_mesh_file(cls, path: str) -> str: + def generate_from_mesh_file(cls, path: str, name: str) -> str: """ Generates a description file from one of the mesh types defined in the mesh_extensions and returns the path of the generated file. :param path: The path to the .obj file. + :param name: The name of the object. :return: The path of the generated description file. """ pass diff --git a/src/pycram/urdf_interface.py b/src/pycram/urdf_interface.py index e33ad8d09..4dc1abe11 100644 --- a/src/pycram/urdf_interface.py +++ b/src/pycram/urdf_interface.py @@ -158,12 +158,13 @@ def load_description(self, path) -> URDF: with open(path, 'r') as file: return URDF.from_xml_string(file.read()) - def generate_from_mesh_file(self, path, color: Optional[Color] = Color()) -> str: + def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color()) -> str: """ Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be used to create a material tag in the URDF. :param path: The path to the mesh file. + :param name: The name of the object. :param color: The color of the object. :return: The absolute path of the created file """ @@ -189,7 +190,7 @@ def generate_from_mesh_file(self, path, color: Optional[Color] = Color()) -> str rgb = " ".join(list(map(str, color.get_rgba()))) pathlib_obj = pathlib.Path(path) path = str(pathlib_obj.resolve()) - content = urdf_template.replace("~a", self.name).replace("~b", path).replace("~c", rgb) + content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) return content def generate_from_description_file(self, path: str) -> str: diff --git a/src/pycram/world.py b/src/pycram/world.py index dc93a2c2f..722325501 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -129,12 +129,13 @@ class World(StateEntity, ABC): data_directory: List[str] = [os.path.join(os.path.dirname(__file__), '..', '..', 'resources')] """ - Global reference for the data directories, this is used to search for the URDF files of the robot and the objects. + Global reference for the data directories, this is used to search for the description files of the robot + and the objects. """ cache_dir = data_directory[0] + '/cached/' """ - Global reference for the cache directory, this is used to cache the URDF files of the robot and the objects. + Global reference for the cache directory, this is used to cache the description files of the robot and the objects. """ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): @@ -240,11 +241,11 @@ def simulation_time_step(self): return 1 / World.simulation_frequency @abstractmethod - def load_description_and_get_object_id(self, path: str, pose: Pose) -> int: + def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: """ Loads a description file (e.g. URDF) at the given pose and returns the id of the loaded object. - :param path: The path to the description file. + :param path: The path to the description file, if None the description file is assumed to be already loaded. :param pose: The pose at which the object should be loaded. :return: The id of the loaded object. """ diff --git a/src/pycram/world_object.py b/src/pycram/world_object.py index ba31d8dc2..8206b21e8 100644 --- a/src/pycram/world_object.py +++ b/src/pycram/world_object.py @@ -77,8 +77,9 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") + f"{self.name}_{self.id}") - if self.description.name == robot_description.name: - self.world.set_robot_if_not_set(self) + if robot_description is not None: + if self.description.name == robot_description.name: + self.world.set_robot_if_not_set(self) self._init_joint_name_and_id_map() self._init_link_name_and_id_map() @@ -93,7 +94,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) - def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, str]: + def _load_object_and_get_id(self, path: Optional[str] = None, + ignore_cached_files: Optional[bool] = False) -> Tuple[int, Union[str, None]]: """ Loads an object to the given World with the given position and orientation. The rgba_color will only be used when an .obj or .stl file is given. @@ -103,19 +105,21 @@ def _load_object_and_get_id(self, path, ignore_cached_files: bool) -> Tuple[int, This new file will have resolved mesh file paths, meaning there will be no references to ROS packges instead there will be absolute file paths. + :param path: The path to the description file, if None then no file will be loaded, this is useful when the + PyCRAM is not responsible for loading the file but another system is. :param ignore_cached_files: Whether to ignore files in the cache directory. :return: The unique id of the object and the path of the file that was loaded. """ + if path is not None: + try: + path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) + except FileNotFoundError as e: + logging.error("Could not generate description from file.") + raise e try: - path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) - except FileNotFoundError as e: - logging.error("Could not generate description from file.") - raise e - - try: - obj_id = self.world.load_description_and_get_object_id(path, Pose(self.get_position_as_list(), - self.get_orientation_as_list())) + obj_id = self.world.load_object_and_get_id(path, Pose(self.get_position_as_list(), + self.get_orientation_as_list())) return obj_id, path except Exception as e: logging.error( From b4a384f89ee8d49c62564249837bde4939bd35c6 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 27 Feb 2024 16:34:37 +0100 Subject: [PATCH 63/69] [Abstract World] solved cache search problem, tests are passing. --- src/pycram/cache_manager.py | 21 ++++++++++----------- test/test_cache_manager.py | 6 ++++-- test/test_description.py | 6 ++++-- 3 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 835c07cef..2dc479d8d 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -41,15 +41,14 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, path_object = pathlib.Path(path) extension = path_object.suffix - path = self.look_for_file_in_data_dir(path, path_object) - self.create_cache_dir_if_not_exists() # save correct path in case the file is already in the cache directory cache_path = self.cache_dir + object_description.get_file_name(path_object, extension, object_name) - # if file is not yet cached preprocess the description file and save it in the cache directory. if not self.is_cached(path, object_description) or ignore_cached_files: + # if file is not yet cached preprocess the description file and save it in the cache directory. + path = self.look_for_file_in_data_dir(path_object) self.generate_description_and_write_to_cache(path, object_name, extension, cache_path, object_description) return cache_path @@ -77,21 +76,21 @@ def write_to_cache(description_string: str, cache_path: str) -> None: with open(cache_path, "w") as file: file.write(description_string) - def look_for_file_in_data_dir(self, path: str, path_object: pathlib.Path) -> str: + def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: """ Looks for a file in the data directory of the World. If the file is not found in the data directory, this method raises a FileNotFoundError. - :param path: The path of the file to look for. :param path_object: The pathlib object of the file to look for. """ - if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path): - for data_dir in self.data_directory: - for file in os.listdir(data_dir): - if file == path: - return data_dir + f"/{path}" + + name = path_object.name + for data_dir in self.data_directory: + for file in os.listdir(data_dir): + if file == name: + return data_dir + f"/{name}" raise FileNotFoundError( - f"File {path_object.name} could not be found in the resource directory {self.data_directory}") + f"File {name} could not be found in the resource directory {self.data_directory}") def create_cache_dir_if_not_exists(self): """ diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index c69e1ea9f..bbdf686d3 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -1,3 +1,5 @@ +from pathlib import Path + from bullet_world_testcase import BulletWorldTestCase from pycram.enums import ObjectType from pycram.urdf_interface import ObjectDescription @@ -9,9 +11,9 @@ class TestCacheManager(BulletWorldTestCase): def test_generate_description_and_write_to_cache(self): cache_manager = self.world.cache_manager path = "../resources/apartment.urdf" - extension = ".urdf" + extension = Path(path).suffix cache_path = self.world.cache_dir + "apartment.urdf" apartment = Object("apartment", ObjectType.ENVIRONMENT, path, ObjectDescription) - cache_manager.generate_description_and_write_to_cache(path, extension, cache_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)) diff --git a/test/test_description.py b/test/test_description.py index 0d427717c..e1c62ccbf 100644 --- a/test/test_description.py +++ b/test/test_description.py @@ -19,7 +19,9 @@ def test_joint_child_link(self): self.assertEqual(self.robot.get_joint_parent_link("base_footprint_joint"), self.robot.root_link) def test_generate_description_from_mesh(self): - self.assertTrue(self.milk.description.generate_description_from_file("../resources/cached/milk.stl", ".stl")) + self.assertTrue(self.milk.description.generate_description_from_file("../resources/cached/milk.stl", + "milk", ".stl")) def test_generate_description_from_description_file(self): - self.assertTrue(self.milk.description.generate_description_from_file("../resources/cached/milk.urdf", ".urdf")) + self.assertTrue(self.milk.description.generate_description_from_file("../resources/cached/milk.urdf", + "milk", ".urdf")) From 953f31238d60efc1bff58dec43218243e3f936b1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 27 Feb 2024 17:10:27 +0100 Subject: [PATCH 64/69] [Abstract World] Defualted description type to URDF as it as it is currently the only used/implement description. --- demos/pycram_bullet_world_demo/demo.py | 12 ++++++------ doc/source/notebooks/bullet_world.ipynb | 6 +++--- src/pycram/bullet_world.py | 2 +- src/pycram/world_object.py | 3 ++- test/bullet_world_testcase.py | 12 ++++++------ test/test_bullet_world.py | 6 +++--- test/test_cache_manager.py | 2 +- test/test_object.py | 6 +++--- 8 files changed, 25 insertions(+), 24 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index f37015e4f..22aa7c54d 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -12,16 +12,16 @@ extension = ObjectDescription.get_file_extension() world = BulletWorld() -robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", ObjectDescription, pose=Pose([1, 2, 0])) -apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}", ObjectDescription) +robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", pose=Pose([1, 2, 0])) +apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") -milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([2.5, 2, 1.02]), +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]), color=Color(1, 0, 0, 1)) -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", ObjectDescription, +cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]), color=Color(0, 1, 0, 1)) -spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", ObjectDescription, pose=Pose([2.4, 2.2, 0.85]), +spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=Color(0, 0, 1, 1)) -bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", ObjectDescription, pose=Pose([2.5, 2.2, 1.02]), +bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=Color(1, 1, 0, 1)) apartment.attach(spoon, 'cabinet10_drawer_top') diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb index cc8874cec..18bdb9814 100644 --- a/doc/source/notebooks/bullet_world.ipynb +++ b/doc/source/notebooks/bullet_world.ipynb @@ -144,7 +144,7 @@ "from pycram.world_object import Object\n", "from pycram.urdf_interface import ObjectDescription\n", "\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", ObjectDescription, pose=Pose([0, 0, 1]))" + "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([0, 0, 1]))" ] }, { @@ -293,7 +293,7 @@ }, "outputs": [], "source": [ - "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", ObjectDescription, pose=Pose([1, 0, 1]))" + "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1, 0, 1]))" ] }, { @@ -396,7 +396,7 @@ } ], "source": [ - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\", ObjectDescription)\n", + "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", "print(pr2.links)" ] }, diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 67536bcf1..15d1cf329 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -63,7 +63,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self.set_gravity([0, 0, -9.8]) if not is_prospection_world: - _ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, ObjectDescription, + _ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, world=self) def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: diff --git a/src/pycram/world_object.py b/src/pycram/world_object.py index 8206b21e8..55d15890a 100644 --- a/src/pycram/world_object.py +++ b/src/pycram/world_object.py @@ -16,6 +16,7 @@ from .world_constraints import Attachment from .world_dataclasses import Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape from .description import ObjectDescription, LinkDescription +from .urdf_interface import ObjectDescription as URDF Link = ObjectDescription.Link @@ -32,7 +33,7 @@ class Object(WorldEntity): """ def __init__(self, name: str, obj_type: ObjectType, path: str, - description: Type[ObjectDescription], + description: Optional[Type[ObjectDescription]] = URDF, pose: Optional[Pose] = None, world: Optional[World] = None, color: Optional[Color] = Color(), diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index bc0d2cc72..525832ba6 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -18,10 +18,10 @@ class BulletWorldTestCase(unittest.TestCase): @classmethod def setUpClass(cls): cls.world = BulletWorld(mode=WorldMode.DIRECT) - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) cls.robot = Object(robot_description.name, ObjectType.ROBOT, - robot_description.name + cls.extension, ObjectDescription) - cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension, ObjectDescription) + robot_description.name + cls.extension) + cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False @@ -50,10 +50,10 @@ class BulletWorldGUITestCase(unittest.TestCase): @classmethod def setUpClass(cls): cls.world = BulletWorld(mode=WorldMode.GUI) - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) cls.robot = Object(robot_description.name, ObjectType.ROBOT, - robot_description.name + cls.extension, ObjectDescription) - cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension, ObjectDescription) + robot_description.name + cls.extension) + cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 2c1bf0682..059c27f8d 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -61,7 +61,7 @@ def test_remove_robot(self): self.world.remove_object(self.robot) self.assertTrue(robot_id not in [obj.id for obj in self.world.objects]) BulletWorldTest.robot = Object(robot_description.name, ObjectType.ROBOT, - robot_description.name + self.extension, ObjectDescription) + robot_description.name + self.extension) def test_get_joint_position(self): self.assertEqual(self.robot.get_joint_position("head_pan_joint"), 0.0) @@ -143,7 +143,7 @@ def test_add_resource_path(self): self.assertTrue("test" in self.world.data_directory) def test_no_prospection_object_found_for_given_object(self): - milk_2 = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + milk_2 = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) time.sleep(0.05) try: prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) @@ -155,7 +155,7 @@ def test_no_prospection_object_found_for_given_object(self): self.assertTrue(True) def test_no_object_found_for_given_prospection_object(self): - milk_2 = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([1.3, 1, 0.9])) + milk_2 = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) time.sleep(0.05) prospection_milk = self.world.get_prospection_object_for_object(milk_2) self.assertTrue(self.world.get_object_for_prospection_object(prospection_milk) == milk_2) diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index bbdf686d3..3df0e3484 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -13,7 +13,7 @@ def test_generate_description_and_write_to_cache(self): path = "../resources/apartment.urdf" extension = Path(path).suffix cache_path = self.world.cache_dir + "apartment.urdf" - apartment = Object("apartment", ObjectType.ENVIRONMENT, path, ObjectDescription) + 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)) diff --git a/test/test_object.py b/test/test_object.py index 80a8f98c0..9ef50883c 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -17,14 +17,14 @@ class TestObject(BulletWorldTestCase): def test_wrong_object_description_path(self): with self.assertRaises(FileNotFoundError): - milk = Object("milk2", ObjectType.MILK, "wrong_path.sk", ObjectDescription) + milk = Object("milk2", ObjectType.MILK, "wrong_path.sk") def test_malformed_object_description(self): malformed_file = "../resources/cached/malformed_description.urdf" with open(malformed_file, "w") as file: file.write("malformed") with self.assertRaises(Exception): - Object("milk2", ObjectType.MILK, malformed_file, ObjectDescription) + Object("milk2", ObjectType.MILK, malformed_file) def test_move_base_to_origin_pose(self): self.milk.set_position(Point(1, 2, 3), base=False) @@ -157,7 +157,7 @@ def test_set_color(self): self.assertEqual(color, Color(0, 1, 0, 1)) def test_object_equal(self): - milk2 = Object("milk2", ObjectType.MILK, "milk.stl", ObjectDescription) + milk2 = Object("milk2", ObjectType.MILK, "milk.stl") self.assertNotEqual(self.milk, milk2) self.assertEqual(self.milk, self.milk) self.assertNotEqual(self.milk, self.cereal) From b6a512e77eb89e90dec166fd0a909705d18e4ef7 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 27 Feb 2024 22:12:09 +0100 Subject: [PATCH 65/69] [AbstractWorld] Restructured files. --- demos/pycram_bullet_world_demo/demo.py | 12 +- demos/pycram_ur5_demo/demo.py | 5 +- resources/ur5e_without_gripper.urdf | 191 ++++++++++++++++++ src/neem_interface_python | 2 +- src/pycram/__init__.py | 2 +- src/pycram/description.py | 12 +- src/pycram/designator.py | 10 +- src/pycram/designators/action_designator.py | 8 +- src/pycram/designators/location_designator.py | 12 +- src/pycram/designators/motion_designator.py | 8 +- src/pycram/designators/object_designator.py | 6 +- src/pycram/external_interfaces/giskard.py | 14 +- src/pycram/external_interfaces/ik.py | 6 +- src/pycram/external_interfaces/robokudo.py | 9 +- src/pycram/helper.py | 4 +- src/pycram/language.py | 4 +- src/pycram/object_descriptors/__init__.py | 0 .../urdf.py} | 8 +- src/pycram/orm/action_designator.py | 2 +- src/pycram/orm/base.py | 2 +- src/pycram/orm/motion_designator.py | 4 +- src/pycram/orm/object_designator.py | 2 +- src/pycram/orm/task.py | 6 +- src/pycram/orm/utils.py | 6 - src/pycram/pose_generator_and_validator.py | 18 +- .../process_modules/boxy_process_modules.py | 7 +- .../default_process_modules.py | 4 +- .../process_modules/donbot_process_modules.py | 4 +- .../process_modules/hsr_process_modules.py | 4 +- .../process_modules/pr2_process_modules.py | 10 +- .../resolver/location/database_location.py | 8 +- .../resolver/location/giskard_location.py | 6 +- src/pycram/resolver/location/jpt_location.py | 8 +- src/pycram/robot_description.py | 2 +- src/pycram/ros/force_torque_sensor.py | 2 +- src/pycram/ros/joint_state_publisher.py | 2 +- src/pycram/ros/robot_state_updater.py | 4 +- src/pycram/ros/tf_broadcaster.py | 4 +- src/pycram/ros/viz_marker_publisher.py | 7 +- src/pycram/task.py | 6 +- src/pycram/world.py | 24 +-- src/pycram/world_reasoning.py | 10 +- src/pycram/worlds/__init__.py | 0 src/pycram/{ => worlds}/bullet_world.py | 14 +- src/pycram/worlds/concepts/__init__.py | 0 .../{ => worlds/concepts}/cache_manager.py | 3 +- .../concepts/constraints.py} | 6 +- src/pycram/{ => worlds/concepts}/costmaps.py | 14 +- src/pycram/{ => worlds/concepts}/event.py | 0 .../{ => worlds/concepts}/world_object.py | 31 +-- src/pycram/worlds/datastructures/__init__.py | 0 .../datastructures/dataclasses.py} | 10 +- .../{ => worlds/datastructures}/enums.py | 0 .../datastructures}/local_transformer.py | 2 +- .../{ => worlds/datastructures}/pose.py | 2 +- test/bullet_world_testcase.py | 10 +- test/test_action_designator.py | 9 +- test/test_bullet_world.py | 10 +- test/test_bullet_world_reasoning.py | 2 +- test/test_cache_manager.py | 5 +- test/test_costmaps.py | 4 +- test/test_database_merger.py | 6 +- test/test_database_resolver.py | 6 +- test/test_failure_handling.py | 6 +- test/test_jpt_resolver.py | 8 +- test/test_language.py | 6 +- test/test_links.py | 4 +- test/test_local_transformer.py | 4 +- test/test_location_designator.py | 6 +- test/test_object.py | 11 +- test/test_object_designator.py | 2 +- test/test_orm.py | 3 +- test/test_pose.py | 2 +- test/test_task_tree.py | 2 +- 74 files changed, 411 insertions(+), 242 deletions(-) create mode 100644 resources/ur5e_without_gripper.urdf create mode 100644 src/pycram/object_descriptors/__init__.py rename src/pycram/{urdf_interface.py => object_descriptors/urdf.py} (98%) create mode 100644 src/pycram/worlds/__init__.py rename src/pycram/{ => worlds}/bullet_world.py (98%) create mode 100644 src/pycram/worlds/concepts/__init__.py rename src/pycram/{ => worlds/concepts}/cache_manager.py (99%) rename src/pycram/{world_constraints.py => worlds/concepts/constraints.py} (98%) rename src/pycram/{ => worlds/concepts}/costmaps.py (98%) rename src/pycram/{ => worlds/concepts}/event.py (100%) rename src/pycram/{ => worlds/concepts}/world_object.py (97%) create mode 100644 src/pycram/worlds/datastructures/__init__.py rename src/pycram/{world_dataclasses.py => worlds/datastructures/dataclasses.py} (96%) rename src/pycram/{ => worlds/datastructures}/enums.py (100%) rename src/pycram/{ => worlds/datastructures}/local_transformer.py (98%) rename src/pycram/{ => worlds/datastructures}/pose.py (99%) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 22aa7c54d..188eb1baa 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -1,13 +1,13 @@ -from pycram.bullet_world import BulletWorld +from pycram.worlds.bullet_world import BulletWorld from pycram.designators.action_designator import * from pycram.designators.location_designator import * from pycram.designators.object_designator import * -from pycram.enums import ObjectType -from pycram.pose import Pose +from pycram.worlds.datastructures.enums import ObjectType +from pycram.worlds.datastructures.pose import Pose from pycram.process_module import simulated_robot, with_simulated_robot -from pycram.urdf_interface import ObjectDescription -from pycram.world_object import Object -from pycram.world_dataclasses import Color +from pycram.object_descriptors.urdf import ObjectDescription +from pycram.worlds.concepts.world_object import Object +from pycram.worlds.datastructures.dataclasses import Color extension = ObjectDescription.get_file_extension() diff --git a/demos/pycram_ur5_demo/demo.py b/demos/pycram_ur5_demo/demo.py index b54b5b6d7..b0cc4358e 100644 --- a/demos/pycram_ur5_demo/demo.py +++ b/demos/pycram_ur5_demo/demo.py @@ -4,10 +4,9 @@ from rospkg import RosPack import pybullet as pb -from pycram import robot_description -from pycram.bullet_world import BulletWorld +from pycram.worlds.bullet_world import BulletWorld from pycram.world import Object -from pycram.pose import Pose +from pycram.worlds.datastructures.pose import Pose from pycram.ros.force_torque_sensor import ForceTorqueSensor from pycram.ros.joint_state_publisher import JointStatePublisher from pycram.ros.tf_broadcaster import TFBroadcaster diff --git a/resources/ur5e_without_gripper.urdf b/resources/ur5e_without_gripper.urdf new file mode 100644 index 000000000..7ecbb9659 --- /dev/null +++ b/resources/ur5e_without_gripper.urdf @@ -0,0 +1,191 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/neem_interface_python b/src/neem_interface_python index 05ad42c41..285033958 160000 --- a/src/neem_interface_python +++ b/src/neem_interface_python @@ -1 +1 @@ -Subproject commit 05ad42c41042335dd2287d865f6a37c115ea8ffe +Subproject commit 28503395862f676fc2ad19877559673bd988fd07 diff --git a/src/pycram/__init__.py b/src/pycram/__init__.py index a081b74a1..878e37441 100644 --- a/src/pycram/__init__.py +++ b/src/pycram/__init__.py @@ -23,7 +23,7 @@ import logging import logging.config -# with utils.suppress_stdout_stderr(): +# with datastructures.suppress_stdout_stderr(): # import pycram.process_modules import pycram.process_modules diff --git a/src/pycram/description.py b/src/pycram/description.py index e0b9ecc1e..dc11387ea 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -8,14 +8,14 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING -from .enums import JointType -from .local_transformer import LocalTransformer -from .pose import Pose, Transform -from .world import WorldEntity -from .world_dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape +from pycram.worlds.datastructures.enums import JointType +from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.world import WorldEntity +from pycram.worlds.datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape if TYPE_CHECKING: - from .world_object import Object + from pycram.worlds.concepts.world_object import Object class EntityDescription(ABC): diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 4dc524b4e..6b1743d64 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -8,18 +8,18 @@ from sqlalchemy.orm.session import Session import rospy -from .world import World -from .world_object import Object as WorldObject +from pycram.world import World +from pycram.worlds.concepts.world_object import Object as WorldObject from .helper import GeneratorList, bcolors from threading import Lock from time import time from typing_extensions import List, Dict, Any, Optional, Union, get_type_hints, Callable, Iterable -from .local_transformer import LocalTransformer +from pycram.worlds.datastructures.local_transformer import LocalTransformer from .language import Language -from .pose import Pose +from pycram.worlds.datastructures.pose import Pose from .robot_descriptions import robot_description -from .enums import ObjectType +from pycram.worlds.datastructures.enums import ObjectType import logging diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index d701b2bb0..987a787a2 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -6,7 +6,7 @@ from .location_designator import CostmapLocation from .motion_designator import * from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart -from ..local_transformer import LocalTransformer +from pycram.worlds.datastructures.local_transformer import LocalTransformer from ..orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction, MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction, @@ -19,10 +19,10 @@ from ..plan_failures import ObjectUnfetchable, ReachabilityFailure from ..robot_descriptions import robot_description from ..task import with_tree -from ..enums import Arms +from pycram.worlds.datastructures.enums import Arms from ..designator import ActionDesignatorDescription -from ..world import World -from ..pose import Pose +from pycram.world import World +from pycram.worlds.datastructures.pose import Pose from ..helper import multiply_quaternions diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 2a087f07f..cc78cedc5 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -2,17 +2,17 @@ from typing_extensions import List, Union, Iterable, Optional, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart -from ..world import World, UseProspectionWorld -from ..world_reasoning import link_pose_for_joint_config +from pycram.world import World, UseProspectionWorld +from pycram.world_reasoning import link_pose_for_joint_config from ..designator import DesignatorError, LocationDesignatorDescription -from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap +from pycram.worlds.concepts.costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap from ..robot_descriptions import robot_description -from ..enums import JointType +from pycram.worlds.datastructures.enums import JointType from ..helper import transform -from ..pose_generator_and_validator import pose_generator, visibility_validator, reachability_validator, \ +from pycram.pose_generator_and_validator import pose_generator, visibility_validator, reachability_validator, \ generate_orientation from ..robot_description import ManipulatorDescription -from ..pose import Pose +from pycram.worlds.datastructures.pose import Pose class Location(LocationDesignatorDescription): diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 88604d71f..f24cb0f05 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -2,8 +2,8 @@ from sqlalchemy.orm import Session from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject -from ..world import World -from ..world_object import Object +from pycram.world import World +from pycram.worlds.concepts.world_object import Object from ..designator import DesignatorError from ..plan_failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager @@ -13,10 +13,10 @@ MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion, MoveGripperMotion as ORMMoveGripperMotion, DetectingMotion as ORMDetectingMotion, OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion) -from ..enums import ObjectType +from pycram.worlds.datastructures.enums import ObjectType from typing_extensions import List, Dict, Callable, Optional -from ..pose import Pose +from pycram.worlds.datastructures.pose import Pose from ..task import with_tree diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index bd24a86a3..6826eef00 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,12 +1,12 @@ import dataclasses from typing_extensions import List, Optional, Callable import sqlalchemy.orm -from ..world import World -from ..world_object import Object as WorldObject +from pycram.world import World +from pycram.worlds.concepts.world_object import Object as WorldObject from ..designator import ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) -from ..pose import Pose +from pycram.worlds.datastructures.pose import Pose from ..external_interfaces.robokudo import query diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 1d60c5a09..ce1afabec 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -1,22 +1,18 @@ import json import threading -import rosnode import rospy import sys import rosnode -import urdf_parser_py - -import traceback -from ..pose import Pose +from pycram.worlds.datastructures.pose import Pose from ..robot_descriptions import robot_description -from ..world import World -from ..world_dataclasses import MeshVisualShape -from ..world_object import Object +from pycram.world import World +from pycram.worlds.datastructures.dataclasses import MeshVisualShape +from pycram.worlds.concepts.world_object import Object from ..robot_description import ManipulatorDescription -from typing_extensions import List, Tuple, Dict, Callable, Optional +from typing_extensions import List, Dict, Callable, Optional from geometry_msgs.msg import PoseStamped, PointStamped, QuaternionStamped, Vector3Stamped from threading import Lock, RLock diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 2f1b2c184..c70a6469c 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -6,10 +6,10 @@ from moveit_msgs.srv import GetPositionIK from sensor_msgs.msg import JointState -from ..world_object import Object +from pycram.worlds.concepts.world_object import Object from ..helper import calculate_wrist_tool_offset, _apply_ik -from ..local_transformer import LocalTransformer -from ..pose import Pose +from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.worlds.datastructures.pose import Pose from ..robot_descriptions import robot_description from ..plan_failures import IKError diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index 0497c04f1..531c0eb6f 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -1,16 +1,15 @@ import sys from typing_extensions import Callable -import rosnode import rospy import actionlib import rosnode from ..designator import ObjectDesignatorDescription -from ..pose import Pose -from ..local_transformer import LocalTransformer -from ..world import World -from ..enums import ObjectType +from pycram.worlds.datastructures.pose import Pose +from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.world import World +from pycram.worlds.datastructures.enums import ObjectType try: from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjetDesignator diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 3b07ce6e3..0da52632a 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -13,8 +13,8 @@ from pytransform3d.rotations import quaternion_wxyz_from_xyzw, quaternion_xyzw_from_wxyz from pytransform3d.transformations import transform_from_pq, transform_from, pq_from_transform -from .world_object import Object as WorldObject -from .pose import Transform, Pose +from pycram.worlds.concepts.world_object import Object as WorldObject +from pycram.worlds.datastructures.pose import Transform, Pose import math diff --git a/src/pycram/language.py b/src/pycram/language.py index 80948cc8d..310f8ce1b 100644 --- a/src/pycram/language.py +++ b/src/pycram/language.py @@ -3,9 +3,9 @@ import time from typing_extensions import Iterable, Optional, Callable, Dict, Any, List, Union -from anytree import NodeMixin, Node, PreOrderIter, RenderTree +from anytree import NodeMixin, Node, PreOrderIter -from .enums import State +from pycram.worlds.datastructures.enums import State import threading from .fluent import Fluent diff --git a/src/pycram/object_descriptors/__init__.py b/src/pycram/object_descriptors/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/urdf_interface.py b/src/pycram/object_descriptors/urdf.py similarity index 98% rename from src/pycram/urdf_interface.py rename to src/pycram/object_descriptors/urdf.py index 4dc1abe11..b2905e170 100644 --- a/src/pycram/urdf_interface.py +++ b/src/pycram/object_descriptors/urdf.py @@ -10,11 +10,11 @@ from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, Sphere as URDF_Sphere, Mesh as URDF_Mesh) -from .enums import JointType, Shape -from .pose import Pose -from .description import JointDescription as AbstractJointDescription, \ +from pycram.worlds.datastructures.enums import JointType, Shape +from pycram.worlds.datastructures.pose import Pose +from pycram.description import JointDescription as AbstractJointDescription, \ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription -from .world_dataclasses import Color +from pycram.worlds.datastructures.dataclasses import Color class LinkDescription(AbstractLinkDescription): diff --git a/src/pycram/orm/action_designator.py b/src/pycram/orm/action_designator.py index d27642cf7..7bbaa0525 100644 --- a/src/pycram/orm/action_designator.py +++ b/src/pycram/orm/action_designator.py @@ -2,7 +2,7 @@ from .base import RobotState, Designator, MapperArgsMixin, PoseMixin from .object_designator import ObjectMixin -from ..enums import Arms +from pycram.worlds.datastructures.enums import Arms from sqlalchemy.orm import Mapped, mapped_column, relationship from sqlalchemy import ForeignKey diff --git a/src/pycram/orm/base.py b/src/pycram/orm/base.py index 407270a17..61a6ab39a 100644 --- a/src/pycram/orm/base.py +++ b/src/pycram/orm/base.py @@ -10,7 +10,7 @@ from sqlalchemy.orm import DeclarativeBase, Mapped, MappedAsDataclass, mapped_column, Session, relationship, \ declared_attr -from ..enums import ObjectType +from pycram.worlds.datastructures.enums import ObjectType def get_pycram_version_from_git() -> Optional[str]: diff --git a/src/pycram/orm/motion_designator.py b/src/pycram/orm/motion_designator.py index 44624d33d..af67aeb9e 100644 --- a/src/pycram/orm/motion_designator.py +++ b/src/pycram/orm/motion_designator.py @@ -8,11 +8,11 @@ from typing_extensions import Optional from .base import MapperArgsMixin, Designator, PoseMixin -from .object_designator import Object, ObjectMixin +from .object_designator import Object from sqlalchemy.orm import Mapped, mapped_column, relationship from sqlalchemy import ForeignKey -from ..enums import ObjectType +from pycram.worlds.datastructures.enums import ObjectType class Motion(MapperArgsMixin, Designator): diff --git a/src/pycram/orm/object_designator.py b/src/pycram/orm/object_designator.py index fad1d3c6e..776ab59ce 100644 --- a/src/pycram/orm/object_designator.py +++ b/src/pycram/orm/object_designator.py @@ -2,7 +2,7 @@ from pycram.orm.base import Base, MapperArgsMixin, PoseMixin from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship from sqlalchemy import ForeignKey -from ..enums import ObjectType +from pycram.worlds.datastructures.enums import ObjectType class ObjectMixin: """ diff --git a/src/pycram/orm/task.py b/src/pycram/orm/task.py index 333bd5d8e..d821a2a5d 100644 --- a/src/pycram/orm/task.py +++ b/src/pycram/orm/task.py @@ -1,11 +1,9 @@ """Implementation of ORM classes associated with pycram.task.""" from typing_extensions import Optional from sqlalchemy import ForeignKey -from sqlalchemy.orm import MappedAsDataclass, Mapped, mapped_column, relationship -from .action_designator import Action +from sqlalchemy.orm import Mapped, mapped_column, relationship from .base import Base, Designator -from .motion_designator import Motion -from ..enums import TaskStatus +from pycram.worlds.datastructures.enums import TaskStatus import datetime diff --git a/src/pycram/orm/utils.py b/src/pycram/orm/utils.py index 5efe3a2e6..624773a88 100644 --- a/src/pycram/orm/utils.py +++ b/src/pycram/orm/utils.py @@ -1,17 +1,11 @@ import traceback -from anytree import Node, RenderTree, LevelOrderIter import rospy import sqlalchemy import pycram.orm.base -from pycram.designators.action_designator import * from pycram.designators.object_designator import * import json from pycram.designators.action_designator import * -from pycram.designators.location_designator import * -from pycram.process_module import simulated_robot -from pycram.enums import Arms, ObjectType -from pycram.task import with_tree import pycram.orm diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 994ff234d..96f1bf26f 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -1,15 +1,15 @@ import tf import numpy as np -from .world import World -from .world_object import Object -from .world_reasoning import contact -from .costmaps import Costmap -from .pose import Pose, Transform -from .robot_descriptions import robot_description -from .external_interfaces.ik import request_ik -from .plan_failures import IKError -from .helper import _apply_ik +from pycram.world import World +from pycram.worlds.concepts.world_object import Object +from pycram.world_reasoning import contact +from pycram.worlds.concepts.costmaps import Costmap +from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.robot_descriptions import robot_description +from pycram.external_interfaces.ik import request_ik +from pycram.plan_failures import IKError +from pycram.helper import _apply_ik from typing_extensions import Tuple, List, Union, Dict, Iterable diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 3b4fa4698..f0aecef08 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -2,14 +2,13 @@ import numpy as np -from ..world import World +from pycram.world import World from ..designators.motion_designator import PlaceMotion -from ..local_transformer import LocalTransformer +from pycram.worlds.datastructures.local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description from ..process_modules.pr2_process_modules import (_park_arms, Pr2Navigation as BoxyNavigation, - Pr2PickUp as BoxyPickUp, Pr2Open as BoxyOpen, - Pr2Close as BoxyClose, Pr2Detecting as BoxyDetecting, + Pr2PickUp as BoxyPickUp, Pr2Detecting as BoxyDetecting, Pr2MoveTCP as BoxyMoveTCP, Pr2MoveArmJoints as BoxyMoveArmJoints, Pr2WorldStateDetecting as BoxyWorldStateDetecting, _move_arm_tcp) diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 19faebb29..8993b10d2 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -8,8 +8,8 @@ Pr2Close as DefaultClose, Pr2MoveGripper as DefaultMoveGripper, Pr2Detecting as DefaultDetecting, \ Pr2MoveTCP as DefaultMoveTCP from ..robot_descriptions import robot_description -from ..world import World -from ..local_transformer import LocalTransformer +from pycram.world import World +from pycram.worlds.datastructures.local_transformer import LocalTransformer from ..designators.motion_designator import LookingMotion, MoveArmJointsMotion, MoveJointsMotion diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 5833d0513..41a8704b9 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -3,9 +3,9 @@ import numpy as np from typing_extensions import Optional -from ..bullet_world import World +from pycram.worlds.bullet_world import World from ..designators.motion_designator import MoveArmJointsMotion, WorldStateDetectingMotion -from ..local_transformer import LocalTransformer +from pycram.worlds.datastructures.local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description from ..process_modules.pr2_process_modules import Pr2PickUp, Pr2Detecting as DonbotDetecting, _move_arm_tcp diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 0b4e11512..75cb82e72 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -3,8 +3,8 @@ from typing_extensions import Optional from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..world import World -from ..pose import Pose, Point +from pycram.world import World +from pycram.worlds.datastructures.pose import Pose, Point from ..helper import _apply_ik from ..external_interfaces.ik import request_ik from .. import world_reasoning as btr diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 34e367404..204b3d1b8 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -12,16 +12,16 @@ from ..process_module import ProcessModule, ProcessModuleManager from ..external_interfaces.ik import request_ik from ..helper import _apply_ik -from ..local_transformer import LocalTransformer +from pycram.worlds.datastructures.local_transformer import LocalTransformer from ..designators.object_designator import ObjectDesignatorDescription from ..designators.motion_designator import MoveMotion, PickUpMotion, PlaceMotion, LookingMotion, \ DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ MoveGripperMotion, OpeningMotion, ClosingMotion, MotionDesignatorDescription from ..robot_descriptions import robot_description -from ..world import World -from ..world_object import Object -from ..pose import Pose -from ..enums import JointType, ObjectType +from pycram.world import World +from pycram.worlds.concepts.world_object import Object +from pycram.worlds.datastructures.pose import Pose +from pycram.worlds.datastructures.enums import JointType, ObjectType from ..external_interfaces import giskard from ..external_interfaces.robokudo import query diff --git a/src/pycram/resolver/location/database_location.py b/src/pycram/resolver/location/database_location.py index f8bb9ef96..4c205bbbb 100644 --- a/src/pycram/resolver/location/database_location.py +++ b/src/pycram/resolver/location/database_location.py @@ -4,14 +4,14 @@ from tf import transformations import pycram.designators.location_designator import pycram.task -from pycram.costmaps import OccupancyCostmap -from pycram.orm.action_designator import PickUpAction, Action +from pycram.worlds.concepts.costmaps import OccupancyCostmap +from pycram.orm.action_designator import PickUpAction from pycram.orm.object_designator import Object -from pycram.orm.base import Position, RobotState, Designator, Base +from pycram.orm.base import Position, RobotState from pycram.orm.task import TaskTreeNode, Code from .jpt_location import JPTCostmapLocation from ... import orm -from ...pose import Pose +from pycram.worlds.datastructures.pose import Pose class DatabaseCostmapLocation(pycram.designators.location_designator.CostmapLocation): diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index add24a41c..506e3503c 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -1,9 +1,9 @@ from ...external_interfaces.giskard import achieve_cartesian_goal from ...designators.location_designator import CostmapLocation -from ...world import UseProspectionWorld, World -from ...pose import Pose +from pycram.world import UseProspectionWorld, World +from pycram.worlds.datastructures.pose import Pose from ...robot_descriptions import robot_description -from ...pose_generator_and_validator import reachability_validator +from pycram.pose_generator_and_validator import reachability_validator from typing_extensions import Tuple, Dict import tf diff --git a/src/pycram/resolver/location/jpt_location.py b/src/pycram/resolver/location/jpt_location.py index 5161ee014..73aa3a6f7 100644 --- a/src/pycram/resolver/location/jpt_location.py +++ b/src/pycram/resolver/location/jpt_location.py @@ -6,10 +6,10 @@ import tf from ...designators import location_designator -from ...costmaps import OccupancyCostmap -from ...pose import Pose -from ...world import Object, World -from ...world_dataclasses import BoxVisualShape, Color +from pycram.worlds.concepts.costmaps import OccupancyCostmap +from pycram.worlds.datastructures.pose import Pose +from pycram.world import Object, World +from pycram.worlds.datastructures.dataclasses import BoxVisualShape, Color class JPTCostmapLocation(location_designator.CostmapLocation): diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 795330b90..462777364 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -139,7 +139,7 @@ class ManipulatorDescription(InteractionDescription): This class allows with the given interaction description to include a gripper description which is placed between the last link of the interaction description and the rest of it. - Independently from that a tool frame can be saved, which allows to use objects + Independently of that a tool frame can be saved, which allows to use objects to manipulate the environment:: |--> (tool_frame) diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros/force_torque_sensor.py index b2d07b2c6..aee3b8f46 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros/force_torque_sensor.py @@ -6,7 +6,7 @@ from geometry_msgs.msg import WrenchStamped from std_msgs.msg import Header -from ..world import World +from pycram.world import World class ForceTorqueSensor: diff --git a/src/pycram/ros/joint_state_publisher.py b/src/pycram/ros/joint_state_publisher.py index 75e4c084c..45f89bf54 100644 --- a/src/pycram/ros/joint_state_publisher.py +++ b/src/pycram/ros/joint_state_publisher.py @@ -6,7 +6,7 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header -from ..world import World +from pycram.world import World class JointStatePublisher: diff --git a/src/pycram/ros/robot_state_updater.py b/src/pycram/ros/robot_state_updater.py index 93ab7ecce..e1400fb5b 100644 --- a/src/pycram/ros/robot_state_updater.py +++ b/src/pycram/ros/robot_state_updater.py @@ -5,9 +5,9 @@ from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState -from ..world import World +from pycram.world import World from ..robot_descriptions import robot_description -from ..pose import Pose +from pycram.worlds.datastructures.pose import Pose class RobotStateUpdater: diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index a727e75bb..c698f9b8b 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -3,8 +3,8 @@ import threading import atexit -from ..pose import Pose -from ..world import World +from pycram.worlds.datastructures.pose import Pose +from pycram.world import World from tf2_msgs.msg import TFMessage diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 8213b1b73..317af4724 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -5,13 +5,12 @@ from geometry_msgs.msg import Vector3 from std_msgs.msg import ColorRGBA -from ..world import World +from pycram.world import World from visualization_msgs.msg import MarkerArray, Marker import rospy -import urdf_parser_py -from pycram.pose import Transform -from ..world_dataclasses import MeshVisualShape, CylinderVisualShape, BoxVisualShape, SphereVisualShape +from pycram.worlds.datastructures.pose import Transform +from pycram.worlds.datastructures.dataclasses import MeshVisualShape, CylinderVisualShape, BoxVisualShape, SphereVisualShape class VizMarkerPublisher: diff --git a/src/pycram/task.py b/src/pycram/task.py index b8baacbe0..de870493f 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -13,13 +13,13 @@ import sqlalchemy.orm.session import tqdm -from .world import World +from pycram.world import World from .orm.task import (Code as ORMCode, TaskTreeNode as ORMTaskTreeNode) from .orm.base import ProcessMetaData from .plan_failures import PlanFailure from .language import Code -from .enums import TaskStatus -from .world_dataclasses import Color +from pycram.worlds.datastructures.enums import TaskStatus +from pycram.worlds.datastructures.dataclasses import Color class TaskCode(Code): diff --git a/src/pycram/world.py b/src/pycram/world.py index 722325501..3df891777 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -14,20 +14,20 @@ from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING from typing_extensions import Union -from .cache_manager import CacheManager -from .enums import JointType, ObjectType, WorldMode -from .event import Event -from .local_transformer import LocalTransformer -from .pose import Pose, Transform -from .world_constraints import Constraint -from .world_dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, - MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, - CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState) +from pycram.worlds.concepts.cache_manager import CacheManager +from pycram.worlds.datastructures.enums import JointType, ObjectType, WorldMode +from pycram.worlds.concepts.event import Event +from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.worlds.concepts.constraints import Constraint +from pycram.worlds.datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, + MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, + ObjectState, State, WorldState) if TYPE_CHECKING: - from .world_object import Object - from .description import Link, Joint + from pycram.worlds.concepts.world_object import Object + from pycram.description import Link, Joint class StateEntity: diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 2e9103084..27111daa7 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -3,11 +3,11 @@ import numpy as np -from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp -from .pose import Pose, Transform -from .robot_descriptions import robot_description -from .world_object import Object -from .world import World, UseProspectionWorld +from pycram.external_interfaces.ik import try_to_reach, try_to_reach_with_grasp +from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.robot_descriptions import robot_description +from pycram.worlds.concepts.world_object import Object +from pycram.world import World, UseProspectionWorld def stable(obj: Object) -> bool: diff --git a/src/pycram/worlds/__init__.py b/src/pycram/worlds/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/bullet_world.py b/src/pycram/worlds/bullet_world.py similarity index 98% rename from src/pycram/bullet_world.py rename to src/pycram/worlds/bullet_world.py index 15d1cf329..addb57190 100755 --- a/src/pycram/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -11,13 +11,13 @@ from geometry_msgs.msg import Point from typing_extensions import List, Optional, Dict -from .enums import ObjectType, WorldMode, JointType -from .pose import Pose -from .urdf_interface import ObjectDescription -from .world import World -from .world_constraints import Constraint -from .world_dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape -from .world_object import Object +from pycram.worlds.datastructures.enums import ObjectType, WorldMode, JointType +from pycram.worlds.datastructures.pose import Pose +from pycram.object_descriptors.urdf import ObjectDescription +from pycram.world import World +from pycram.worlds.concepts.constraints import Constraint +from pycram.worlds.datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape +from pycram.worlds.concepts.world_object import Object Link = ObjectDescription.Link RootLink = ObjectDescription.RootLink diff --git a/src/pycram/worlds/concepts/__init__.py b/src/pycram/worlds/concepts/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/cache_manager.py b/src/pycram/worlds/concepts/cache_manager.py similarity index 99% rename from src/pycram/cache_manager.py rename to src/pycram/worlds/concepts/cache_manager.py index 2dc479d8d..a8995636b 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/worlds/concepts/cache_manager.py @@ -1,11 +1,10 @@ import os import pathlib -import re from typing_extensions import List, TYPE_CHECKING if TYPE_CHECKING: - from .description import ObjectDescription + from pycram.description import ObjectDescription class CacheManager: diff --git a/src/pycram/world_constraints.py b/src/pycram/worlds/concepts/constraints.py similarity index 98% rename from src/pycram/world_constraints.py rename to src/pycram/worlds/concepts/constraints.py index 6dc75e33b..c36b733a2 100644 --- a/src/pycram/world_constraints.py +++ b/src/pycram/worlds/concepts/constraints.py @@ -3,11 +3,11 @@ from geometry_msgs.msg import Point from typing_extensions import Union, List, Optional, TYPE_CHECKING -from .enums import JointType -from .pose import Transform, Pose +from pycram.worlds.datastructures.enums import JointType +from pycram.worlds.datastructures.pose import Transform, Pose if TYPE_CHECKING: - from .description import Link + from pycram.description import Link class AbstractConstraint: diff --git a/src/pycram/costmaps.py b/src/pycram/worlds/concepts/costmaps.py similarity index 98% rename from src/pycram/costmaps.py rename to src/pycram/worlds/concepts/costmaps.py index 01b9820db..8411e5fa0 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/worlds/concepts/costmaps.py @@ -10,13 +10,13 @@ from matplotlib import colors from nav_msgs.msg import OccupancyGrid, MapMetaData -from .world import UseProspectionWorld -from .world_object import Object -from .description import Link -from .local_transformer import LocalTransformer -from .pose import Pose, Transform -from .world import World -from .world_dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color +from pycram.world import UseProspectionWorld +from pycram.worlds.concepts.world_object import Object +from pycram.description import Link +from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.world import World +from pycram.worlds.datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color class Costmap: diff --git a/src/pycram/event.py b/src/pycram/worlds/concepts/event.py similarity index 100% rename from src/pycram/event.py rename to src/pycram/worlds/concepts/event.py diff --git a/src/pycram/world_object.py b/src/pycram/worlds/concepts/world_object.py similarity index 97% rename from src/pycram/world_object.py rename to src/pycram/worlds/concepts/world_object.py index 55d15890a..71b05579c 100644 --- a/src/pycram/world_object.py +++ b/src/pycram/worlds/concepts/world_object.py @@ -8,15 +8,16 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from .enums import ObjectType, JointType -from .local_transformer import LocalTransformer -from .pose import Pose, Transform -from .robot_descriptions import robot_description -from .world import WorldEntity, World -from .world_constraints import Attachment -from .world_dataclasses import Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape -from .description import ObjectDescription, LinkDescription -from .urdf_interface import ObjectDescription as URDF +from pycram.description import ObjectDescription, LinkDescription +from pycram.object_descriptors.urdf import ObjectDescription as URDFObject +from pycram.robot_descriptions import robot_description +from pycram.world import WorldEntity, World +from pycram.worlds.concepts.constraints import Attachment +from pycram.worlds.datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, + AxisAlignedBoundingBox, VisualShape) +from pycram.worlds.datastructures.enums import ObjectType, JointType +from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.worlds.datastructures.pose import Pose, Transform Link = ObjectDescription.Link @@ -33,7 +34,7 @@ class Object(WorldEntity): """ def __init__(self, name: str, obj_type: ObjectType, path: str, - description: Optional[Type[ObjectDescription]] = URDF, + description: Optional[Type[ObjectDescription]] = URDFObject, pose: Optional[Pose] = None, world: Optional[World] = None, color: Optional[Color] = Color(), @@ -119,9 +120,15 @@ def _load_object_and_get_id(self, path: Optional[str] = None, raise e try: - obj_id = self.world.load_object_and_get_id(path, Pose(self.get_position_as_list(), - self.get_orientation_as_list())) + simulator_object_path = path + if simulator_object_path is None: + # This is useful when the object is already loaded in the simulator so it would use its name instead of + # its path + simulator_object_path = self.name + obj_id = self.world.load_object_and_get_id(simulator_object_path, Pose(self.get_position_as_list(), + self.get_orientation_as_list())) return obj_id, path + except Exception as e: logging.error( "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or" diff --git a/src/pycram/worlds/datastructures/__init__.py b/src/pycram/worlds/datastructures/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/world_dataclasses.py b/src/pycram/worlds/datastructures/dataclasses.py similarity index 96% rename from src/pycram/world_dataclasses.py rename to src/pycram/worlds/datastructures/dataclasses.py index 7aea14338..c41a41b50 100644 --- a/src/pycram/world_dataclasses.py +++ b/src/pycram/worlds/datastructures/dataclasses.py @@ -2,14 +2,14 @@ from dataclasses import dataclass from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING -from .enums import JointType, Shape -from .pose import Pose, Point +from pycram.worlds.datastructures.enums import JointType, Shape +from pycram.worlds.datastructures.pose import Pose, Point from abc import ABC, abstractmethod if TYPE_CHECKING: - from .description import Link - from .world_object import Object - from .world_constraints import Attachment + from pycram.description import Link + from pycram.worlds.concepts.world_object import Object + from pycram.worlds.concepts.constraints import Attachment def get_point_as_list(point: Point) -> List[float]: diff --git a/src/pycram/enums.py b/src/pycram/worlds/datastructures/enums.py similarity index 100% rename from src/pycram/enums.py rename to src/pycram/worlds/datastructures/enums.py diff --git a/src/pycram/local_transformer.py b/src/pycram/worlds/datastructures/local_transformer.py similarity index 98% rename from src/pycram/local_transformer.py rename to src/pycram/worlds/datastructures/local_transformer.py index 3ea74dea2..635a6fa38 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/worlds/datastructures/local_transformer.py @@ -9,7 +9,7 @@ from rospy import Duration from geometry_msgs.msg import TransformStamped -from .pose import Pose, Transform +from pycram.worlds.datastructures.pose import Pose, Transform from typing_extensions import List, Optional, Union, Iterable diff --git a/src/pycram/pose.py b/src/pycram/worlds/datastructures/pose.py similarity index 99% rename from src/pycram/pose.py rename to src/pycram/worlds/datastructures/pose.py index 74bda8b8d..79eadaeef 100644 --- a/src/pycram/pose.py +++ b/src/pycram/worlds/datastructures/pose.py @@ -11,7 +11,7 @@ from geometry_msgs.msg import PoseStamped, TransformStamped, Vector3, Point from geometry_msgs.msg import (Pose as GeoPose, Quaternion as GeoQuaternion) from tf import transformations -from .orm.base import Pose as ORMPose, Position, Quaternion, ProcessMetaData +from pycram.orm.base import Pose as ORMPose, Position, Quaternion, ProcessMetaData def get_normalized_quaternion(quaternion: np.ndarray) -> GeoQuaternion: diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 525832ba6..46144af36 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -1,13 +1,13 @@ import time import unittest -from pycram.bullet_world import BulletWorld -from pycram.world_object import Object -from pycram.pose import Pose +from pycram.worlds.bullet_world import BulletWorld +from pycram.worlds.concepts.world_object import Object +from pycram.worlds.datastructures.pose import Pose from pycram.robot_descriptions import robot_description from pycram.process_module import ProcessModule -from pycram.enums import ObjectType, WorldMode -from pycram.urdf_interface import ObjectDescription +from pycram.worlds.datastructures.enums import ObjectType, WorldMode +from pycram.object_descriptors.urdf import ObjectDescription class BulletWorldTestCase(unittest.TestCase): diff --git a/test/test_action_designator.py b/test/test_action_designator.py index d972aa0c0..656df74b8 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -2,9 +2,8 @@ from pycram.designators import action_designator, object_designator from pycram.robot_descriptions import robot_description from pycram.process_module import simulated_robot -from pycram.pose import Pose -from pycram.enums import ObjectType -import pycram.enums +from pycram.worlds.datastructures.pose import Pose +from pycram.worlds.datastructures.enums import ObjectType, Arms from bullet_world_testcase import BulletWorldTestCase import numpy as np @@ -42,8 +41,8 @@ def test_grip(self): self.assertEqual(description.ground().object_designator.name, "milk") def test_park_arms(self): - description = action_designator.ParkArmsAction([pycram.enums.Arms.BOTH]) - self.assertEqual(description.ground().arm, pycram.enums.Arms.BOTH) + description = action_designator.ParkArmsAction([Arms.BOTH]) + self.assertEqual(description.ground().arm, Arms.BOTH) with simulated_robot: description.resolve().perform() for joint, pose in robot_description.get_static_joint_chain("right", "park").items(): diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 059c27f8d..058adb0c4 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -5,12 +5,12 @@ import rospkg from bullet_world_testcase import BulletWorldTestCase, BulletWorldGUITestCase -from pycram.enums import ObjectType, WorldMode -from pycram.pose import Pose +from pycram.worlds.datastructures.enums import ObjectType, WorldMode +from pycram.worlds.datastructures.pose import Pose from pycram.robot_descriptions import robot_description -from pycram.urdf_interface import ObjectDescription -from pycram.world_dataclasses import Color -from pycram.world_object import Object +from pycram.object_descriptors.urdf import ObjectDescription +from pycram.worlds.datastructures.dataclasses import Color +from pycram.worlds.concepts.world_object import Object fix_missing_inertial = ObjectDescription.fix_missing_inertial diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index db81af34f..4438d37ce 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -2,7 +2,7 @@ import pycram.world_reasoning as btr from bullet_world_testcase import BulletWorldTestCase -from pycram.pose import Pose +from pycram.worlds.datastructures.pose import Pose from pycram.robot_descriptions import robot_description use_new = True diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index 3df0e3484..ef126f2eb 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -1,9 +1,8 @@ from pathlib import Path from bullet_world_testcase import BulletWorldTestCase -from pycram.enums import ObjectType -from pycram.urdf_interface import ObjectDescription -from pycram.world_object import Object +from pycram.worlds.datastructures.enums import ObjectType +from pycram.worlds.concepts.world_object import Object class TestCacheManager(BulletWorldTestCase): diff --git a/test/test_costmaps.py b/test/test_costmaps.py index ba8c855ff..1bffb32c2 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -1,8 +1,8 @@ import numpy as np from bullet_world_testcase import BulletWorldTestCase -from pycram.costmaps import OccupancyCostmap -from pycram.pose import Pose +from pycram.worlds.concepts.costmaps import OccupancyCostmap +from pycram.worlds.datastructures.pose import Pose class TestCostmapsCase(BulletWorldTestCase): diff --git a/test/test_database_merger.py b/test/test_database_merger.py index 6ed3faaf2..fbe396431 100644 --- a/test/test_database_merger.py +++ b/test/test_database_merger.py @@ -8,11 +8,11 @@ from pycram.designators.action_designator import * from pycram.designators.location_designator import * from pycram.process_module import simulated_robot -from pycram.enums import Arms, ObjectType, WorldMode +from pycram.worlds.datastructures.enums import Arms, ObjectType, WorldMode from pycram.task import with_tree import pycram.task -from pycram.bullet_world import BulletWorld -from pycram.world_object import Object +from pycram.worlds.bullet_world import BulletWorld +from pycram.worlds.concepts.world_object import Object from pycram.designators.object_designator import * from dataclasses import dataclass, field diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 31f4cc28d..2809396d2 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -3,17 +3,17 @@ import sqlalchemy import sqlalchemy.orm import pycram.plan_failures -from pycram.world_object import Object +from pycram.worlds.concepts.world_object import Object from pycram import task from pycram.world import World from pycram.designators import action_designator, object_designator from pycram.orm.base import Base from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot -from pycram.pose import Pose +from pycram.worlds.datastructures.pose import Pose from pycram.robot_descriptions import robot_description from pycram.task import with_tree -from pycram.enums import ObjectType, WorldMode +from pycram.worlds.datastructures.enums import ObjectType, WorldMode # check if jpt is installed jpt_installed = True diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index cc9e7fd5e..bd7f36258 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -2,15 +2,15 @@ import roslaunch -from pycram.bullet_world import BulletWorld, Object +from pycram.worlds.bullet_world import BulletWorld, Object from pycram.designator import ActionDesignatorDescription from pycram.designators.action_designator import ParkArmsAction -from pycram.enums import ObjectType, Arms, WorldMode +from pycram.worlds.datastructures.enums import ObjectType, Arms, WorldMode from pycram.failure_handling import Retry from pycram.plan_failures import PlanFailure from pycram.process_module import ProcessModule, simulated_robot from pycram.robot_descriptions import robot_description -from pycram.urdf_interface import ObjectDescription +from pycram.object_descriptors.urdf import ObjectDescription extension = ObjectDescription.get_file_extension() diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index b03319785..534d6e4e3 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -5,14 +5,14 @@ import requests import pycram.plan_failures -from pycram.bullet_world import BulletWorld -from pycram.world_object import Object +from pycram.worlds.bullet_world import BulletWorld +from pycram.worlds.concepts.world_object import Object from pycram.designators import action_designator, object_designator from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot from pycram.robot_descriptions import robot_description -from pycram.pose import Pose -from pycram.enums import WorldMode +from pycram.worlds.datastructures.pose import Pose +from pycram.worlds.datastructures.enums import WorldMode # check if jpt is installed jpt_installed = True diff --git a/test/test_language.py b/test/test_language.py index 94cff57a3..458fbe0ac 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -2,11 +2,11 @@ import time import unittest from pycram.designators.action_designator import * -from pycram.enums import ObjectType, State +from pycram.worlds.datastructures.enums import ObjectType, State from pycram.fluent import Fluent from pycram.plan_failures import PlanFailure -from pycram.pose import Pose -from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Repeat, Code, RenderTree +from pycram.worlds.datastructures.pose import Pose +from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Code from pycram.process_module import simulated_robot from bullet_world_testcase import BulletWorldTestCase diff --git a/test/test_links.py b/test/test_links.py index af26bb39e..f737a55ce 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -1,7 +1,5 @@ -import time - from bullet_world_testcase import BulletWorldTestCase -from pycram.world_dataclasses import Color +from pycram.worlds.datastructures.dataclasses import Color class TestLinks(BulletWorldTestCase): diff --git a/test/test_local_transformer.py b/test/test_local_transformer.py index 4cad086e7..afaac7a71 100644 --- a/test/test_local_transformer.py +++ b/test/test_local_transformer.py @@ -1,8 +1,8 @@ import rospy -from pycram.local_transformer import LocalTransformer -from pycram.pose import Pose, Transform +from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.worlds.datastructures.pose import Pose, Transform from bullet_world_testcase import BulletWorldTestCase diff --git a/test/test_location_designator.py b/test/test_location_designator.py index 11ea179c0..54860a746 100644 --- a/test/test_location_designator.py +++ b/test/test_location_designator.py @@ -1,10 +1,6 @@ -import unittest from pycram.designators.location_designator import * -from pycram.designators import action_designator, object_designator from pycram.robot_descriptions import robot_description -from pycram.process_module import simulated_robot -from pycram.pose import Pose -import pycram.enums +from pycram.worlds.datastructures.pose import Pose from bullet_world_testcase import BulletWorldTestCase diff --git a/test/test_object.py b/test/test_object.py index 9ef50883c..d0c0ee12b 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -1,14 +1,11 @@ -import os - import numpy as np from bullet_world_testcase import BulletWorldTestCase -from pycram.enums import JointType, ObjectType -from pycram.pose import Pose -from pycram.world_dataclasses import Color -from pycram.world_object import Object -from pycram.urdf_interface import ObjectDescription +from pycram.worlds.datastructures.enums import JointType, ObjectType +from pycram.worlds.datastructures.pose import Pose +from pycram.worlds.datastructures.dataclasses import Color +from pycram.worlds.concepts.world_object import Object from geometry_msgs.msg import Point, Quaternion diff --git a/test/test_object_designator.py b/test/test_object_designator.py index f203a514d..695fc6b84 100644 --- a/test/test_object_designator.py +++ b/test/test_object_designator.py @@ -1,7 +1,7 @@ import unittest from bullet_world_testcase import BulletWorldTestCase from pycram.designators.object_designator import * -from pycram.enums import ObjectType +from pycram.worlds.datastructures.enums import ObjectType class TestObjectDesignator(BulletWorldTestCase): diff --git a/test/test_orm.py b/test/test_orm.py index ceec18c5d..f8697b78e 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -1,4 +1,3 @@ -import os import unittest import sqlalchemy @@ -14,7 +13,7 @@ from bullet_world_testcase import BulletWorldTestCase import test_task_tree from pycram.designators import action_designator, object_designator -from pycram.pose import Pose +from pycram.worlds.datastructures.pose import Pose from pycram.process_module import simulated_robot from pycram.task import with_tree diff --git a/test/test_pose.py b/test/test_pose.py index 97064a56d..c6ce5a63e 100644 --- a/test/test_pose.py +++ b/test/test_pose.py @@ -1,6 +1,6 @@ import unittest -from pycram.pose import Pose, Transform +from pycram.worlds.datastructures.pose import Pose, Transform class TestPose(unittest.TestCase): diff --git a/test/test_task_tree.py b/test/test_task_tree.py index 36d994663..ee06fd717 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -1,4 +1,4 @@ -from pycram.pose import Pose +from pycram.worlds.datastructures.pose import Pose from pycram.process_module import simulated_robot import pycram.task from pycram.task import with_tree From 44b7d714755f016016cc3e1fe3f949cb8d30ddfe Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 19 Mar 2024 09:32:52 +0100 Subject: [PATCH 66/69] [AbstractWorld] restructured folders and files. --- demos/pycram_bullet_world_demo/demo.py | 8 +++---- demos/pycram_ur5_demo/demo.py | 2 +- src/neem_interface_python | 2 +- .../{worlds/concepts => }/cache_manager.py | 0 .../concepts => datastructures}/__init__.py | 0 .../datastructures/dataclasses.py | 8 +++---- .../{worlds => }/datastructures/enums.py | 0 .../datastructures/local_transformer.py | 2 +- .../{worlds => }/datastructures/pose.py | 0 src/pycram/description.py | 10 ++++----- src/pycram/designator.py | 8 +++---- src/pycram/designators/action_designator.py | 6 ++--- src/pycram/designators/location_designator.py | 6 ++--- src/pycram/designators/motion_designator.py | 6 ++--- src/pycram/designators/object_designator.py | 4 ++-- src/pycram/external_interfaces/giskard.py | 6 ++--- src/pycram/external_interfaces/ik.py | 6 ++--- src/pycram/external_interfaces/robokudo.py | 6 ++--- src/pycram/helper.py | 4 ++-- src/pycram/language.py | 2 +- src/pycram/object_descriptors/urdf.py | 6 ++--- src/pycram/orm/action_designator.py | 2 +- src/pycram/orm/base.py | 2 +- src/pycram/orm/motion_designator.py | 2 +- src/pycram/orm/object_designator.py | 2 +- src/pycram/orm/task.py | 2 +- src/pycram/pose_generator_and_validator.py | 6 ++--- .../process_modules/boxy_process_modules.py | 2 +- .../default_process_modules.py | 2 +- .../process_modules/donbot_process_modules.py | 2 +- .../process_modules/hsr_process_modules.py | 2 +- .../process_modules/pr2_process_modules.py | 8 +++---- .../resolver/location/database_location.py | 4 ++-- .../resolver/location/giskard_location.py | 2 +- src/pycram/resolver/location/jpt_location.py | 6 ++--- src/pycram/ros/robot_state_updater.py | 2 +- src/pycram/ros/tf_broadcaster.py | 2 +- src/pycram/ros/viz_marker_publisher.py | 4 ++-- src/pycram/task.py | 4 ++-- src/pycram/world.py | 22 +++++++++---------- src/pycram/world_concepts/__init__.py | 1 + .../constraints.py | 4 ++-- .../concepts => world_concepts}/costmaps.py | 8 +++---- .../concepts => world_concepts}/event.py | 0 .../world_object.py | 12 +++++----- src/pycram/world_reasoning.py | 4 ++-- src/pycram/worlds/bullet_world.py | 10 ++++----- src/pycram/worlds/datastructures/__init__.py | 0 test/bullet_world_testcase.py | 6 ++--- test/test_action_designator.py | 4 ++-- test/test_bullet_world.py | 8 +++---- test/test_bullet_world_reasoning.py | 2 +- test/test_cache_manager.py | 4 ++-- test/test_costmaps.py | 4 ++-- test/test_database_merger.py | 4 ++-- test/test_database_resolver.py | 6 ++--- test/test_failure_handling.py | 2 +- test/test_jpt_resolver.py | 6 ++--- test/test_language.py | 4 ++-- test/test_links.py | 2 +- test/test_local_transformer.py | 4 ++-- test/test_location_designator.py | 2 +- test/test_object.py | 8 +++---- test/test_object_designator.py | 2 +- test/test_orm.py | 2 +- test/test_pose.py | 2 +- test/test_task_tree.py | 2 +- 67 files changed, 142 insertions(+), 141 deletions(-) rename src/pycram/{worlds/concepts => }/cache_manager.py (100%) rename src/pycram/{worlds/concepts => datastructures}/__init__.py (100%) rename src/pycram/{worlds => }/datastructures/dataclasses.py (96%) rename src/pycram/{worlds => }/datastructures/enums.py (100%) rename src/pycram/{worlds => }/datastructures/local_transformer.py (98%) rename src/pycram/{worlds => }/datastructures/pose.py (100%) create mode 100644 src/pycram/world_concepts/__init__.py rename src/pycram/{worlds/concepts => world_concepts}/constraints.py (98%) rename src/pycram/{worlds/concepts => world_concepts}/costmaps.py (99%) rename src/pycram/{worlds/concepts => world_concepts}/event.py (100%) rename src/pycram/{worlds/concepts => world_concepts}/world_object.py (98%) delete mode 100644 src/pycram/worlds/datastructures/__init__.py diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 188eb1baa..c44ca362c 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -2,12 +2,12 @@ from pycram.designators.action_designator import * from pycram.designators.location_designator import * from pycram.designators.object_designator import * -from pycram.worlds.datastructures.enums import ObjectType -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.enums import ObjectType +from pycram.datastructures.pose import Pose from pycram.process_module import simulated_robot, with_simulated_robot from pycram.object_descriptors.urdf import ObjectDescription -from pycram.worlds.concepts.world_object import Object -from pycram.worlds.datastructures.dataclasses import Color +from pycram.world_concepts.world_object import Object +from pycram.datastructures.dataclasses import Color extension = ObjectDescription.get_file_extension() diff --git a/demos/pycram_ur5_demo/demo.py b/demos/pycram_ur5_demo/demo.py index b0cc4358e..a9839ea4d 100644 --- a/demos/pycram_ur5_demo/demo.py +++ b/demos/pycram_ur5_demo/demo.py @@ -6,7 +6,7 @@ from pycram.worlds.bullet_world import BulletWorld from pycram.world import Object -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from pycram.ros.force_torque_sensor import ForceTorqueSensor from pycram.ros.joint_state_publisher import JointStatePublisher from pycram.ros.tf_broadcaster import TFBroadcaster diff --git a/src/neem_interface_python b/src/neem_interface_python index 285033958..899d74eca 160000 --- a/src/neem_interface_python +++ b/src/neem_interface_python @@ -1 +1 @@ -Subproject commit 28503395862f676fc2ad19877559673bd988fd07 +Subproject commit 899d74eca6c34d8bb53202d41677fdae9ee9d0a6 diff --git a/src/pycram/worlds/concepts/cache_manager.py b/src/pycram/cache_manager.py similarity index 100% rename from src/pycram/worlds/concepts/cache_manager.py rename to src/pycram/cache_manager.py diff --git a/src/pycram/worlds/concepts/__init__.py b/src/pycram/datastructures/__init__.py similarity index 100% rename from src/pycram/worlds/concepts/__init__.py rename to src/pycram/datastructures/__init__.py diff --git a/src/pycram/worlds/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py similarity index 96% rename from src/pycram/worlds/datastructures/dataclasses.py rename to src/pycram/datastructures/dataclasses.py index c41a41b50..b67833fbc 100644 --- a/src/pycram/worlds/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -2,14 +2,14 @@ from dataclasses import dataclass from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING -from pycram.worlds.datastructures.enums import JointType, Shape -from pycram.worlds.datastructures.pose import Pose, Point +from pycram.datastructures.enums import JointType, Shape +from pycram.datastructures.pose import Pose, Point from abc import ABC, abstractmethod if TYPE_CHECKING: from pycram.description import Link - from pycram.worlds.concepts.world_object import Object - from pycram.worlds.concepts.constraints import Attachment + from pycram.world_concepts.world_object import Object + from pycram.world_concepts.constraints import Attachment def get_point_as_list(point: Point) -> List[float]: diff --git a/src/pycram/worlds/datastructures/enums.py b/src/pycram/datastructures/enums.py similarity index 100% rename from src/pycram/worlds/datastructures/enums.py rename to src/pycram/datastructures/enums.py diff --git a/src/pycram/worlds/datastructures/local_transformer.py b/src/pycram/datastructures/local_transformer.py similarity index 98% rename from src/pycram/worlds/datastructures/local_transformer.py rename to src/pycram/datastructures/local_transformer.py index 635a6fa38..59a9f594d 100644 --- a/src/pycram/worlds/datastructures/local_transformer.py +++ b/src/pycram/datastructures/local_transformer.py @@ -9,7 +9,7 @@ from rospy import Duration from geometry_msgs.msg import TransformStamped -from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.datastructures.pose import Pose, Transform from typing_extensions import List, Optional, Union, Iterable diff --git a/src/pycram/worlds/datastructures/pose.py b/src/pycram/datastructures/pose.py similarity index 100% rename from src/pycram/worlds/datastructures/pose.py rename to src/pycram/datastructures/pose.py diff --git a/src/pycram/description.py b/src/pycram/description.py index dc11387ea..f10a8efd0 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -8,14 +8,14 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING -from pycram.worlds.datastructures.enums import JointType -from pycram.worlds.datastructures.local_transformer import LocalTransformer -from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.datastructures.enums import JointType +from pycram.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.pose import Pose, Transform from pycram.world import WorldEntity -from pycram.worlds.datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape +from pycram.datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape if TYPE_CHECKING: - from pycram.worlds.concepts.world_object import Object + from pycram.world_concepts.world_object import Object class EntityDescription(ABC): diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 6b1743d64..eec2b4e87 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -9,17 +9,17 @@ import rospy from pycram.world import World -from pycram.worlds.concepts.world_object import Object as WorldObject +from pycram.world_concepts.world_object import Object as WorldObject from .helper import GeneratorList, bcolors from threading import Lock from time import time from typing_extensions import List, Dict, Any, Optional, Union, get_type_hints, Callable, Iterable -from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.local_transformer import LocalTransformer from .language import Language -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from .robot_descriptions import robot_description -from pycram.worlds.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType import logging diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 987a787a2..8cff34ed4 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -6,7 +6,7 @@ from .location_designator import CostmapLocation from .motion_designator import * from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart -from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.local_transformer import LocalTransformer from ..orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction, MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction, @@ -19,10 +19,10 @@ from ..plan_failures import ObjectUnfetchable, ReachabilityFailure from ..robot_descriptions import robot_description from ..task import with_tree -from pycram.worlds.datastructures.enums import Arms +from pycram.datastructures.enums import Arms from ..designator import ActionDesignatorDescription from pycram.world import World -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from ..helper import multiply_quaternions diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index cc78cedc5..810c94d0f 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -5,14 +5,14 @@ from pycram.world import World, UseProspectionWorld from pycram.world_reasoning import link_pose_for_joint_config from ..designator import DesignatorError, LocationDesignatorDescription -from pycram.worlds.concepts.costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap +from pycram.world_concepts.costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap from ..robot_descriptions import robot_description -from pycram.worlds.datastructures.enums import JointType +from pycram.datastructures.enums import JointType from ..helper import transform from pycram.pose_generator_and_validator import pose_generator, visibility_validator, reachability_validator, \ generate_orientation from ..robot_description import ManipulatorDescription -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose class Location(LocationDesignatorDescription): diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index f24cb0f05..6b439996f 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -3,7 +3,7 @@ from sqlalchemy.orm import Session from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject from pycram.world import World -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object from ..designator import DesignatorError from ..plan_failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager @@ -13,10 +13,10 @@ MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion, MoveGripperMotion as ORMMoveGripperMotion, DetectingMotion as ORMDetectingMotion, OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion) -from pycram.worlds.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType from typing_extensions import List, Dict, Callable, Optional -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from ..task import with_tree diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 6826eef00..6fcb5c40e 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -2,11 +2,11 @@ from typing_extensions import List, Optional, Callable import sqlalchemy.orm from pycram.world import World -from pycram.worlds.concepts.world_object import Object as WorldObject +from pycram.world_concepts.world_object import Object as WorldObject from ..designator import ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from ..external_interfaces.robokudo import query diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index ce1afabec..bd88af990 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -5,11 +5,11 @@ import sys import rosnode -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from ..robot_descriptions import robot_description from pycram.world import World -from pycram.worlds.datastructures.dataclasses import MeshVisualShape -from pycram.worlds.concepts.world_object import Object +from pycram.datastructures.dataclasses import MeshVisualShape +from pycram.world_concepts.world_object import Object from ..robot_description import ManipulatorDescription from typing_extensions import List, Dict, Callable, Optional diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index c70a6469c..1d98a30a1 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -6,10 +6,10 @@ from moveit_msgs.srv import GetPositionIK from sensor_msgs.msg import JointState -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object from ..helper import calculate_wrist_tool_offset, _apply_ik -from pycram.worlds.datastructures.local_transformer import LocalTransformer -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.pose import Pose from ..robot_descriptions import robot_description from ..plan_failures import IKError diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index 531c0eb6f..23c571cbc 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -6,10 +6,10 @@ import rosnode from ..designator import ObjectDesignatorDescription -from pycram.worlds.datastructures.pose import Pose -from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.pose import Pose +from pycram.datastructures.local_transformer import LocalTransformer from pycram.world import World -from pycram.worlds.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType try: from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjetDesignator diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 0da52632a..672d9951f 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -13,8 +13,8 @@ from pytransform3d.rotations import quaternion_wxyz_from_xyzw, quaternion_xyzw_from_wxyz from pytransform3d.transformations import transform_from_pq, transform_from, pq_from_transform -from pycram.worlds.concepts.world_object import Object as WorldObject -from pycram.worlds.datastructures.pose import Transform, Pose +from pycram.world_concepts.world_object import Object as WorldObject +from pycram.datastructures.pose import Transform, Pose import math diff --git a/src/pycram/language.py b/src/pycram/language.py index 310f8ce1b..8786f9309 100644 --- a/src/pycram/language.py +++ b/src/pycram/language.py @@ -5,7 +5,7 @@ from typing_extensions import Iterable, Optional, Callable, Dict, Any, List, Union from anytree import NodeMixin, Node, PreOrderIter -from pycram.worlds.datastructures.enums import State +from pycram.datastructures.enums import State import threading from .fluent import Fluent diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index b2905e170..4db166b3b 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -10,11 +10,11 @@ from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, Sphere as URDF_Sphere, Mesh as URDF_Mesh) -from pycram.worlds.datastructures.enums import JointType, Shape -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.enums import JointType, Shape +from pycram.datastructures.pose import Pose from pycram.description import JointDescription as AbstractJointDescription, \ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription -from pycram.worlds.datastructures.dataclasses import Color +from pycram.datastructures.dataclasses import Color class LinkDescription(AbstractLinkDescription): diff --git a/src/pycram/orm/action_designator.py b/src/pycram/orm/action_designator.py index 7bbaa0525..527fade81 100644 --- a/src/pycram/orm/action_designator.py +++ b/src/pycram/orm/action_designator.py @@ -2,7 +2,7 @@ from .base import RobotState, Designator, MapperArgsMixin, PoseMixin from .object_designator import ObjectMixin -from pycram.worlds.datastructures.enums import Arms +from pycram.datastructures.enums import Arms from sqlalchemy.orm import Mapped, mapped_column, relationship from sqlalchemy import ForeignKey diff --git a/src/pycram/orm/base.py b/src/pycram/orm/base.py index 61a6ab39a..e076fc929 100644 --- a/src/pycram/orm/base.py +++ b/src/pycram/orm/base.py @@ -10,7 +10,7 @@ from sqlalchemy.orm import DeclarativeBase, Mapped, MappedAsDataclass, mapped_column, Session, relationship, \ declared_attr -from pycram.worlds.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType def get_pycram_version_from_git() -> Optional[str]: diff --git a/src/pycram/orm/motion_designator.py b/src/pycram/orm/motion_designator.py index af67aeb9e..bada973b4 100644 --- a/src/pycram/orm/motion_designator.py +++ b/src/pycram/orm/motion_designator.py @@ -12,7 +12,7 @@ from sqlalchemy.orm import Mapped, mapped_column, relationship from sqlalchemy import ForeignKey -from pycram.worlds.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType class Motion(MapperArgsMixin, Designator): diff --git a/src/pycram/orm/object_designator.py b/src/pycram/orm/object_designator.py index 776ab59ce..ff7fd0665 100644 --- a/src/pycram/orm/object_designator.py +++ b/src/pycram/orm/object_designator.py @@ -2,7 +2,7 @@ from pycram.orm.base import Base, MapperArgsMixin, PoseMixin from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship from sqlalchemy import ForeignKey -from pycram.worlds.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType class ObjectMixin: """ diff --git a/src/pycram/orm/task.py b/src/pycram/orm/task.py index d821a2a5d..427fee0a2 100644 --- a/src/pycram/orm/task.py +++ b/src/pycram/orm/task.py @@ -3,7 +3,7 @@ from sqlalchemy import ForeignKey from sqlalchemy.orm import Mapped, mapped_column, relationship from .base import Base, Designator -from pycram.worlds.datastructures.enums import TaskStatus +from pycram.datastructures.enums import TaskStatus import datetime diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 96f1bf26f..fc2e0fff1 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -2,10 +2,10 @@ import numpy as np from pycram.world import World -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object from pycram.world_reasoning import contact -from pycram.worlds.concepts.costmaps import Costmap -from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.world_concepts.costmaps import Costmap +from pycram.datastructures.pose import Pose, Transform from pycram.robot_descriptions import robot_description from pycram.external_interfaces.ik import request_ik from pycram.plan_failures import IKError diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index f0aecef08..366cac9dd 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -4,7 +4,7 @@ from pycram.world import World from ..designators.motion_designator import PlaceMotion -from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description from ..process_modules.pr2_process_modules import (_park_arms, Pr2Navigation as BoxyNavigation, diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 8993b10d2..9ee40414d 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -9,7 +9,7 @@ Pr2MoveTCP as DefaultMoveTCP from ..robot_descriptions import robot_description from pycram.world import World -from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.local_transformer import LocalTransformer from ..designators.motion_designator import LookingMotion, MoveArmJointsMotion, MoveJointsMotion diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index 41a8704b9..7371a8876 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -5,7 +5,7 @@ from pycram.worlds.bullet_world import World from ..designators.motion_designator import MoveArmJointsMotion, WorldStateDetectingMotion -from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description from ..process_modules.pr2_process_modules import Pr2PickUp, Pr2Detecting as DonbotDetecting, _move_arm_tcp diff --git a/src/pycram/process_modules/hsr_process_modules.py b/src/pycram/process_modules/hsr_process_modules.py index 75cb82e72..871c4b808 100644 --- a/src/pycram/process_modules/hsr_process_modules.py +++ b/src/pycram/process_modules/hsr_process_modules.py @@ -4,7 +4,7 @@ from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager from pycram.world import World -from pycram.worlds.datastructures.pose import Pose, Point +from pycram.datastructures.pose import Pose, Point from ..helper import _apply_ik from ..external_interfaces.ik import request_ik from .. import world_reasoning as btr diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 204b3d1b8..784f18385 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -12,16 +12,16 @@ from ..process_module import ProcessModule, ProcessModuleManager from ..external_interfaces.ik import request_ik from ..helper import _apply_ik -from pycram.worlds.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.local_transformer import LocalTransformer from ..designators.object_designator import ObjectDesignatorDescription from ..designators.motion_designator import MoveMotion, PickUpMotion, PlaceMotion, LookingMotion, \ DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ MoveGripperMotion, OpeningMotion, ClosingMotion, MotionDesignatorDescription from ..robot_descriptions import robot_description from pycram.world import World -from pycram.worlds.concepts.world_object import Object -from pycram.worlds.datastructures.pose import Pose -from pycram.worlds.datastructures.enums import JointType, ObjectType +from pycram.world_concepts.world_object import Object +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import JointType, ObjectType from ..external_interfaces import giskard from ..external_interfaces.robokudo import query diff --git a/src/pycram/resolver/location/database_location.py b/src/pycram/resolver/location/database_location.py index 4c205bbbb..3c9beb519 100644 --- a/src/pycram/resolver/location/database_location.py +++ b/src/pycram/resolver/location/database_location.py @@ -4,14 +4,14 @@ from tf import transformations import pycram.designators.location_designator import pycram.task -from pycram.worlds.concepts.costmaps import OccupancyCostmap +from pycram.world_concepts.costmaps import OccupancyCostmap from pycram.orm.action_designator import PickUpAction from pycram.orm.object_designator import Object from pycram.orm.base import Position, RobotState from pycram.orm.task import TaskTreeNode, Code from .jpt_location import JPTCostmapLocation from ... import orm -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose class DatabaseCostmapLocation(pycram.designators.location_designator.CostmapLocation): diff --git a/src/pycram/resolver/location/giskard_location.py b/src/pycram/resolver/location/giskard_location.py index 506e3503c..3d8ee79ec 100644 --- a/src/pycram/resolver/location/giskard_location.py +++ b/src/pycram/resolver/location/giskard_location.py @@ -1,7 +1,7 @@ from ...external_interfaces.giskard import achieve_cartesian_goal from ...designators.location_designator import CostmapLocation from pycram.world import UseProspectionWorld, World -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from ...robot_descriptions import robot_description from pycram.pose_generator_and_validator import reachability_validator from typing_extensions import Tuple, Dict diff --git a/src/pycram/resolver/location/jpt_location.py b/src/pycram/resolver/location/jpt_location.py index 73aa3a6f7..bd71413ae 100644 --- a/src/pycram/resolver/location/jpt_location.py +++ b/src/pycram/resolver/location/jpt_location.py @@ -6,10 +6,10 @@ import tf from ...designators import location_designator -from pycram.worlds.concepts.costmaps import OccupancyCostmap -from pycram.worlds.datastructures.pose import Pose +from pycram.world_concepts.costmaps import OccupancyCostmap +from pycram.datastructures.pose import Pose from pycram.world import Object, World -from pycram.worlds.datastructures.dataclasses import BoxVisualShape, Color +from pycram.datastructures.dataclasses import BoxVisualShape, Color class JPTCostmapLocation(location_designator.CostmapLocation): diff --git a/src/pycram/ros/robot_state_updater.py b/src/pycram/ros/robot_state_updater.py index e1400fb5b..8c8a8e43b 100644 --- a/src/pycram/ros/robot_state_updater.py +++ b/src/pycram/ros/robot_state_updater.py @@ -7,7 +7,7 @@ from sensor_msgs.msg import JointState from pycram.world import World from ..robot_descriptions import robot_description -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose class RobotStateUpdater: diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros/tf_broadcaster.py index c698f9b8b..ab14c1297 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros/tf_broadcaster.py @@ -3,7 +3,7 @@ import threading import atexit -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from pycram.world import World from tf2_msgs.msg import TFMessage diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 317af4724..1b98da46c 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -9,8 +9,8 @@ from visualization_msgs.msg import MarkerArray, Marker import rospy -from pycram.worlds.datastructures.pose import Transform -from pycram.worlds.datastructures.dataclasses import MeshVisualShape, CylinderVisualShape, BoxVisualShape, SphereVisualShape +from pycram.datastructures.pose import Transform +from pycram.datastructures.dataclasses import MeshVisualShape, CylinderVisualShape, BoxVisualShape, SphereVisualShape class VizMarkerPublisher: diff --git a/src/pycram/task.py b/src/pycram/task.py index de870493f..415d7d68c 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -18,8 +18,8 @@ from .orm.base import ProcessMetaData from .plan_failures import PlanFailure from .language import Code -from pycram.worlds.datastructures.enums import TaskStatus -from pycram.worlds.datastructures.dataclasses import Color +from pycram.datastructures.enums import TaskStatus +from pycram.datastructures.dataclasses import Color class TaskCode(Code): diff --git a/src/pycram/world.py b/src/pycram/world.py index 3df891777..97e4361f7 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -14,19 +14,19 @@ from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING from typing_extensions import Union -from pycram.worlds.concepts.cache_manager import CacheManager -from pycram.worlds.datastructures.enums import JointType, ObjectType, WorldMode -from pycram.worlds.concepts.event import Event -from pycram.worlds.datastructures.local_transformer import LocalTransformer -from pycram.worlds.datastructures.pose import Pose, Transform -from pycram.worlds.concepts.constraints import Constraint -from pycram.worlds.datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, - MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, - CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState) +from pycram.cache_manager import CacheManager +from pycram.datastructures.enums import JointType, ObjectType, WorldMode +from pycram.world_concepts.event import Event +from pycram.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.pose import Pose, Transform +from pycram.world_concepts.constraints import Constraint +from pycram.datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, + MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, + ObjectState, State, WorldState) if TYPE_CHECKING: - from pycram.worlds.concepts.world_object import Object + from pycram.world_concepts.world_object import Object from pycram.description import Link, Joint diff --git a/src/pycram/world_concepts/__init__.py b/src/pycram/world_concepts/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/src/pycram/world_concepts/__init__.py @@ -0,0 +1 @@ + diff --git a/src/pycram/worlds/concepts/constraints.py b/src/pycram/world_concepts/constraints.py similarity index 98% rename from src/pycram/worlds/concepts/constraints.py rename to src/pycram/world_concepts/constraints.py index c36b733a2..540155ff5 100644 --- a/src/pycram/worlds/concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -3,8 +3,8 @@ from geometry_msgs.msg import Point from typing_extensions import Union, List, Optional, TYPE_CHECKING -from pycram.worlds.datastructures.enums import JointType -from pycram.worlds.datastructures.pose import Transform, Pose +from pycram.datastructures.enums import JointType +from pycram.datastructures.pose import Transform, Pose if TYPE_CHECKING: from pycram.description import Link diff --git a/src/pycram/worlds/concepts/costmaps.py b/src/pycram/world_concepts/costmaps.py similarity index 99% rename from src/pycram/worlds/concepts/costmaps.py rename to src/pycram/world_concepts/costmaps.py index 8411e5fa0..7f714afa8 100644 --- a/src/pycram/worlds/concepts/costmaps.py +++ b/src/pycram/world_concepts/costmaps.py @@ -11,12 +11,12 @@ from nav_msgs.msg import OccupancyGrid, MapMetaData from pycram.world import UseProspectionWorld -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object from pycram.description import Link -from pycram.worlds.datastructures.local_transformer import LocalTransformer -from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.pose import Pose, Transform from pycram.world import World -from pycram.worlds.datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color +from pycram.datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color class Costmap: diff --git a/src/pycram/worlds/concepts/event.py b/src/pycram/world_concepts/event.py similarity index 100% rename from src/pycram/worlds/concepts/event.py rename to src/pycram/world_concepts/event.py diff --git a/src/pycram/worlds/concepts/world_object.py b/src/pycram/world_concepts/world_object.py similarity index 98% rename from src/pycram/worlds/concepts/world_object.py rename to src/pycram/world_concepts/world_object.py index 71b05579c..3f9757b1a 100644 --- a/src/pycram/worlds/concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -12,12 +12,12 @@ from pycram.object_descriptors.urdf import ObjectDescription as URDFObject from pycram.robot_descriptions import robot_description from pycram.world import WorldEntity, World -from pycram.worlds.concepts.constraints import Attachment -from pycram.worlds.datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, - AxisAlignedBoundingBox, VisualShape) -from pycram.worlds.datastructures.enums import ObjectType, JointType -from pycram.worlds.datastructures.local_transformer import LocalTransformer -from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.world_concepts.constraints import Attachment +from pycram.datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, + AxisAlignedBoundingBox, VisualShape) +from pycram.datastructures.enums import ObjectType, JointType +from pycram.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.pose import Pose, Transform Link = ObjectDescription.Link diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 27111daa7..7e7d89712 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -4,9 +4,9 @@ import numpy as np from pycram.external_interfaces.ik import try_to_reach, try_to_reach_with_grasp -from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.datastructures.pose import Pose, Transform from pycram.robot_descriptions import robot_description -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object from pycram.world import World, UseProspectionWorld diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index addb57190..0013a52d5 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -11,13 +11,13 @@ from geometry_msgs.msg import Point from typing_extensions import List, Optional, Dict -from pycram.worlds.datastructures.enums import ObjectType, WorldMode, JointType -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.enums import ObjectType, WorldMode, JointType +from pycram.datastructures.pose import Pose from pycram.object_descriptors.urdf import ObjectDescription from pycram.world import World -from pycram.worlds.concepts.constraints import Constraint -from pycram.worlds.datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.constraints import Constraint +from pycram.datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape +from pycram.world_concepts.world_object import Object Link = ObjectDescription.Link RootLink = ObjectDescription.RootLink diff --git a/src/pycram/worlds/datastructures/__init__.py b/src/pycram/worlds/datastructures/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 46144af36..3b8ae3ea7 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -2,11 +2,11 @@ import unittest from pycram.worlds.bullet_world import BulletWorld -from pycram.worlds.concepts.world_object import Object -from pycram.worlds.datastructures.pose import Pose +from pycram.world_concepts.world_object import Object +from pycram.datastructures.pose import Pose from pycram.robot_descriptions import robot_description from pycram.process_module import ProcessModule -from pycram.worlds.datastructures.enums import ObjectType, WorldMode +from pycram.datastructures.enums import ObjectType, WorldMode from pycram.object_descriptors.urdf import ObjectDescription diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 656df74b8..363ae8602 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -2,8 +2,8 @@ from pycram.designators import action_designator, object_designator from pycram.robot_descriptions import robot_description from pycram.process_module import simulated_robot -from pycram.worlds.datastructures.pose import Pose -from pycram.worlds.datastructures.enums import ObjectType, Arms +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import ObjectType, Arms from bullet_world_testcase import BulletWorldTestCase import numpy as np diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 058adb0c4..bf0c5d75e 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -5,12 +5,12 @@ import rospkg from bullet_world_testcase import BulletWorldTestCase, BulletWorldGUITestCase -from pycram.worlds.datastructures.enums import ObjectType, WorldMode -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.enums import ObjectType, WorldMode +from pycram.datastructures.pose import Pose from pycram.robot_descriptions import robot_description from pycram.object_descriptors.urdf import ObjectDescription -from pycram.worlds.datastructures.dataclasses import Color -from pycram.worlds.concepts.world_object import Object +from pycram.datastructures.dataclasses import Color +from pycram.world_concepts.world_object import Object fix_missing_inertial = ObjectDescription.fix_missing_inertial diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index 4438d37ce..660c9682b 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -2,7 +2,7 @@ import pycram.world_reasoning as btr from bullet_world_testcase import BulletWorldTestCase -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from pycram.robot_descriptions import robot_description use_new = True diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index ef126f2eb..0a4ac8524 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -1,8 +1,8 @@ from pathlib import Path from bullet_world_testcase import BulletWorldTestCase -from pycram.worlds.datastructures.enums import ObjectType -from pycram.worlds.concepts.world_object import Object +from pycram.datastructures.enums import ObjectType +from pycram.world_concepts.world_object import Object class TestCacheManager(BulletWorldTestCase): diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 1bffb32c2..429021dec 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -1,8 +1,8 @@ import numpy as np from bullet_world_testcase import BulletWorldTestCase -from pycram.worlds.concepts.costmaps import OccupancyCostmap -from pycram.worlds.datastructures.pose import Pose +from pycram.world_concepts.costmaps import OccupancyCostmap +from pycram.datastructures.pose import Pose class TestCostmapsCase(BulletWorldTestCase): diff --git a/test/test_database_merger.py b/test/test_database_merger.py index fbe396431..c0d3018f5 100644 --- a/test/test_database_merger.py +++ b/test/test_database_merger.py @@ -8,11 +8,11 @@ from pycram.designators.action_designator import * from pycram.designators.location_designator import * from pycram.process_module import simulated_robot -from pycram.worlds.datastructures.enums import Arms, ObjectType, WorldMode +from pycram.datastructures.enums import Arms, ObjectType, WorldMode from pycram.task import with_tree import pycram.task from pycram.worlds.bullet_world import BulletWorld -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object from pycram.designators.object_designator import * from dataclasses import dataclass, field diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 2809396d2..ce458b127 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -3,17 +3,17 @@ import sqlalchemy import sqlalchemy.orm import pycram.plan_failures -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object from pycram import task from pycram.world import World from pycram.designators import action_designator, object_designator from pycram.orm.base import Base from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from pycram.robot_descriptions import robot_description from pycram.task import with_tree -from pycram.worlds.datastructures.enums import ObjectType, WorldMode +from pycram.datastructures.enums import ObjectType, WorldMode # check if jpt is installed jpt_installed = True diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index bd7f36258..9008431e6 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -5,7 +5,7 @@ from pycram.worlds.bullet_world import BulletWorld, Object from pycram.designator import ActionDesignatorDescription from pycram.designators.action_designator import ParkArmsAction -from pycram.worlds.datastructures.enums import ObjectType, Arms, WorldMode +from pycram.datastructures.enums import ObjectType, Arms, WorldMode from pycram.failure_handling import Retry from pycram.plan_failures import PlanFailure from pycram.process_module import ProcessModule, simulated_robot diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index 534d6e4e3..b20cb64f1 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -6,13 +6,13 @@ import pycram.plan_failures from pycram.worlds.bullet_world import BulletWorld -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object from pycram.designators import action_designator, object_designator from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot from pycram.robot_descriptions import robot_description -from pycram.worlds.datastructures.pose import Pose -from pycram.worlds.datastructures.enums import WorldMode +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import WorldMode # check if jpt is installed jpt_installed = True diff --git a/test/test_language.py b/test/test_language.py index 458fbe0ac..4dcfa6d3e 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -2,10 +2,10 @@ import time import unittest from pycram.designators.action_designator import * -from pycram.worlds.datastructures.enums import ObjectType, State +from pycram.datastructures.enums import ObjectType, State from pycram.fluent import Fluent from pycram.plan_failures import PlanFailure -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Code from pycram.process_module import simulated_robot from bullet_world_testcase import BulletWorldTestCase diff --git a/test/test_links.py b/test/test_links.py index f737a55ce..695e670c1 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -1,5 +1,5 @@ from bullet_world_testcase import BulletWorldTestCase -from pycram.worlds.datastructures.dataclasses import Color +from pycram.datastructures.dataclasses import Color class TestLinks(BulletWorldTestCase): diff --git a/test/test_local_transformer.py b/test/test_local_transformer.py index afaac7a71..e64682d0d 100644 --- a/test/test_local_transformer.py +++ b/test/test_local_transformer.py @@ -1,8 +1,8 @@ import rospy -from pycram.worlds.datastructures.local_transformer import LocalTransformer -from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.datastructures.local_transformer import LocalTransformer +from pycram.datastructures.pose import Pose, Transform from bullet_world_testcase import BulletWorldTestCase diff --git a/test/test_location_designator.py b/test/test_location_designator.py index 54860a746..714f8ad54 100644 --- a/test/test_location_designator.py +++ b/test/test_location_designator.py @@ -1,6 +1,6 @@ from pycram.designators.location_designator import * from pycram.robot_descriptions import robot_description -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from bullet_world_testcase import BulletWorldTestCase diff --git a/test/test_object.py b/test/test_object.py index d0c0ee12b..5c71fc347 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -2,10 +2,10 @@ from bullet_world_testcase import BulletWorldTestCase -from pycram.worlds.datastructures.enums import JointType, ObjectType -from pycram.worlds.datastructures.pose import Pose -from pycram.worlds.datastructures.dataclasses import Color -from pycram.worlds.concepts.world_object import Object +from pycram.datastructures.enums import JointType, ObjectType +from pycram.datastructures.pose import Pose +from pycram.datastructures.dataclasses import Color +from pycram.world_concepts.world_object import Object from geometry_msgs.msg import Point, Quaternion diff --git a/test/test_object_designator.py b/test/test_object_designator.py index 695fc6b84..f85b036f4 100644 --- a/test/test_object_designator.py +++ b/test/test_object_designator.py @@ -1,7 +1,7 @@ import unittest from bullet_world_testcase import BulletWorldTestCase from pycram.designators.object_designator import * -from pycram.worlds.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType class TestObjectDesignator(BulletWorldTestCase): diff --git a/test/test_orm.py b/test/test_orm.py index f8697b78e..c2bd6d544 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -13,7 +13,7 @@ from bullet_world_testcase import BulletWorldTestCase import test_task_tree from pycram.designators import action_designator, object_designator -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from pycram.process_module import simulated_robot from pycram.task import with_tree diff --git a/test/test_pose.py b/test/test_pose.py index c6ce5a63e..5cee8c022 100644 --- a/test/test_pose.py +++ b/test/test_pose.py @@ -1,6 +1,6 @@ import unittest -from pycram.worlds.datastructures.pose import Pose, Transform +from pycram.datastructures.pose import Pose, Transform class TestPose(unittest.TestCase): diff --git a/test/test_task_tree.py b/test/test_task_tree.py index ee06fd717..9a0a6bd45 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -1,4 +1,4 @@ -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.pose import Pose from pycram.process_module import simulated_robot import pycram.task from pycram.task import with_tree From 4155d6af9feabf1681ab39c16b2b25510bae8939 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 21 Mar 2024 09:52:28 +0100 Subject: [PATCH 67/69] [AbstractWorld] Fixed viz_marker_publisher.py to use VisualShape for geometry. --- src/pycram/datastructures/enums.py | 4 +-- src/pycram/datastructures/pose.py | 4 +-- src/pycram/description.py | 2 +- src/pycram/object_descriptors/urdf.py | 41 ++++++++++++++++------- src/pycram/ros/viz_marker_publisher.py | 4 +-- src/pycram/world_concepts/world_object.py | 2 +- 6 files changed, 36 insertions(+), 21 deletions(-) diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 2f03e422b..a6867246f 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -19,6 +19,7 @@ class TaskStatus(Enum): SUCCEEDED = 2 FAILED = 3 + class JointType(Enum): """ Enum for readable joint types. @@ -32,9 +33,6 @@ class JointType(Enum): CONTINUOUS = 6 FLOATING = 7 - # override enum method to return an int - - class Grasp(Enum): """ diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 79eadaeef..7927ea277 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -58,10 +58,10 @@ def __init__(self, position: Optional[List[float]] = None, orientation: Optional :param time: The time at which this Pose is valid, as ROS time """ super().__init__() - if position: + if position is not None: self.position = position - if orientation: + if orientation is not None: self.orientation = orientation else: self.pose.orientation.w = 1.0 diff --git a/src/pycram/description.py b/src/pycram/description.py index f10a8efd0..0bd412e54 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -51,7 +51,7 @@ def __init__(self, parsed_link_description: Any): @property @abstractmethod - def geometry(self) -> VisualShape: + def geometry(self) -> Union[VisualShape, None]: """ Returns the geometry type of the collision element of this link. """ diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 4db166b3b..47b76931a 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -10,38 +10,55 @@ from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, Sphere as URDF_Sphere, Mesh as URDF_Mesh) -from pycram.datastructures.enums import JointType, Shape +from pycram.datastructures.enums import JointType from pycram.datastructures.pose import Pose from pycram.description import JointDescription as AbstractJointDescription, \ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription -from pycram.datastructures.dataclasses import Color +from pycram.datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ + SphereVisualShape, MeshVisualShape class LinkDescription(AbstractLinkDescription): """ A class that represents a link description of an object. """ - urdf_shape_map = { - URDF_Box: Shape.BOX, - URDF_Cylinder: Shape.CYLINDER, - URDF_Sphere: Shape.SPHERE, - URDF_Mesh: Shape.MESH - } def __init__(self, urdf_description: urdf.Link): super().__init__(urdf_description) @property - def geometry(self) -> Union[Shape, None]: + def geometry(self) -> Union[VisualShape, None]: """ Returns the geometry type of the URDF collision element of this link. """ - return self.urdf_shape_map[type(self.collision.geometry)] + if self.collision is None: + return None + urdf_geometry = self.collision.geometry + return self._get_visual_shape(urdf_geometry) + + @staticmethod + def _get_visual_shape(urdf_geometry) -> Union[VisualShape, None]: + """ + Returns the VisualShape of the given URDF geometry. + """ + if isinstance(urdf_geometry, URDF_Box): + return BoxVisualShape(Color(), [0, 0, 0], urdf_geometry.size) + if isinstance(urdf_geometry, URDF_Cylinder): + return CylinderVisualShape(Color(), [0, 0, 0], urdf_geometry.radius, urdf_geometry.length) + if isinstance(urdf_geometry, URDF_Sphere): + return SphereVisualShape(Color(), [0, 0, 0], urdf_geometry.radius) + if isinstance(urdf_geometry, URDF_Mesh): + return MeshVisualShape(Color(), [0, 0, 0], urdf_geometry.scale, urdf_geometry.filename) + return None @property - def origin(self) -> Pose: + def origin(self) -> Union[Pose, None]: + if self.collision is None: + return None + if self.collision.origin is None: + return None return Pose(self.collision.origin.xyz, - quaternion_from_euler(*self.collision.origin.rpy)) + quaternion_from_euler(*self.collision.origin.rpy).tolist()) @property def name(self) -> str: diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 1b98da46c..5b2189189 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -70,14 +70,14 @@ def _make_marker_array(self) -> MarkerArray: msg.type = Marker.MESH_RESOURCE msg.action = Marker.ADD link_pose = obj.get_link_transform(link) - if obj.get_link_origin(link): + if obj.get_link_origin(link) is not None: link_origin = obj.get_link_origin_transform(link) else: link_origin = Transform() link_pose_with_origin = link_pose * link_origin msg.pose = link_pose_with_origin.to_pose().pose - color = [1, 1, 1, 1] if obj.link_name_to_id[link] == -1 else obj.get_color() + color = [1, 1, 1, 1] if obj.link_name_to_id[link] == -1 else obj.get_link_color(link).get_rgba() msg.color = ColorRGBA(*color) msg.lifetime = rospy.Duration(1) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 3f9757b1a..54a80f8ae 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -286,7 +286,7 @@ def set_link_color(self, link_name: str, color: List[float]) -> None: """ self.links[link_name].color = Color.from_list(color) - def get_link_geometry(self, link_name: str) -> VisualShape: + def get_link_geometry(self, link_name: str) -> Union[VisualShape, None]: """ Returns the geometry of the link with the given name. :param link_name: The name of the link. From 5a918452b173351b0b77849e0a0439b571f20849 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 22 Mar 2024 11:04:22 +0100 Subject: [PATCH 68/69] [AbstractWorld] Add dae to possible mesh types. --- src/pycram/description.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 0bd412e54..1bbf7fc52 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -530,7 +530,7 @@ class ObjectDescription(EntityDescription): A class that represents the description of an object. """ - mesh_extensions: Tuple[str] = (".obj", ".stl") + mesh_extensions: Tuple[str] = (".obj", ".stl", ".dae") """ The file extensions of the mesh files that can be used to generate a description file. """ From 0f54c8bc01a7116e4ef59edde5f8c44ff1c5918b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 4 Apr 2024 16:56:24 +0200 Subject: [PATCH 69/69] [AbstractWorld] added dae to possible meshes and updated cache file write to ensure file is wrote. --- src/pycram/cache_manager.py | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index a8995636b..93c4c7673 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -1,3 +1,4 @@ +import logging import os import pathlib @@ -63,17 +64,27 @@ def generate_description_and_write_to_cache(self, path: str, name: str, extensio :param object_description: The object description of the file. """ description_string = object_description.generate_description_from_file(path, name, extension) - self.write_to_cache(description_string, cache_path) + wrote = self.write_to_cache(description_string, cache_path) + if wrote: + while not pathlib.Path(cache_path).exists(): + pass @staticmethod - def write_to_cache(description_string: str, cache_path: str) -> None: + def write_to_cache(description_string: str, cache_path: str) -> bool: """ Writes the description string to the cache directory. :param description_string: The description string to write to the cache directory. :param cache_path: The path of the file in the cache directory. - """ - with open(cache_path, "w") as file: - file.write(description_string) + Returns: + - True if the description string was written to the cache directory. + """ + try: + with open(cache_path, "w") as file: + file.write(description_string) + return True + except Exception as e: + logging.warning(f"Could not write to cache: {e}") + return False def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: """