From ae10d6f2da43613f94c350ce72edba19f7ecd02d Mon Sep 17 00:00:00 2001 From: Nils Leusmann Date: Thu, 18 Jan 2024 14:29:40 +0100 Subject: [PATCH 1/3] Updates the reachability_validator to check all tool_frames for reachabilty, keeps the retrace pose in mind --- src/pycram/pose_generator_and_validator.py | 112 +++++++++------------ 1 file changed, 48 insertions(+), 64 deletions(-) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index f959fd126..0b39af7ee 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -6,7 +6,9 @@ from .bullet_world import Object, BulletWorld, Use_shadow_world from .bullet_world_reasoning import contact from .costmaps import Costmap +from .local_transformer import LocalTransformer from .pose import Pose, Transform +from .robot_description import ManipulatorDescription from .robot_descriptions import robot_description from .external_interfaces.ik import request_ik from .plan_failures import IKError @@ -70,7 +72,7 @@ def generate_orientation(position: List[float], origin: Pose) -> List[float]: robot should face. :return: A quaternion of the calculated orientation """ - angle = np.arctan2(position[1]-origin.position.y, position[0]-origin.position.x) + np.pi + angle = np.arctan2(position[1] - origin.position.y, position[0] - origin.position.x) + np.pi quaternion = list(tf.transformations.quaternion_from_euler(0, 0, angle, axes="sxyz")) return quaternion @@ -134,74 +136,56 @@ def reachability_validator(pose: Pose, target = target.get_pose() robot.set_pose(pose) - - left_gripper = robot_description.get_tool_frame('left') - right_gripper = robot_description.get_tool_frame('right') - - # left_joints = robot_description._safely_access_chains('left').joints - left_joints = robot_description.chains['left'].joints - # right_joints = robot_description._safely_access_chains('right').joints - right_joints = robot_description.chains['right'].joints - # TODO Make orientation adhere to grasping orientation - res = False - arms = [] + for name, chain in robot_description.chains.items(): + if isinstance(chain, ManipulatorDescription): + retract_traget_pose = LocalTransformer().transform_pose(target,robot.get_link_tf_frame(chain.tool_frame)) + #BulletWorld.robot.get_link_tf_frame(robot_description.get_tool_frame(chain.tool_frame))) + retract_traget_pose.position.x -= 0.07 # Care hard coded value copied from PlaceAction class + + joints = robot_description.chains[chain.name].joints + # TODO Make orientation adhere to grasping orientation + res = False + arms = [] + in_contact = False + + joint_state_before_ik = robot._current_joint_states + try: + # test the possible solution and apply it to the robot + resp = request_ik(target, robot, chain.joints, chain.tool_frame) + _apply_ik(robot, resp, joints) + + in_contact = collision_check(robot, allowed_collision) + if not in_contact: # only check for retract pose if pose worked + resp = request_ik(retract_traget_pose, robot, chain.joints, chain.tool_frame) + _apply_ik(robot, resp, joints) + in_contact = collision_check(robot, allowed_collision) + if not in_contact: + arms.append(chain.name) + res = True + except IKError: + pass + finally: + robot.set_joint_states(joint_state_before_ik) + + return res, arms + + +def collision_check(robot, allowed_collision): in_contact = False - allowed_robot_links = [] if robot in allowed_collision.keys(): allowed_robot_links = allowed_collision[robot] - joint_state_before_ik=robot._current_joint_states - 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_bullet_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] in allowed_robot_links or link[1] in allowed_links: - in_contact = False - - if not in_contact: - arms.append("left") - res = True - except IKError: - pass - finally: - robot.set_joint_states(joint_state_before_ik) - - 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_bullet_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: + for obj in BulletWorld.current_bullet_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 link[0] in allowed_robot_links or link[1] in allowed_links: - in_contact = False + if in_contact: + for link in contact_links: - if not in_contact: - arms.append("right") - res = True - except IKError: - pass - finally: - robot.set_joint_states(joint_state_before_ik) + if link[0] in allowed_robot_links or link[1] in allowed_links: + in_contact = False - return res, arms + return in_contact From 685025a76be4ce2ab7b1be73acdebf8adb7d2c44 Mon Sep 17 00:00:00 2001 From: Nils Leusmann Date: Fri, 26 Jan 2024 10:03:22 +0100 Subject: [PATCH 2/3] The IK Server seems to have troubles if the retract_target_pose is not in map frame + bug fixing --- src/pycram/pose_generator_and_validator.py | 28 ++++++++++++---------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 0b39af7ee..02f74db2a 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -136,38 +136,42 @@ def reachability_validator(pose: Pose, target = target.get_pose() robot.set_pose(pose) + res = False + arms = [] for name, chain in robot_description.chains.items(): if isinstance(chain, ManipulatorDescription): - retract_traget_pose = LocalTransformer().transform_pose(target,robot.get_link_tf_frame(chain.tool_frame)) - #BulletWorld.robot.get_link_tf_frame(robot_description.get_tool_frame(chain.tool_frame))) - retract_traget_pose.position.x -= 0.07 # Care hard coded value copied from PlaceAction class + retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame(chain.tool_frame)) + retract_target_pose.position.x -= 0.07 # Care hard coded value copied from PlaceAction class + + # retract_pose needs to be in world frame? + retract_target_pose = LocalTransformer().transform_pose(retract_target_pose, "map") + + joints = robot_description.chains[name].joints + tool_frame = robot_description.get_tool_frame(name) - joints = robot_description.chains[chain.name].joints # TODO Make orientation adhere to grasping orientation - res = False - arms = [] in_contact = False joint_state_before_ik = robot._current_joint_states try: # test the possible solution and apply it to the robot - resp = request_ik(target, robot, chain.joints, chain.tool_frame) + resp = request_ik(target, robot, joints, tool_frame) _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: # only check for retract pose if pose worked - resp = request_ik(retract_traget_pose, robot, chain.joints, chain.tool_frame) + resp = request_ik(retract_target_pose, robot, joints, tool_frame) _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: - arms.append(chain.name) - res = True + arms.append(name) except IKError: pass finally: robot.set_joint_states(joint_state_before_ik) - - return res, arms + if arms: + res = True + return res, arms def collision_check(robot, allowed_collision): From 37de05480c69d7266df3467ec736e6fd30ec6989 Mon Sep 17 00:00:00 2001 From: Nils Leusmann Date: Wed, 3 Apr 2024 12:00:36 +0200 Subject: [PATCH 3/3] Adds documentation strings for new functions --- src/pycram/pose_generator_and_validator.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index e2fcf7aed..74366b6d3 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -140,7 +140,8 @@ def reachability_validator(pose: Pose, should be validated. :param target: The target position or object instance which should be the target for reachability. - :param allowed_collision: + :param allowed_collision: dict of objects with which the robot is allowed to collide each + object correlates to a list of links of which this object consists :return: True if the target is reachable for the robot and False in any other case. """ @@ -189,7 +190,22 @@ def reachability_validator(pose: Pose, return res, arms -def collision_check(robot, allowed_collision): +def collision_check(robot: Object, allowed_collision: list): + """ + This method checks if a given robot collides with any object within the world + which it is not allowed to collide with. + This is done checking iterating over every object within the world and checking + if the robot collides with it. Careful the floor will be ignored. + If there is a collision with an object that was not within the allowed collision + list the function returns True else it will return False + + :param robot: The robot object in the (Bullet)World where it should be checked if it collides + with something + :param allowed_collision: dict of objects with which the robot is allowed to collide each + object correlates to a list of links of which this object consists + :return: True if the target is reachable for the robot and False in any other + case. + """ in_contact = False allowed_robot_links = [] if robot in allowed_collision.keys():