From 7434361a0227e6348d316901a75ee550749d1a52 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 21 Sep 2023 14:46:40 +0200 Subject: [PATCH 1/9] [local transformer] only updates transforms when needed --- src/pycram/bullet_world.py | 10 ++++++---- src/pycram/local_transformer.py | 20 +++++++++++++------- 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 7a832dac1..dc87ebced 100644 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -462,14 +462,16 @@ def run(self): self.remove_obj_queue.task_done() for bulletworld_obj, shadow_obj in self.object_mapping.items(): - shadow_obj.set_pose(bulletworld_obj.get_pose()) + if bulletworld_obj.get_pose() != shadow_obj.get_pose(): + shadow_obj.set_pose(bulletworld_obj.get_pose()) # shadow_obj.set_position(bulletworld_obj.get_position()) # shadow_obj.set_orientation(bulletworld_obj.get_orientation()) # Manage joint positions if len(bulletworld_obj.joints) > 2: for joint_name in bulletworld_obj.joints.keys(): - shadow_obj.set_joint_state(joint_name, bulletworld_obj.get_joint_state(joint_name)) + if shadow_obj.get_joint_state(joint_name) != bulletworld_obj.get_joint_state(joint_name): + shadow_obj.set_joint_state(joint_name, bulletworld_obj.get_joint_state(joint_name)) self.check_for_pause() # self.check_for_equal() @@ -880,7 +882,7 @@ def set_pose(self, pose: Pose, base: bool = False) -> None: if base: position = np.array(position) + self.base_origin_shift p.resetBasePositionAndOrientation(self.id, position, orientation, self.world.client_id) - self.local_transformer.update_transforms_for_object(self) + # self.local_transformer.update_transforms_for_object(self) self._set_attached_objects([self]) @property @@ -1106,7 +1108,7 @@ 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.local_transformer.update_transforms_for_object(self) + # self.local_transformer.update_transforms_for_object(self) self._set_attached_objects([self]) def get_joint_state(self, joint_name: str) -> float: diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 716eaa88f..dd6a396e9 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -57,13 +57,13 @@ def __init__(self): # If the singelton was already initialized self._initialized = True - def update_objects(self) -> None: + 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` """ - if self.bullet_world: - for obj in list(self.bullet_world.current_bullet_world.objects): - self.update_transforms_for_object(obj) + curr_time = rospy.Time.now() + for obj in list(self.bullet_world.current_bullet_world.objects): + self.update_transforms_for_object(obj) def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: """ @@ -73,6 +73,7 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: :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() copy_pose = pose.copy() copy_pose.header.stamp = rospy.Time(0) if not self.canTransform(target_frame, pose.frame, rospy.Time(0)): @@ -115,23 +116,28 @@ def tf_transform(self, source_frame: str, target_frame: str, :param time: Time at which the transform should be :return: """ + self.update_objects_for_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, bullet_object: 'bullet_world.Object') -> None: + def update_transforms_for_object(self, bullet_object: 'bullet_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 time: a specific time that should be used """ - self.setTransform( - bullet_object.get_pose().to_transform(bullet_object.tf_frame)) + time = time if time else rospy.Time().now() + obj_transform = bullet_object.get_pose().to_transform(bullet_object.tf_frame) + obj_transform.header.stamp = time + self.setTransform(obj_transform) for link_name, id in bullet_object.links.items(): if id == -1: continue tf_stamped = bullet_object.get_link_pose(link_name).to_transform( bullet_object.get_link_tf_frame(link_name)) + tf_stamped.header.stamp = time self.setTransform(tf_stamped) def get_all_frames(self) -> List[str]: From effa5f100633c156027dc0a2961500701fb17a17 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 22 Sep 2023 15:42:23 +0200 Subject: [PATCH 2/9] [bullet/local-transformer] Optimzation by reducing the number of new Poses created --- src/pycram/bullet_world.py | 50 +++++++++++++++++++++++++++------ src/pycram/helper.py | 6 ++-- src/pycram/local_transformer.py | 4 +-- 3 files changed, 48 insertions(+), 12 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index dc87ebced..dbf669507 100644 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -371,8 +371,9 @@ def reset_bullet_world(self) -> None: attached_objects = list(obj.attachments.keys()) for att_obj in attached_objects: obj.detach(att_obj) - for joint_name in obj.joints.keys(): - obj.set_joint_state(joint_name, 0) + 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) @@ -462,10 +463,11 @@ def run(self): self.remove_obj_queue.task_done() for bulletworld_obj, shadow_obj in self.object_mapping.items(): - if bulletworld_obj.get_pose() != shadow_obj.get_pose(): + b_pose = bulletworld_obj.get_pose() + s_pose = shadow_obj.get_pose() + #if bulletworld_obj.get_pose() != shadow_obj.get_pose(): + if b_pose.dist(s_pose) != 0.0: shadow_obj.set_pose(bulletworld_obj.get_pose()) - # shadow_obj.set_position(bulletworld_obj.get_position()) - # shadow_obj.set_orientation(bulletworld_obj.get_orientation()) # Manage joint positions if len(bulletworld_obj.joints) > 2: @@ -748,7 +750,6 @@ def __init__(self, name: str, type: str, path: str, self.attachments: Dict[Object, List] = {} self.cids: Dict[Object, int] = {} self.original_pose = pose_in_map - self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list()) self.tf_frame = ("shadow/" if self.world.is_shadow_world else "") + self.name + "_" + str(self.id) @@ -763,6 +764,12 @@ def __init__(self, name: str, type: str, path: str, BulletWorld.robot = self self.links[self.urdf_object.get_root()] = -1 + + self._current_pose = pose_in_map + self._current_link_poses = {} + 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() @@ -868,7 +875,8 @@ def get_pose(self) -> Pose: :return: The current pose of this object """ - return Pose(*p.getBasePositionAndOrientation(self.id, physicsClientId=self.world.client_id)) + return self._current_pose + # return Pose(*p.getBasePositionAndOrientation(self.id, physicsClientId=self.world.client_id)) def set_pose(self, pose: Pose, base: bool = False) -> None: """ @@ -882,6 +890,8 @@ def set_pose(self, pose: Pose, base: bool = False) -> None: 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.local_transformer.update_transforms_for_object(self) self._set_attached_objects([self]) @@ -944,6 +954,7 @@ def _set_attached_objects(self, prev_object: List[Object]) -> None: 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: @@ -1086,7 +1097,8 @@ def get_link_pose(self, name: str) -> Pose: """ if name in self.links.keys() and self.links[name] == -1: return self.get_pose() - return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6]) + 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: """ @@ -1109,6 +1121,20 @@ def set_joint_state(self, joint_name: str, joint_pose: float) -> None: # return p.resetJointState(self.id, self.joints[joint_name], joint_pose, physicsClientId=self.world.client_id) # 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: + """ + 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._update_link_poses() self._set_attached_objects([self]) def get_joint_state(self, joint_name: str) -> float: @@ -1349,6 +1375,14 @@ 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_poses(self): + for link_name in self.links.keys(): + if link_name == self.urdf_object.get_root(): + self._current_link_poses[link_name] = self._current_pose + else: + self._current_link_poses[link_name] = Pose(*p.getLinkState(self.id, self.links[link_name], + physicsClientId=self.world.client_id)[4:6]) + def filter_contact_points(contact_points, exclude_ids) -> List: """ diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 1c261dbe5..22f6a9c5d 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -45,6 +45,7 @@ class bcolors: def _apply_ik(robot: BulletWorldObject, joint_poses: List[float], joints: List[str]) -> None: """ Apllies a list of joint poses calculated by an inverse kinematics solver to a robot + :param robot: The robot the joint poses should be applied on :param joint_poses: The joint poses to be applied :param gripper: specifies the gripper for which the ik solution should be applied @@ -53,8 +54,9 @@ 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 - for i in range(0, len(joints)): - robot.set_joint_state(joints[i], joint_poses[i]) + robot.set_joint_states(dict(zip(joints, joint_poses))) + # for i in range(0, len(joints)): + # robot.set_joint_state(joints[i], joint_poses[i]) def _transform_to_torso(pose_and_rotation: Tuple[List[float], List[float]], robot: BulletWorldObject) -> Tuple[ diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index dd6a396e9..2f1099b88 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -63,7 +63,7 @@ def update_objects_for_current_world(self) -> None: """ curr_time = rospy.Time.now() for obj in list(self.bullet_world.current_bullet_world.objects): - self.update_transforms_for_object(obj) + self.update_transforms_for_object(obj, curr_time) def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: """ @@ -128,7 +128,7 @@ def update_transforms_for_object(self, bullet_object: 'bullet_world.Object', tim :param bullet_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() + time = time if time else rospy.Time.now() obj_transform = bullet_object.get_pose().to_transform(bullet_object.tf_frame) obj_transform.header.stamp = time self.setTransform(obj_transform) From f4bf93e941c54515003fbe4ee8caff405d34af1c Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 25 Sep 2023 14:43:58 +0200 Subject: [PATCH 3/9] [pose] Pose and Transform now accept time at init --- src/pycram/pose.py | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/pycram/pose.py b/src/pycram/pose.py index 7b51abcd3..b0c8cf661 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -9,6 +9,7 @@ import rospy from geometry_msgs.msg import PoseStamped, TransformStamped, Vector3 from geometry_msgs.msg import (Pose as GeoPose, Quaternion as GeoQuaternion) +from std_msgs.msg import Header from tf import transformations @@ -26,7 +27,7 @@ class Pose(PoseStamped): """ def __init__(self, position: Optional[List[float]] = None, orientation: Optional[List[float]] = None, - frame: str = "map"): + frame: str = "map", time: rospy.Time = None): """ Poses can be initialized by a position and orientation given as lists, this is optional. By default, Poses are initialized with the position being [0, 0, 0], the orientation being [0, 0, 0, 1] and the frame being 'map'. @@ -45,7 +46,8 @@ def __init__(self, position: Optional[List[float]] = None, orientation: Optional self.pose.orientation.w = 1.0 self.header.frame_id = frame - self.header.stamp = rospy.Time.now() + + self.header.stamp = time if time else rospy.Time.now() self.frame = frame @@ -108,8 +110,6 @@ 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: - print(type(value)) - print(value) rospy.logwarn("Orientation can only be a list or geometry_msgs/Quaternion") return @@ -142,7 +142,7 @@ 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) + return Transform(self.position_as_list(), self.orientation_as_list(), self.frame, child_frame, self.header.stamp) def copy(self) -> Pose: """ @@ -150,9 +150,9 @@ def copy(self) -> Pose: :return: A copy of this pose """ - p = Pose(self.position_as_list(), self.orientation_as_list(), self.frame) + 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 + # p.header.stamp = self.header.stamp return p def position_as_list(self) -> List[float]: @@ -232,7 +232,7 @@ class Transform(TransformStamped): Rotation: A quaternion representing the conversion of rotation between both frames """ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional[List[float]] = None, - frame: Optional[str] = "map", child_frame: Optional[str] = ""): + frame: Optional[str] = "map", child_frame: Optional[str] = "", time: rospy.Time = None): """ Transforms take a translation, rotation, frame and child_frame as optional arguments. If nothing is given the Transform will be initialized with [0, 0, 0] for translation, [0, 0, 0, 1] for rotation, 'map' for frame and an @@ -254,7 +254,7 @@ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional self.header.frame_id = frame self.child_frame_id = child_frame - self.header.stamp = rospy.Time.now() + self.header.stamp = time if time else rospy.Time.now() self.frame = frame @@ -339,9 +339,9 @@ def copy(self) -> Transform: :return: A copy of this pose """ - t = Transform(self.translation_as_list(), self.rotation_as_list(), self.frame, self.child_frame_id) + t = Transform(self.translation_as_list(), self.rotation_as_list(), self.frame, self.child_frame_id, self.header.stamp) t.header.frame_id = self.header.frame_id - t.header.stamp = self.header.stamp + # t.header.stamp = self.header.stamp return t def translation_as_list(self) -> List[float]: @@ -367,7 +367,7 @@ def to_pose(self) -> Pose: :return: A new pose with same translation as position and rotation as orientation """ - return Pose(self.translation_as_list(), self.rotation_as_list(), self.frame) + return Pose(self.translation_as_list(), self.rotation_as_list(), self.frame, self.header.stamp) def invert(self) -> Transform: """ @@ -380,7 +380,7 @@ def invert(self) -> Transform: inverse_transform = transformations.inverse_matrix(transform) translation = transformations.translation_from_matrix(inverse_transform) quaternion = transformations.quaternion_from_matrix(inverse_transform) - return Transform(list(translation), list(quaternion), self.child_frame_id, self.header.frame_id) + return Transform(list(translation), list(quaternion), self.child_frame_id, self.header.frame_id, self.header.stamp) def __mul__(self, other: Transform) -> Union[Transform, None]: """ @@ -451,3 +451,4 @@ def set_rotation(self, new_rotation: List[float]) -> None: """ self.rotation = new_rotation + From bce73faf84b596a19535e94ba2f1202a3a6a9cff Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 25 Sep 2023 14:44:35 +0200 Subject: [PATCH 4/9] [pose] added doc --- src/pycram/pose.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/pycram/pose.py b/src/pycram/pose.py index b0c8cf661..2c066159a 100644 --- a/src/pycram/pose.py +++ b/src/pycram/pose.py @@ -35,6 +35,7 @@ def __init__(self, position: Optional[List[float]] = None, orientation: Optional :param position: An optional position of this Pose :param orientation: An optional orientation of this Pose :param frame: An optional frame in which this pose is + :param time: The time at which this Pose is valid, as ROS time """ super().__init__() if position: @@ -242,6 +243,7 @@ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional :param rotation: Optional rotation from frame to child frame given as quaternion :param frame: Origin TF frame of this Transform :param child_frame: Target frame for this Transform + :param time: The time at which this Transform is valid, as ROS time """ super().__init__() if translation: From 3445c49302890bd22ac04a6b924112cc84d1b99a Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 25 Sep 2023 14:49:00 +0200 Subject: [PATCH 5/9] [bullet world] Optimization for set-joint and get-joint --- src/pycram/bullet_world.py | 39 ++++++++++++++++++++++++-------------- 1 file changed, 25 insertions(+), 14 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index dbf669507..1a4e5e3e9 100644 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -465,7 +465,6 @@ def run(self): for bulletworld_obj, shadow_obj in self.object_mapping.items(): b_pose = bulletworld_obj.get_pose() s_pose = shadow_obj.get_pose() - #if bulletworld_obj.get_pose() != shadow_obj.get_pose(): if b_pose.dist(s_pose) != 0.0: shadow_obj.set_pose(bulletworld_obj.get_pose()) @@ -473,7 +472,8 @@ def run(self): 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_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() @@ -522,10 +522,11 @@ def run(self): else: self.world.client_id = p.connect(p.GUI) - #Disable the side windows of the 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]) + p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, + cameraTargetPosition=[-2, 0, 1]) # Get the initial camera target location cameraTargetPosition = p.getDebugVisualizerCamera()[11] @@ -761,12 +762,14 @@ def __init__(self, name: str, type: str, path: str, 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 + BulletWorld.robot = self self.links[self.urdf_object.get_root()] = -1 self._current_pose = pose_in_map self._current_link_poses = {} + 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()) @@ -876,7 +879,6 @@ def get_pose(self) -> Pose: :return: The current pose of this object """ return self._current_pose - # return Pose(*p.getBasePositionAndOrientation(self.id, physicsClientId=self.world.client_id)) def set_pose(self, pose: Pose, base: bool = False) -> None: """ @@ -892,7 +894,6 @@ def set_pose(self, pose: Pose, base: bool = False) -> None: p.resetBasePositionAndOrientation(self.id, position, orientation, self.world.client_id) self._current_pose = pose_in_map self._update_link_poses() - # self.local_transformer.update_transforms_for_object(self) self._set_attached_objects([self]) @property @@ -1097,7 +1098,7 @@ 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 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: @@ -1120,6 +1121,7 @@ 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.local_transformer.update_transforms_for_object(self) self._update_link_poses() self._set_attached_objects([self]) @@ -1134,6 +1136,7 @@ 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._update_link_poses() self._set_attached_objects([self]) @@ -1144,7 +1147,7 @@ def get_joint_state(self, joint_name: str) -> float: :param joint_name: The name of the joint :return: The current pose of the joint """ - return p.getJointState(self.id, self.joints[joint_name], physicsClientId=self.world.client_id)[0] + return self._current_joint_states[joint_name] def contact_points(self) -> List: """ @@ -1345,10 +1348,7 @@ def get_complete_joint_state(self) -> Dict[str: float]: :return: A dictionary with the complete joint state """ - result = {} - for joint in self.joints.keys(): - result[joint] = self.get_joint_state(joint) - return result + return self._current_joint_states def get_link_tf_frame(self, link_name: str) -> str: """ @@ -1375,7 +1375,10 @@ 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_poses(self): + def _update_link_poses(self) -> None: + """ + Updates the cached poses 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 @@ -1383,6 +1386,14 @@ def _update_link_poses(self): self._current_link_poses[link_name] = Pose(*p.getLinkState(self.id, self.links[link_name], physicsClientId=self.world.client_id)[4:6]) + 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] = \ + p.getJointState(self.id, self.joints[joint_name], physicsClientId=self.world.client_id)[0] + def filter_contact_points(contact_points, exclude_ids) -> List: """ From 8269c8723ffe34712123ce9ea2e3d043ce8cf5f9 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 25 Sep 2023 14:49:35 +0200 Subject: [PATCH 6/9] [pr2-pms] MoveArm joints uses new set-joint-states method --- src/pycram/process_modules/pr2_process_modules.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index fbb6010fb..1bff40f01 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -173,18 +173,18 @@ def _execute(self, desig: MoveArmJointsMotion.Motion): robot = BulletWorld.robot if desig.right_arm_poses: - for joint, pose in desig.right_arm_poses.items(): - robot.set_joint_state(joint, pose) + robot.set_joint_states(desig.right_arm_poses) if desig.left_arm_poses: - for joint, pose in desig.left_arm_poses.items(): - robot.set_joint_state(joint, pose) + robot.set_joint_states(desig.left_arm_poses) 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 - for joint, pose in zip(desig.names, desig.positions): - robot.set_joint_state(joint, pose) + robot.set_joint_states(dict(zip(desig.names, desig.positions))) class Pr2WorldStateDetecting(ProcessModule): From 876ae78817965701c1092a8d8fcbe12825a42a81 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 25 Sep 2023 15:53:10 +0200 Subject: [PATCH 7/9] [doc] Extened install tutorial --- doc/source/installation.rst | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/doc/source/installation.rst b/doc/source/installation.rst index 5d7f7c9c8..8a0147938 100644 --- a/doc/source/installation.rst +++ b/doc/source/installation.rst @@ -14,6 +14,9 @@ All dependencies are available via PyPi. PyCRAM is developed and tested currently with Python3.8, Ubuntu 20.04 and ROS Noetic. +This guide excpects you to have a GitHub account with an SSH key (you can read about adding a new ssh key +`here `_). + Installing ROS ============== @@ -66,6 +69,8 @@ Now you can install PyCRAM into your ROS workspace. rosdep install --ignore-src --from-paths . -r cd .. catkin_make + source devel/setup.bash + echo "~/workspace/ros/devel/setup.bash" >> ~/.bashrc The cloning and setting up can take several minutes. After the command finishes you should see a number of repositories in your ROS workspace. @@ -112,13 +117,15 @@ Building and sourcing your ROS workspace using catkin compiles all ROS packages respective PATH variables. This is necessary to be able to import PyCRAM via the Python import system and to find the robot descriptions in the launch file. +If you have been following the tutorial steps until now you can skip this part. + You can build your ROS workspace with the following commands: .. code-block:: console cd ~/workspace/ros catkin_make - source devel/local_setup.bash + source devel/setup.bash Using PyCRAM ============ From 080eca30c192a1a8a49504d46d5847bd9909c6eb Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 25 Sep 2023 15:55:11 +0200 Subject: [PATCH 8/9] [demos] deleted unnecessary run file --- demos/pycram_bullet_world_demo/run.py | 5 ----- 1 file changed, 5 deletions(-) delete mode 100644 demos/pycram_bullet_world_demo/run.py diff --git a/demos/pycram_bullet_world_demo/run.py b/demos/pycram_bullet_world_demo/run.py deleted file mode 100644 index 48d43c65d..000000000 --- a/demos/pycram_bullet_world_demo/run.py +++ /dev/null @@ -1,5 +0,0 @@ -import sys -import os -sys.path.append(os.getcwd() + "/../../src/") -import macropy.activate -import demo From 294def709078e7e057608c1e1e15cff90998c5b2 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 25 Sep 2023 16:54:14 +0200 Subject: [PATCH 9/9] [bullet/local transformer] Caching of transforms in Objects --- src/pycram/bullet_world.py | 6 +++++- src/pycram/local_transformer.py | 13 +++---------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 1a4e5e3e9..bcaf4a06d 100644 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -768,6 +768,7 @@ def __init__(self, name: str, type: str, 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._update_link_poses() @@ -1377,14 +1378,17 @@ def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]: def _update_link_poses(self) -> None: """ - Updates the cached poses for each link of this Object + 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_joint_states(self) -> None: """ diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 2f1099b88..28a0a5710 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -129,16 +129,9 @@ def update_transforms_for_object(self, bullet_object: 'bullet_world.Object', tim :param time: a specific time that should be used """ time = time if time else rospy.Time.now() - obj_transform = bullet_object.get_pose().to_transform(bullet_object.tf_frame) - obj_transform.header.stamp = time - self.setTransform(obj_transform) - for link_name, id in bullet_object.links.items(): - if id == -1: - continue - tf_stamped = bullet_object.get_link_pose(link_name).to_transform( - bullet_object.get_link_tf_frame(link_name)) - tf_stamped.header.stamp = time - self.setTransform(tf_stamped) + for transform in bullet_object._current_link_transforms.values(): + transform.header.stamp = time + self.setTransform(transform) def get_all_frames(self) -> List[str]: """