From 99f745c231cf0e67f6a44a37e8a43757cc2972f0 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 11:36:14 +0200 Subject: [PATCH 01/10] fixed move_base of tests --- test/utils_for_tests.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/test/utils_for_tests.py b/test/utils_for_tests.py index 792e5473b4..72e67b4ff7 100644 --- a/test/utils_for_tests.py +++ b/test/utils_for_tests.py @@ -874,9 +874,13 @@ def check_cpi_leq(self, links, distance_threshold, check_external=True, check_se f'({min_contact.original_link_a} with {min_contact.original_link_b})' def move_base(self, goal_pose) -> None: - tip = self.get_odometry_joint().parent_link_name - self.motion_goals.add_cartesian_pose(goal_pose, tip_link=tip.short_name, root_link='map') - self.plan_and_execute() + tip = self.get_odometry_joint().child_link_name + monitor = self.monitors.add_cartesian_pose(goal_pose=goal_pose, tip_link=tip.short_name, root_link='map', + name='base goal') + self.motion_goals.add_cartesian_pose(goal_pose=goal_pose, tip_link=tip.short_name, root_link='map', + name='base goal', + end_condition=monitor) + self.execute() def reset(self): pass From d6964e066efc42d886a6d24634d1faf791db7bf4 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 14:43:54 +0200 Subject: [PATCH 02/10] fixed bug, where setting default collision avoidance in config had no effect --- .../configs/collision_avoidance_config.py | 23 +++++++++++++------ 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/giskardpy/configs/collision_avoidance_config.py b/src/giskardpy/configs/collision_avoidance_config.py index 71a8f0a567..e160db92d7 100644 --- a/src/giskardpy/configs/collision_avoidance_config.py +++ b/src/giskardpy/configs/collision_avoidance_config.py @@ -1,6 +1,7 @@ from __future__ import annotations import abc +from copy import copy from typing import Dict, Optional, List, Union, DefaultDict, Tuple from giskardpy.exceptions import SetupException @@ -98,13 +99,19 @@ def set_default_external_collision_avoidance(self, :param hard_threshold: distance threshold not allowed to be violated :param max_velocity: how fast it will move away from collisions """ + new_default = CollisionAvoidanceThresholds( + number_of_repeller=number_of_repeller, + soft_threshold=soft_threshold, + hard_threshold=hard_threshold, + max_velocity=max_velocity + ) + # overwrite the default of default + old_default = self.collision_scene.collision_avoidance_configs.default_factory() + old_default.external_collision_avoidance.default_factory = lambda: copy(new_default) + self.collision_scene.collision_avoidance_configs.default_factory = lambda: copy(old_default) + # overwrite the defaults of existing entries for config in self.collision_scene.collision_avoidance_configs.values(): - config.external_collision_avoidance.default_factory = lambda: CollisionAvoidanceThresholds( - number_of_repeller=number_of_repeller, - soft_threshold=soft_threshold, - hard_threshold=hard_threshold, - max_velocity=max_velocity - ) + config.external_collision_avoidance.default_factory = lambda: copy(new_default) def overwrite_external_collision_avoidance(self, joint_name: str, @@ -173,7 +180,8 @@ def load_self_collision_matrix(self, path_to_srdf: str, group_name: Optional[str if group_name not in self.collision_scene.self_collision_matrix_cache: self.collision_scene.load_self_collision_matrix_from_srdf(path_to_srdf, group_name) else: - path_to_srdf, self_collision_matrix, disabled_links = self.collision_scene.self_collision_matrix_cache[group_name] + path_to_srdf, self_collision_matrix, disabled_links = self.collision_scene.self_collision_matrix_cache[ + group_name] self.collision_scene.self_collision_matrix = self_collision_matrix self.collision_scene.disabled_links = disabled_links @@ -209,6 +217,7 @@ def setup(self): def _sanity_check(self): pass + class DisableCollisionAvoidanceConfig(CollisionAvoidanceConfig): def __init__(self): super().__init__(CollisionCheckerLib.none) From 6e0690baa7db3ce0a4a0d2345954bdd13d018eb6 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 19 Jun 2024 14:52:28 +0200 Subject: [PATCH 03/10] quaternion() in unit is more stable to avoid triggering bug in reference code --- test/test_cas_wrapper.py | 1 - test/utils_for_tests.py | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/test/test_cas_wrapper.py b/test/test_cas_wrapper.py index 69817772fb..5dcb449753 100644 --- a/test/test_cas_wrapper.py +++ b/test/test_cas_wrapper.py @@ -16,7 +16,6 @@ from giskardpy import casadi_wrapper as cas from giskardpy.qp import pos_in_vel_limits as cas2 import giskardpy.utils.math as giskard_math -from giskardpy.data_types import Derivatives from giskardpy.utils.math import compare_orientations, axis_angle_from_quaternion, rotation_matrix_from_quaternion from utils_for_tests import float_no_nan_no_inf, unit_vector, quaternion, vector, \ pykdl_frame_to_numpy, lists_of_same_length, random_angle, compare_axis_angle, angle_positive, sq_matrix, \ diff --git a/test/utils_for_tests.py b/test/utils_for_tests.py index 72e67b4ff7..5df8f62bce 100644 --- a/test/utils_for_tests.py +++ b/test/utils_for_tests.py @@ -188,8 +188,8 @@ def normalize(v): return st.builds(normalize, vector) -def quaternion(elements=None): - return unit_vector(4, elements) +def quaternion(): + return unit_vector(4, float_no_nan_no_inf(outer_limit=1)) def pykdl_frame_to_numpy(pykdl_frame): From c3c4e9ac586c66f6e2024b940b1a0e223ddefbfd Mon Sep 17 00:00:00 2001 From: Simon Date: Mon, 24 Jun 2024 13:32:36 +0200 Subject: [PATCH 04/10] added exceptions for force torque error codes --- src/giskardpy/exceptions.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/giskardpy/exceptions.py b/src/giskardpy/exceptions.py index f8da4b3e1b..499fac05b3 100644 --- a/src/giskardpy/exceptions.py +++ b/src/giskardpy/exceptions.py @@ -237,3 +237,24 @@ class ExecutionSucceededPrematurely(ExecutionException): @GiskardException.register_error_code(GiskardError.BEHAVIOR_TREE_ERROR) class BehaviorTreeException(GiskardException): pass + + +# %% force torque exceptions +@GiskardException.register_error_code(GiskardError.FORCE_TORQUE_MONITOR_ERROR) +class ForceTorqueExceptions(GiskardException): + pass + + +@GiskardException.register_error_code(GiskardError.FORCE_TORQUE_MONITOR_GRASPING_MISSED_OBJECT) +class ForceTorqueMonitorGraspsingMissedObjectExceptions(GiskardException): + pass + + +@GiskardException.register_error_code(GiskardError.FORCE_TORQUE_MONITOR_TRANSPORTING_LOST_OBJECT) +class ForceTorqueTransportingLostObjectExceptions(GiskardException): + pass + + +@GiskardException.register_error_code(GiskardError.FORCE_TORQUE_MONITOR_PLACING_MISSED_PLACING_LOCATION) +class ForceTorquePlacingMissedPlacingLocationExceptions(GiskardException): + pass From d19ba597cb76928f69367043d71eb1c115ad00ed Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 26 Jun 2024 14:17:29 +0200 Subject: [PATCH 05/10] first version of FollowPointPath --- src/giskardpy/configs/behavior_tree_config.py | 6 +- src/giskardpy/goals/base_traj_follower.py | 483 +++++++++++++++++- test/test_integration_pr2.py | 40 ++ 3 files changed, 526 insertions(+), 3 deletions(-) diff --git a/src/giskardpy/configs/behavior_tree_config.py b/src/giskardpy/configs/behavior_tree_config.py index d8a9d9682a..85c4407dba 100644 --- a/src/giskardpy/configs/behavior_tree_config.py +++ b/src/giskardpy/configs/behavior_tree_config.py @@ -141,6 +141,10 @@ def add_tf_publisher(self, include_prefix: bool = True, tf_topic: str = 'tf', self.tree.wait_for_goal.publish_state.add_tf_publisher(include_prefix=include_prefix, tf_topic=tf_topic, mode=mode) + if god_map.is_standalone(): + self.tree.control_loop_branch.publish_state.add_tf_publisher(include_prefix=include_prefix, + tf_topic=tf_topic, + mode=mode) def add_evaluate_debug_expressions(self): self.tree.prepare_control_loop.add_compile_debug_expressions() @@ -193,7 +197,7 @@ def __init__(self, """ self.include_prefix = include_prefix if is_running_in_pytest(): - publish_tf = False + # publish_tf = False publish_js = False if god_map.is_in_github_workflow(): debug_mode = False diff --git a/src/giskardpy/goals/base_traj_follower.py b/src/giskardpy/goals/base_traj_follower.py index a8b2a8f208..45d209bbd6 100644 --- a/src/giskardpy/goals/base_traj_follower.py +++ b/src/giskardpy/goals/base_traj_follower.py @@ -1,4 +1,4 @@ -from __future__ import division +from __future__ import division, annotations from typing import Optional, List, Dict, Tuple @@ -6,6 +6,7 @@ # import matplotlib.pyplot as plt import rospy from geometry_msgs.msg import PointStamped, Vector3, Point +from nav_msgs.msg import Path from sensor_msgs.msg import LaserScan from visualization_msgs.msg import MarkerArray, Marker @@ -164,6 +165,485 @@ def add_rot_constraints(self): name='/rot') +class FollowPointPath(Goal): + trajectory: np.ndarray + traj_data: List[np.ndarray] + laser_thresholds: Dict[int, np.ndarray] + pub: rospy.Publisher = None + laser_subs: List[rospy.Subscriber] + last_scan: Dict[int, LaserScan] + odom_joint: OmniDrive + + # class LaserSub: + # def __init__(self, host: FollowPointPath, id: int): + # self.id = id + # self.host = host + # + # def __call__(self, ): + + def __init__(self, + name: str, + path: Path, + laser_topics: Tuple[str] = ('/hsrb/base_scan',), + odom_joint_name: Optional[str] = None, + root_link: Optional[str] = None, + camera_link: str = 'head_rgbd_sensor_link', + distance_to_target_stop_threshold: float = 1, + laser_scan_age_threshold: float = 2, + laser_distance_threshold: float = 0.5, + laser_distance_threshold_width: float = 0.8, + laser_avoidance_angle_cutout: float = np.pi / 4, + laser_avoidance_sideways_buffer: float = 0.04, + base_orientation_threshold: float = np.pi / 16, + max_rotation_velocity: float = 0.5, + max_rotation_velocity_head: float = 1, + max_translation_velocity: float = 0.38, + traj_tracking_radius: float = 0.4, + height_for_camera_target: float = 1, + laser_frame_id: str = 'base_range_sensor_link', + start_condition: cas.Expression = cas.TrueSymbol, + hold_condition: cas.Expression = cas.FalseSymbol, + end_condition: cas.Expression = cas.FalseSymbol): + super().__init__(name=name) + self.path_to_trajectory(path=path) + self.end_of_traj_reached = False + self.laser_thresholds = {} + self.last_scan = {} + self.enable_laser_avoidance = len(laser_topics) > 0 + if FollowPointPath.pub is None: + FollowPointPath.pub = rospy.Publisher('~visualization_marker_array', MarkerArray) + self.laser_topics = list(laser_topics) + self.laser_distance_threshold_width = laser_distance_threshold_width / 2 + self.closest_laser_left = [self.laser_distance_threshold_width] * len(self.laser_topics) + self.closest_laser_right = [-self.laser_distance_threshold_width] * len(self.laser_topics) + self.closest_laser_reading = [0] * len(self.laser_topics) + self.laser_frame = laser_frame_id + # self.last_scan = LaserScan() + # self.last_scan.header.stamp = rospy.get_rostime() + # self.last_scan_pc = LaserScan() + # self.last_scan_pc.header.stamp = rospy.get_rostime() + self.laser_scan_age_threshold = laser_scan_age_threshold + self.laser_avoidance_angle_cutout = laser_avoidance_angle_cutout + self.laser_avoidance_sideways_buffer = laser_avoidance_sideways_buffer + self.base_orientation_threshold = base_orientation_threshold + if odom_joint_name is None: + self.odom_joint = god_map.world.search_for_joint_of_type(OmniDrive)[0] + self.odom_joint_name = self.odom_joint.name + else: + self.odom_joint_name = god_map.world.search_for_joint_name(odom_joint_name) + self.odom_joint = god_map.world.get_joint(self.odom_joint_name) + if root_link is None: + self.root = god_map.world.root_link_name + else: + self.root = god_map.world.search_for_link_name(root_link) + self.camera_link = god_map.world.search_for_link_name(camera_link) + self.tip_V_camera_axis = Vector3() + self.tip_V_camera_axis.z = 1 + self.tip = self.odom_joint.child_link_name + self.odom = self.odom_joint.parent_link_name + self.tip_V_pointing_axis = Vector3() + self.tip_V_pointing_axis.x = 1 + self.max_rotation_velocity = max_rotation_velocity + self.max_rotation_velocity_head = max_rotation_velocity_head + self.max_translation_velocity = max_translation_velocity + self.weight = WEIGHT_BELOW_CA + self.min_distance_to_target = distance_to_target_stop_threshold + self.laser_distance_threshold = laser_distance_threshold + self.traj_tracking_radius = traj_tracking_radius + self.interpolation_step_size = 0.05 + self.max_temp_distance = int(self.traj_tracking_radius / self.interpolation_step_size) + self.human_point = PointStamped() + self.height_for_camera_target = height_for_camera_target + self.laser_subs = [] + for i, laser_topic in enumerate(self.laser_topics): + cb = lambda scan: self.laser_cb(scan, i) + self.laser_subs.append(rospy.Subscriber(laser_topic, LaserScan, cb, queue_size=10)) + self.publish_tracking_radius() + self.publish_distance_to_target() + self.publish_trajectory() + + # %% real shit + root_T_bf = god_map.world.compose_fk_expression(self.root, self.tip) + root_T_odom = god_map.world.compose_fk_expression(self.root, self.odom) + root_T_camera = god_map.world.compose_fk_expression(self.root, self.camera_link) + root_P_bf = root_T_bf.to_position() + + if self.enable_laser_avoidance: + closest_laser_left = symbol_manager.get_symbol(self + f'.closest_laser_left[0]') + closest_laser_right = symbol_manager.get_symbol(self + f'.closest_laser_right[0]') + closest_laser_reading = symbol_manager.get_symbol(self + f'.closest_laser_reading[0]') + for laser_id in range(1, len(self.laser_subs)): + next_min_left_violation = symbol_manager.get_symbol(self + f'.closest_laser_left[{laser_id}]') + next_min_right_violation = symbol_manager.get_symbol(self + f'.closest_laser_right[{laser_id}]') + next_closest_laser_reading = symbol_manager.get_symbol(self + f'.closest_laser_reading[{laser_id}]') + closest_laser_left = cas.min(closest_laser_left, next_min_left_violation) + closest_laser_right = cas.max(closest_laser_right, next_min_right_violation) + closest_laser_reading = cas.min(closest_laser_reading, next_closest_laser_reading) + + next_x = symbol_manager.get_symbol(self + '.get_current_target()[\'next_x\']') + next_y = symbol_manager.get_symbol(self + '.get_current_target()[\'next_y\']') + closest_x = symbol_manager.get_symbol(self + '.get_current_target()[\'closest_x\']') + closest_y = symbol_manager.get_symbol(self + '.get_current_target()[\'closest_y\']') + clear_memo(self.get_current_target) + root_P_goal_point = cas.Point3([next_x, next_y, 0]) + root_P_closest_point = cas.Point3([closest_x, closest_y, 0]) + tip_V_pointing_axis = cas.Vector3(self.tip_V_pointing_axis) + + map_P_human = root_P_goal_point + + # %% orient to goal + orient_to_goal = self.create_and_add_task('orient to goal') + _, _, map_odom_angle = root_T_odom.to_rotation().to_rpy() + odom_current_angle = self.odom_joint.yaw.get_symbol(Derivatives.position) + map_current_angle = map_odom_angle + odom_current_angle + root_V_tip_to_closest = root_P_bf - root_P_closest_point + root_P_between_tip_and_closest = root_P_closest_point + root_V_tip_to_closest / 2 + root_V_goal_axis = root_P_goal_point - root_P_between_tip_and_closest + root_V_goal_axis2 = cas.Vector3(root_V_goal_axis) + root_V_goal_axis2.scale(1) + map_P_human = map_P_human + root_V_goal_axis2 * 1.5 + root_V_goal_axis.scale(1) + root_V_pointing_axis = root_T_bf.dot(tip_V_pointing_axis) + root_V_pointing_axis.vis_frame = self.tip + root_V_goal_axis.vis_frame = self.tip + map_goal_angle = cas.angle_between_vector(cas.Vector3([1, 0, 0]), root_V_goal_axis) + map_goal_angle = cas.if_greater(root_V_goal_axis.y, 0, map_goal_angle, -map_goal_angle) + map_angle_error = cas.shortest_angular_distance(map_current_angle, map_goal_angle) + buffer = 0 + ll = map_angle_error - buffer + ul = map_angle_error + buffer + orient_to_goal.add_inequality_constraint(reference_velocity=self.max_rotation_velocity, + lower_error=ll, + upper_error=ul, + weight=self.weight, + task_expression=map_current_angle, + name='rot') + + # %% look at goal + camera_V_camera_axis = cas.Vector3(self.tip_V_camera_axis) + root_V_camera_axis = root_T_camera.dot(camera_V_camera_axis) + root_P_camera = root_T_camera.to_position() + map_P_human.z = self.height_for_camera_target + root_V_camera_goal_axis = map_P_human - root_P_camera + root_V_camera_goal_axis.scale(1) + look_at_target = self.create_and_add_task('look at target') + look_at_target.add_vector_goal_constraints(frame_V_current=root_V_camera_axis, + frame_V_goal=root_V_camera_goal_axis, + reference_velocity=self.max_rotation_velocity_head, + weight=self.weight, + name='move camera') + + # %% follow next point + follow_next_point = self.create_and_add_task('follow next') + root_V_camera_axis.vis_frame = self.camera_link + root_V_camera_goal_axis.vis_frame = self.camera_link + + laser_violated = ExpressionMonitor(name='laser violated') + self.add_monitor(laser_violated) + laser_violated.expression = cas.less(closest_laser_reading, 0) + oriented_towards_next = ExpressionMonitor(name='oriented towards next') + self.add_monitor(oriented_towards_next) + oriented_towards_next.expression = cas.abs(map_angle_error) > self.base_orientation_threshold + + follow_next_point.hold_condition = (laser_violated.get_state_expression() + | oriented_towards_next.get_state_expression()) + follow_next_point.add_point_goal_constraints(frame_P_current=root_P_bf, + frame_P_goal=root_P_goal_point, + reference_velocity=self.max_translation_velocity, + weight=self.weight, + name='min dist to next') + god_map.debug_expression_manager.add_debug_expression('root_P_goal_point', root_P_goal_point) + + # %% keep the closest point in footprint radius + stay_in_circle = self.create_and_add_task('in circle') + buffer = self.traj_tracking_radius + distance_to_closest_point = cas.norm(root_P_closest_point - root_P_bf) + stay_in_circle.add_inequality_constraint(task_expression=distance_to_closest_point, + lower_error=-distance_to_closest_point - buffer, + upper_error=-distance_to_closest_point + buffer, + reference_velocity=self.max_translation_velocity, + weight=self.weight, + name='stay in circle') + god_map.debug_expression_manager.add_debug_expression('root_P_closest_point', root_P_closest_point) + + # %% laser avoidance + if self.enable_laser_avoidance: + laser_avoidance_task = self.create_and_add_task('laser avoidance') + sideways_vel = (closest_laser_left + closest_laser_right) + bf_V_laser_avoidance_direction = cas.Vector3([0, sideways_vel, 0]) + map_V_laser_avoidance_direction = root_T_bf.dot(bf_V_laser_avoidance_direction) + map_V_laser_avoidance_direction.vis_frame = god_map.world.search_for_link_name(self.laser_frame) + god_map.debug_expression_manager.add_debug_expression('base_V_laser_avoidance_direction', + map_V_laser_avoidance_direction) + odom_y_vel = self.odom_joint.y_vel.get_symbol(Derivatives.position) + + active = ExpressionMonitor(name='too far from path') + self.add_monitor(active) + active.expression = cas.greater(distance_to_closest_point, self.traj_tracking_radius) + + buffer = self.laser_avoidance_sideways_buffer / 2 + + laser_avoidance_task.hold_condition = active.get_state_expression() + laser_avoidance_task.add_inequality_constraint(reference_velocity=self.max_translation_velocity, + lower_error=sideways_vel - buffer, + upper_error=sideways_vel + buffer, + weight=WEIGHT_COLLISION_AVOIDANCE, + task_expression=odom_y_vel, + name='move sideways') + + last_point = cas.Point3([self.trajectory[-1][0], self.trajectory[-1][1], 0]) + goal_reached = ExpressionMonitor(name='goal reached?') + self.add_monitor(goal_reached) + goal_reached.expression = cas.euclidean_distance(last_point, root_P_bf) < 0.03 + self.connect_end_condition_to_all_tasks(goal_reached.get_state_expression()) + end = EndMotion(name='done') + end.start_condition = goal_reached.get_state_expression() + self.add_monitor(end) + + self.connect_start_condition_to_all_tasks(start_condition) + self.connect_hold_condition_to_all_tasks(hold_condition) + self.connect_end_condition_to_all_tasks(end_condition) + + def path_to_trajectory(self, path: Path): + self.traj_data = [] + for p in path.poses: + self.traj_data.append(np.array([p.pose.position.x, p.pose.position.y])) + self.trajectory = np.array(self.traj_data) + + def clean_up(self): + for sub in self.laser_subs: + sub.unregister() + + def init_laser_stuff(self, laser_scan: LaserScan): + thresholds = [] + if len(laser_scan.ranges) % 2 == 0: + print('laser range is even') + angles = np.arange(laser_scan.angle_min, + laser_scan.angle_max, + laser_scan.angle_increment)[:-1] + else: + angles = np.arange(laser_scan.angle_min, + laser_scan.angle_max, + laser_scan.angle_increment) + for angle in angles: + if angle < 0: + y = -self.laser_distance_threshold_width + length = y / np.sin((angle)) + x = np.cos(angle) * length + thresholds.append((x, y, length, angle)) + else: + y = self.laser_distance_threshold_width + length = y / np.sin((angle)) + x = np.cos(angle) * length + thresholds.append((x, y, length, angle)) + if length > self.laser_distance_threshold: + length = self.laser_distance_threshold + x = np.cos(angle) * length + y = np.sin(angle) * length + thresholds[-1] = (x, y, length, angle) + thresholds = np.array(thresholds) + assert len(thresholds) == len(laser_scan.ranges) + return thresholds + + def muddle_laser_scan(self, scan: LaserScan, thresholds: np.ndarray): + data = np.array(scan.ranges) + xs = np.cos(thresholds[:, 3]) * data + ys = np.sin(thresholds[:, 3]) * data + violations = data < thresholds[:, 2] + xs_error = xs - thresholds[:, 0] + half = int(data.shape[0] / 2) + # x_positive = np.where(thresholds[:, 0] > 0)[0] + x_below_laser_avoidance_threshold1 = thresholds[:, -1] > self.laser_avoidance_angle_cutout + x_below_laser_avoidance_threshold2 = thresholds[:, -1] < -self.laser_avoidance_angle_cutout + x_below_laser_avoidance_threshold = x_below_laser_avoidance_threshold1 | x_below_laser_avoidance_threshold2 + y_filter = x_below_laser_avoidance_threshold & violations + closest_laser_right = ys[:half][y_filter[:half]] + closest_laser_left = ys[half:][y_filter[half:]] + + x_positive = np.where(np.invert(x_below_laser_avoidance_threshold))[0] + x_start = x_positive[0] + x_end = x_positive[-1] + + front_violation = xs_error[x_start:x_end][violations[x_start:x_end]] + if len(closest_laser_left) > 0: + closest_laser_left = min(closest_laser_left) + else: + closest_laser_left = self.laser_distance_threshold_width + if len(closest_laser_right) > 0: + closest_laser_right = max(closest_laser_right) + else: + closest_laser_right = -self.laser_distance_threshold_width + if len(front_violation) > 0: + closest_laser_reading = min(front_violation) + else: + closest_laser_reading = 0 + return closest_laser_reading, closest_laser_left, closest_laser_right + + def laser_cb(self, scan: LaserScan, id_: int): + self.last_scan[id_] = scan + if id_ not in self.laser_thresholds: + self.laser_thresholds[id_] = self.init_laser_stuff(scan) + self.publish_laser_thresholds() + closest, left, right = self.muddle_laser_scan(scan, self.laser_thresholds[id_]) + self.closest_laser_reading[id_] = closest + self.closest_laser_left[id_] = left + self.closest_laser_right[id_] = right + + def get_current_point(self) -> np.ndarray: + root_T_tip = god_map.world.compute_fk_np(self.root, self.tip) + x = root_T_tip[0, 3] + y = root_T_tip[1, 3] + return np.array([x, y]) + + @memoize_with_counter(4) + def get_current_target(self) -> Dict[str, float]: + if self.enable_laser_avoidance: + self.check_laser_scan_age() + traj = self.trajectory.copy() + current_point = self.get_current_point() + error = traj - current_point + distances = np.linalg.norm(error, axis=1) + # cut off old points + in_radius = np.where(distances < self.traj_tracking_radius)[0] + if len(in_radius) > 0: + next_idx = max(in_radius) + offset = max(0, next_idx - self.max_temp_distance) + closest_idx = np.argmin(distances[offset:]) + offset + else: + next_idx = closest_idx = np.argmin(distances) + # CarryMyBullshit.traj_data = CarryMyBullshit.traj_data[closest_idx:] + if closest_idx == self.trajectory.shape[0] - 1: + self.end_of_traj_reached = True + result = { + 'next_x': traj[next_idx, 0], + 'next_y': traj[next_idx, 1], + 'closest_x': traj[closest_idx, 0], + 'closest_y': traj[closest_idx, 1], + } + return result + + def check_laser_scan_age(self): + current_time = rospy.get_rostime().to_sec() + for id_, scan in self.last_scan.items(): + base_laser_age = current_time - scan.header.stamp.to_sec() + if base_laser_age > self.laser_scan_age_threshold: + logging.logwarn(f'last base laser scan is too old: {base_laser_age}') + self.closest_laser_left[id_] = self.laser_distance_threshold_width + self.closest_laser_right[id_] = -self.laser_distance_threshold_width + self.closest_laser_reading[id_] = 0 + + def publish_trajectory(self): + ms = MarkerArray() + m_line = Marker() + m_line.action = m_line.ADD + m_line.ns = 'traj' + m_line.id = 1 + m_line.type = m_line.LINE_STRIP + m_line.header.frame_id = str(god_map.world.root_link_name) + m_line.scale.x = 0.05 + m_line.color.a = 1 + m_line.color.r = 1 + try: + for item in self.trajectory: + p = Point() + p.x = item[0] + p.y = item[1] + m_line.points.append(p) + ms.markers.append(m_line) + except Exception as e: + logging.logwarn('failed to create traj marker') + self.pub.publish(ms) + + def publish_laser_thresholds(self): + ms = MarkerArray() + m_line = Marker() + m_line.action = m_line.ADD + m_line.ns = 'laser_thresholds' + m_line.id = 1332 + m_line.type = m_line.LINE_STRIP + m_line.header.frame_id = self.laser_frame + m_line.scale.x = 0.05 + m_line.color.a = 1 + m_line.color.r = 0.5 + m_line.color.b = 1 + m_line.frame_locked = True + for item in self.laser_thresholds[0]: + p = Point() + p.x = item[0] + p.y = item[1] + m_line.points.append(p) + ms.markers.append(m_line) + square = Marker() + square.action = m_line.ADD + square.ns = 'laser_avoidance_angle_cutout' + square.id = 1333 + square.type = m_line.LINE_STRIP + square.header.frame_id = self.laser_frame + # p = Point() + # p.x = self.thresholds[0, 0] + # p.y = self.thresholds[0, 1] + # square.points.append(p) + p = Point() + idx = np.where(self.laser_thresholds[0][:, -1] < -self.laser_avoidance_angle_cutout)[0][-1] + p.x = self.laser_thresholds[0][idx, 0] + p.y = self.laser_thresholds[0][idx, 1] + square.points.append(p) + p = Point() + square.points.append(p) + p = Point() + idx = np.where(self.laser_thresholds[0][:, -1] > self.laser_avoidance_angle_cutout)[0][0] + p.x = self.laser_thresholds[0][idx, 0] + p.y = self.laser_thresholds[0][idx, 1] + square.points.append(p) + # p = Point() + # p.x = self.thresholds[-1, 0] + # p.y = self.thresholds[-1, 1] + # square.points.append(p) + square.scale.x = 0.05 + square.color.a = 1 + square.color.r = 0.5 + square.color.b = 1 + square.frame_locked = True + ms.markers.append(square) + self.pub.publish(ms) + + def publish_tracking_radius(self): + ms = MarkerArray() + m_line = Marker() + m_line.action = m_line.ADD + m_line.ns = 'traj_tracking_radius' + m_line.id = 1332 + m_line.type = m_line.CYLINDER + m_line.header.frame_id = str(self.tip.short_name) + m_line.scale.x = self.traj_tracking_radius * 2 + m_line.scale.y = self.traj_tracking_radius * 2 + m_line.scale.z = 0.05 + m_line.color.a = 1 + m_line.color.b = 1 + m_line.pose.orientation.w = 1 + m_line.frame_locked = True + ms.markers.append(m_line) + self.pub.publish(ms) + + def publish_distance_to_target(self): + ms = MarkerArray() + m_line = Marker() + m_line.action = m_line.ADD + m_line.ns = 'distance_to_target_stop_threshold' + m_line.id = 1332 + m_line.type = m_line.CYLINDER + m_line.header.frame_id = str(self.tip.short_name) + m_line.scale.x = self.min_distance_to_target * 2 + m_line.scale.y = self.min_distance_to_target * 2 + m_line.scale.z = 0.01 + m_line.color.a = 0.5 + m_line.color.g = 1 + m_line.pose.orientation.w = 1 + m_line.frame_locked = True + ms.markers.append(m_line) + self.pub.publish(ms) + + class CarryMyBullshit(Goal): trajectory: np.ndarray = np.array([]) traj_data: List[np.ndarray] = None @@ -338,7 +818,6 @@ def __init__(self, # root_V_tangent = cas.Vector3([tangent.x, tangent.y, 0]) tip_V_pointing_axis = cas.Vector3(self.tip_V_pointing_axis) - if self.drive_back: map_P_human = root_P_goal_point else: diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index ec481fd854..4af2d6318a 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -10,6 +10,7 @@ import pytest import rospy from geometry_msgs.msg import PoseStamped, Point, Quaternion, Vector3Stamped, PointStamped, QuaternionStamped, Pose +from nav_msgs.msg import Path from numpy import pi from shape_msgs.msg import SolidPrimitive from tf.transformations import quaternion_from_matrix, quaternion_about_axis @@ -20,6 +21,7 @@ from giskardpy.configs.giskard import Giskard from giskardpy.configs.iai_robots.pr2 import PR2CollisionAvoidance, PR2StandaloneInterface, WorldWithPR2Config from giskardpy.configs.qp_controller_config import SupportedQPSolver, QPControllerConfig +from giskardpy.goals.base_traj_follower import FollowPointPath from giskardpy.goals.cartesian_goals import RelativePositionSequence from giskardpy.goals.caster import Circle, Wave from giskardpy.goals.collision_avoidance import CollisionAvoidanceHint @@ -162,6 +164,7 @@ def __init__(self, giskard: Optional[Giskard] = None): robot_interface_config=PR2StandaloneInterface(drive_joint_name=drive_joint_name), collision_avoidance_config=PR2CollisionAvoidance(drive_joint_name=drive_joint_name), behavior_tree_config=StandAloneBTConfig(debug_mode=True, + publish_tf=True, simulation_max_hz=None), # qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.gurobi)) qp_controller_config=QPControllerConfig()) @@ -1043,6 +1046,43 @@ def test_collision_avoidance_sequence(self, fake_table_setup: PR2TestWrapper): class TestConstraints: + def test_follow_nav_path(self, zero_pose: PR2TestWrapper): + path_msg = Path() + path_msg.header.frame_id = 'map' + + poses_data = [ + {'position': (3.4403343200683594, 2.349609851837158, 0.0), 'orientation': (0.0, 0.0, -0.9999553938074013, 0.009445125488052377)}, + {'position': (3.498216525117533, 2.3331048932770795, 0.0), 'orientation': (0.0, 0.0, 0.9993770281155905, 0.035292430843599024)}, + {'position': (3.5582080985686915, 2.3288624659763553, 0.0), 'orientation': (0.0, 0.0, 0.9996662404939319, 0.02583423342636984)}, + {'position': (3.6068967725310745, 2.326344275148637, 0.0), 'orientation': (0.0, 0.0, 0.9997591723006155, 0.021945327538867403)}, + {'position': (3.6598472962032886, 2.324018561554272, 0.0), 'orientation': (0.0, 0.0, 0.9992479949741707, 0.03877427678370992)}, + {'position': (3.6924761139448794, 2.3214825211531753, 0.0), 'orientation': (0.0, 0.0, 0.9991740059338614, 0.04063626294431946)}, + {'position': (3.727106939527923, 2.318660992860927, 0.0), 'orientation': (0.0, 0.0, 0.9991306638705609, 0.0416883258667487)}, + {'position': (3.7636721910961253, 2.3156043305022376, 0.0), 'orientation': (0.0, 0.0, 0.9990501048487416, 0.043576232073442425)}, + {'position': (3.8028157945033154, 2.31218311653614, 0.0), 'orientation': (0.0, 0.0, 0.9987543902336266, 0.04989657291895693)}, + {'position': (3.8460085061683724, 2.307856605808622, 0.0), 'orientation': (0.0, 0.0, 0.9975155500290784, 0.07044662838053373)}, + {'position': (3.8961558228154374, 2.3007380861823137, 0.0), 'orientation': (0.0, 0.0, 0.9969280676127491, 0.07832258937184026)}, + {'position': (3.9597676634174857, 2.2906808170884503, 0.0), 'orientation': (0.0, 0.0, -0.9986858657493689, 0.05124979563308978)}, + {'position': (4.03348337802564, 2.2982665669040685, 0.0), 'orientation': (0.0, 0.0, -0.9986858657493689, 0.05124979563308978)}, + ] + + for pose_data in poses_data: + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pose_data['position'] + pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = \ + pose_data['orientation'] + path_msg.poses.append(pose) + zero_pose.motion_goals.add_motion_goal(motion_goal_class=FollowPointPath.__name__, + name='follow', + camera_link='head_mount_kinect_rgb_optical_frame', + laser_frame_id='base_laser_link', + # laser_topics=[], + path=path_msg) + # local_min = zero_pose.monitors.add_local_minimum_reached() + # zero_pose.monitors.add_end_motion(local_min) + zero_pose.execute(add_local_minimum_reached=False) + # TODO write buggy constraints that test sanity checks def test_empty_problem(self, zero_pose: PR2TestWrapper): zero_pose.allow_all_collisions() From 6d792a25f51cb141c660360f9fb0f4497390aca4 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 26 Jun 2024 15:22:25 +0200 Subject: [PATCH 06/10] fixed bug where connect_end_condition_to_all_tasks overwrote all end conditions with if the default was set --- src/giskardpy/goals/goal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/giskardpy/goals/goal.py b/src/giskardpy/goals/goal.py index 27c44f49ec..095281ea1d 100644 --- a/src/giskardpy/goals/goal.py +++ b/src/giskardpy/goals/goal.py @@ -86,7 +86,7 @@ def connect_end_condition_to_all_tasks(self, condition: cas.Expression) -> None: for task in self.tasks: if cas.is_false(task.end_condition): task.end_condition = condition - else: + elif not cas.is_false(condition): task.end_condition = cas.logic_and(task.end_condition, condition) def connect_monitors_to_all_tasks(self, From e06c0759ad93ad0910073ca7179c29411ffcfd58 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 26 Jun 2024 15:22:38 +0200 Subject: [PATCH 07/10] updated follow point path --- src/giskardpy/goals/base_traj_follower.py | 92 ++++++++++++----------- test/test_integration_pr2.py | 69 +++++++++++++---- 2 files changed, 101 insertions(+), 60 deletions(-) diff --git a/src/giskardpy/goals/base_traj_follower.py b/src/giskardpy/goals/base_traj_follower.py index 45d209bbd6..a3bf51b039 100644 --- a/src/giskardpy/goals/base_traj_follower.py +++ b/src/giskardpy/goals/base_traj_follower.py @@ -174,13 +174,6 @@ class FollowPointPath(Goal): last_scan: Dict[int, LaserScan] odom_joint: OmniDrive - # class LaserSub: - # def __init__(self, host: FollowPointPath, id: int): - # self.id = id - # self.host = host - # - # def __call__(self, ): - def __init__(self, name: str, path: Path, @@ -205,7 +198,6 @@ def __init__(self, hold_condition: cas.Expression = cas.FalseSymbol, end_condition: cas.Expression = cas.FalseSymbol): super().__init__(name=name) - self.path_to_trajectory(path=path) self.end_of_traj_reached = False self.laser_thresholds = {} self.last_scan = {} @@ -218,10 +210,6 @@ def __init__(self, self.closest_laser_right = [-self.laser_distance_threshold_width] * len(self.laser_topics) self.closest_laser_reading = [0] * len(self.laser_topics) self.laser_frame = laser_frame_id - # self.last_scan = LaserScan() - # self.last_scan.header.stamp = rospy.get_rostime() - # self.last_scan_pc = LaserScan() - # self.last_scan_pc.header.stamp = rospy.get_rostime() self.laser_scan_age_threshold = laser_scan_age_threshold self.laser_avoidance_angle_cutout = laser_avoidance_angle_cutout self.laser_avoidance_sideways_buffer = laser_avoidance_sideways_buffer @@ -259,8 +247,7 @@ def __init__(self, cb = lambda scan: self.laser_cb(scan, i) self.laser_subs.append(rospy.Subscriber(laser_topic, LaserScan, cb, queue_size=10)) self.publish_tracking_radius() - self.publish_distance_to_target() - self.publish_trajectory() + self.path_to_trajectory(path=path) # %% real shit root_T_bf = god_map.world.compose_fk_expression(self.root, self.tip) @@ -338,15 +325,16 @@ def __init__(self, root_V_camera_axis.vis_frame = self.camera_link root_V_camera_goal_axis.vis_frame = self.camera_link - laser_violated = ExpressionMonitor(name='laser violated') - self.add_monitor(laser_violated) - laser_violated.expression = cas.less(closest_laser_reading, 0) oriented_towards_next = ExpressionMonitor(name='oriented towards next') self.add_monitor(oriented_towards_next) oriented_towards_next.expression = cas.abs(map_angle_error) > self.base_orientation_threshold - follow_next_point.hold_condition = (laser_violated.get_state_expression() - | oriented_towards_next.get_state_expression()) + follow_next_point.hold_condition = oriented_towards_next.get_state_expression() + if self.enable_laser_avoidance: + laser_violated = ExpressionMonitor(name='laser violated') + self.add_monitor(laser_violated) + laser_violated.expression = cas.less(closest_laser_reading, 0) + follow_next_point.hold_condition |= laser_violated.get_state_expression() follow_next_point.add_point_goal_constraints(frame_P_current=root_P_bf, frame_P_goal=root_P_goal_point, reference_velocity=self.max_translation_velocity, @@ -357,14 +345,14 @@ def __init__(self, # %% keep the closest point in footprint radius stay_in_circle = self.create_and_add_task('in circle') buffer = self.traj_tracking_radius - distance_to_closest_point = cas.norm(root_P_closest_point - root_P_bf) + hack = cas.Vector3([0, 0, 0.0001]) + distance_to_closest_point = cas.norm(root_P_closest_point + hack - root_P_bf) stay_in_circle.add_inequality_constraint(task_expression=distance_to_closest_point, lower_error=-distance_to_closest_point - buffer, upper_error=-distance_to_closest_point + buffer, reference_velocity=self.max_translation_velocity, weight=self.weight, name='stay in circle') - god_map.debug_expression_manager.add_debug_expression('root_P_closest_point', root_P_closest_point) # %% laser avoidance if self.enable_laser_avoidance: @@ -396,19 +384,53 @@ def __init__(self, self.add_monitor(goal_reached) goal_reached.expression = cas.euclidean_distance(last_point, root_P_bf) < 0.03 self.connect_end_condition_to_all_tasks(goal_reached.get_state_expression()) + + final_orientation = self.create_and_add_task('final orientation') + frame_R_current = root_T_bf.to_rotation() + current_R_frame_eval = god_map.world.compose_fk_evaluated_expression(self.tip, self.root).to_rotation() + frame_R_goal = cas.TransMatrix(path.poses[-1]).to_rotation() + final_orientation.add_rotation_goal_constraints(frame_R_current=frame_R_current, + frame_R_goal=frame_R_goal, + current_R_frame_eval=current_R_frame_eval, + reference_velocity=self.max_rotation_velocity, + weight=self.weight) + final_orientation.start_condition = goal_reached.get_state_expression() + + orientation_reached = ExpressionMonitor(name='final orientation reached', + start_condition=goal_reached.get_state_expression()) + self.add_monitor(orientation_reached) + rotation_error = cas.rotational_error(frame_R_current, frame_R_goal) + orientation_reached.expression = cas.less(cas.abs(rotation_error), 0.01) + end = EndMotion(name='done') - end.start_condition = goal_reached.get_state_expression() + end.start_condition = orientation_reached.get_state_expression() self.add_monitor(end) - self.connect_start_condition_to_all_tasks(start_condition) self.connect_hold_condition_to_all_tasks(hold_condition) self.connect_end_condition_to_all_tasks(end_condition) def path_to_trajectory(self, path: Path): - self.traj_data = [] + self.traj_data = [self.get_current_point()] for p in path.poses: - self.traj_data.append(np.array([p.pose.position.x, p.pose.position.y])) + self.append_point_to_traj(p.pose.position.x, p.pose.position.y) self.trajectory = np.array(self.traj_data) + self.publish_trajectory() + + def append_point_to_traj(self, x: float, y: float): + current_point = np.array([x, y], dtype=np.float64) + last_point = self.traj_data[-1] + error_vector = current_point - last_point + distance = np.linalg.norm(error_vector) + if distance < self.interpolation_step_size * 2: + self.traj_data[-1] = 0.5 * self.traj_data[-1] + 0.5 * current_point + else: + error_vector /= distance + ranges = np.arange(self.interpolation_step_size, distance, self.interpolation_step_size) + interpolated_distance = distance / len(ranges) + for i, dt in enumerate(ranges): + interpolated_point = last_point + error_vector * interpolated_distance * (i + 1) + self.traj_data.append(interpolated_point) + self.traj_data.append(current_point) def clean_up(self): for sub in self.laser_subs: @@ -623,25 +645,7 @@ def publish_tracking_radius(self): m_line.pose.orientation.w = 1 m_line.frame_locked = True ms.markers.append(m_line) - self.pub.publish(ms) - - def publish_distance_to_target(self): - ms = MarkerArray() - m_line = Marker() - m_line.action = m_line.ADD - m_line.ns = 'distance_to_target_stop_threshold' - m_line.id = 1332 - m_line.type = m_line.CYLINDER - m_line.header.frame_id = str(self.tip.short_name) - m_line.scale.x = self.min_distance_to_target * 2 - m_line.scale.y = self.min_distance_to_target * 2 - m_line.scale.z = 0.01 - m_line.color.a = 0.5 - m_line.color.g = 1 - m_line.pose.orientation.w = 1 - m_line.frame_locked = True - ms.markers.append(m_line) - self.pub.publish(ms) + FollowPointPath.pub.publish(ms) class CarryMyBullshit(Goal): diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index 4af2d6318a..47087202b8 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -1051,19 +1051,32 @@ def test_follow_nav_path(self, zero_pose: PR2TestWrapper): path_msg.header.frame_id = 'map' poses_data = [ - {'position': (3.4403343200683594, 2.349609851837158, 0.0), 'orientation': (0.0, 0.0, -0.9999553938074013, 0.009445125488052377)}, - {'position': (3.498216525117533, 2.3331048932770795, 0.0), 'orientation': (0.0, 0.0, 0.9993770281155905, 0.035292430843599024)}, - {'position': (3.5582080985686915, 2.3288624659763553, 0.0), 'orientation': (0.0, 0.0, 0.9996662404939319, 0.02583423342636984)}, - {'position': (3.6068967725310745, 2.326344275148637, 0.0), 'orientation': (0.0, 0.0, 0.9997591723006155, 0.021945327538867403)}, - {'position': (3.6598472962032886, 2.324018561554272, 0.0), 'orientation': (0.0, 0.0, 0.9992479949741707, 0.03877427678370992)}, - {'position': (3.6924761139448794, 2.3214825211531753, 0.0), 'orientation': (0.0, 0.0, 0.9991740059338614, 0.04063626294431946)}, - {'position': (3.727106939527923, 2.318660992860927, 0.0), 'orientation': (0.0, 0.0, 0.9991306638705609, 0.0416883258667487)}, - {'position': (3.7636721910961253, 2.3156043305022376, 0.0), 'orientation': (0.0, 0.0, 0.9990501048487416, 0.043576232073442425)}, - {'position': (3.8028157945033154, 2.31218311653614, 0.0), 'orientation': (0.0, 0.0, 0.9987543902336266, 0.04989657291895693)}, - {'position': (3.8460085061683724, 2.307856605808622, 0.0), 'orientation': (0.0, 0.0, 0.9975155500290784, 0.07044662838053373)}, - {'position': (3.8961558228154374, 2.3007380861823137, 0.0), 'orientation': (0.0, 0.0, 0.9969280676127491, 0.07832258937184026)}, - {'position': (3.9597676634174857, 2.2906808170884503, 0.0), 'orientation': (0.0, 0.0, -0.9986858657493689, 0.05124979563308978)}, - {'position': (4.03348337802564, 2.2982665669040685, 0.0), 'orientation': (0.0, 0.0, -0.9986858657493689, 0.05124979563308978)}, + {'position': (3.4403343200683594, 2.349609851837158, 0.0), + 'orientation': (0.0, 0.0, -0.9999553938074013, 0.009445125488052377)}, + {'position': (3.498216525117533, 2.3331048932770795, 0.0), + 'orientation': (0.0, 0.0, 0.9993770281155905, 0.035292430843599024)}, + {'position': (3.5582080985686915, 2.3288624659763553, 0.0), + 'orientation': (0.0, 0.0, 0.9996662404939319, 0.02583423342636984)}, + {'position': (3.6068967725310745, 2.326344275148637, 0.0), + 'orientation': (0.0, 0.0, 0.9997591723006155, 0.021945327538867403)}, + {'position': (3.6598472962032886, 2.324018561554272, 0.0), + 'orientation': (0.0, 0.0, 0.9992479949741707, 0.03877427678370992)}, + {'position': (3.6924761139448794, 2.3214825211531753, 0.0), + 'orientation': (0.0, 0.0, 0.9991740059338614, 0.04063626294431946)}, + {'position': (3.727106939527923, 2.318660992860927, 0.0), + 'orientation': (0.0, 0.0, 0.9991306638705609, 0.0416883258667487)}, + {'position': (3.7636721910961253, 2.3156043305022376, 0.0), + 'orientation': (0.0, 0.0, 0.9990501048487416, 0.043576232073442425)}, + {'position': (3.8028157945033154, 2.31218311653614, 0.0), + 'orientation': (0.0, 0.0, 0.9987543902336266, 0.04989657291895693)}, + {'position': (3.8460085061683724, 2.307856605808622, 0.0), + 'orientation': (0.0, 0.0, 0.9975155500290784, 0.07044662838053373)}, + {'position': (3.8961558228154374, 2.3007380861823137, 0.0), + 'orientation': (0.0, 0.0, 0.9969280676127491, 0.07832258937184026)}, + {'position': (3.9597676634174857, 2.2906808170884503, 0.0), + 'orientation': (0.0, 0.0, -0.9986858657493689, 0.05124979563308978)}, + {'position': (4.03348337802564, 2.2982665669040685, 0.0), + 'orientation': (0.0, 0.0, -0.9986858657493689, 0.05124979563308978)}, ] for pose_data in poses_data: @@ -1071,7 +1084,33 @@ def test_follow_nav_path(self, zero_pose: PR2TestWrapper): pose.header.frame_id = 'map' pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pose_data['position'] pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = \ - pose_data['orientation'] + pose_data['orientation'] + path_msg.poses.append(pose) + zero_pose.motion_goals.add_motion_goal(motion_goal_class=FollowPointPath.__name__, + name='follow', + camera_link='head_mount_kinect_rgb_optical_frame', + laser_frame_id='base_laser_link', + # laser_topics=[], + path=path_msg) + zero_pose.execute(add_local_minimum_reached=False) + + def test_follow_nav_path2(self, zero_pose: PR2TestWrapper): + path_msg = Path() + path_msg.header.frame_id = 'map' + + poses_data = [ + {'position': (1, 0, 0.0), 'orientation': (0.0, 0.0, 0, 1)}, + {'position': (1, 1, 0.0), 'orientation': (0.0, 0.0, 0, 1)}, + {'position': (-1, 1, 0.0), 'orientation': (0.0, 0.0, 0, 1)}, + {'position': (-1, -1, 0.0), 'orientation': (0.0, 0.0, 0, 1)}, + ] + + for pose_data in poses_data: + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pose_data['position'] + pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = \ + pose_data['orientation'] path_msg.poses.append(pose) zero_pose.motion_goals.add_motion_goal(motion_goal_class=FollowPointPath.__name__, name='follow', @@ -1079,8 +1118,6 @@ def test_follow_nav_path(self, zero_pose: PR2TestWrapper): laser_frame_id='base_laser_link', # laser_topics=[], path=path_msg) - # local_min = zero_pose.monitors.add_local_minimum_reached() - # zero_pose.monitors.add_end_motion(local_min) zero_pose.execute(add_local_minimum_reached=False) # TODO write buggy constraints that test sanity checks From 4db61106e6c97bdb4ad1bcebcfaffc127134830e Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 26 Jun 2024 15:30:41 +0200 Subject: [PATCH 08/10] added add_follow_nav_path to python interface --- src/giskardpy/goals/base_traj_follower.py | 12 ++- .../python_interface/python_interface.py | 77 ++++++++++++++++++- test/test_integration_pr2.py | 15 ++-- 3 files changed, 88 insertions(+), 16 deletions(-) diff --git a/src/giskardpy/goals/base_traj_follower.py b/src/giskardpy/goals/base_traj_follower.py index a3bf51b039..ea256af9e8 100644 --- a/src/giskardpy/goals/base_traj_follower.py +++ b/src/giskardpy/goals/base_traj_follower.py @@ -165,7 +165,7 @@ def add_rot_constraints(self): name='/rot') -class FollowPointPath(Goal): +class FollowNavPath(Goal): trajectory: np.ndarray traj_data: List[np.ndarray] laser_thresholds: Dict[int, np.ndarray] @@ -181,7 +181,6 @@ def __init__(self, odom_joint_name: Optional[str] = None, root_link: Optional[str] = None, camera_link: str = 'head_rgbd_sensor_link', - distance_to_target_stop_threshold: float = 1, laser_scan_age_threshold: float = 2, laser_distance_threshold: float = 0.5, laser_distance_threshold_width: float = 0.8, @@ -191,7 +190,7 @@ def __init__(self, max_rotation_velocity: float = 0.5, max_rotation_velocity_head: float = 1, max_translation_velocity: float = 0.38, - traj_tracking_radius: float = 0.4, + traj_tracking_radius: float = 0.5, height_for_camera_target: float = 1, laser_frame_id: str = 'base_range_sensor_link', start_condition: cas.Expression = cas.TrueSymbol, @@ -202,8 +201,8 @@ def __init__(self, self.laser_thresholds = {} self.last_scan = {} self.enable_laser_avoidance = len(laser_topics) > 0 - if FollowPointPath.pub is None: - FollowPointPath.pub = rospy.Publisher('~visualization_marker_array', MarkerArray) + if FollowNavPath.pub is None: + FollowNavPath.pub = rospy.Publisher('~visualization_marker_array', MarkerArray) self.laser_topics = list(laser_topics) self.laser_distance_threshold_width = laser_distance_threshold_width / 2 self.closest_laser_left = [self.laser_distance_threshold_width] * len(self.laser_topics) @@ -235,7 +234,6 @@ def __init__(self, self.max_rotation_velocity_head = max_rotation_velocity_head self.max_translation_velocity = max_translation_velocity self.weight = WEIGHT_BELOW_CA - self.min_distance_to_target = distance_to_target_stop_threshold self.laser_distance_threshold = laser_distance_threshold self.traj_tracking_radius = traj_tracking_radius self.interpolation_step_size = 0.05 @@ -645,7 +643,7 @@ def publish_tracking_radius(self): m_line.pose.orientation.w = 1 m_line.frame_locked = True ms.markers.append(m_line) - FollowPointPath.pub.publish(ms) + FollowNavPath.pub.publish(ms) class CarryMyBullshit(Goal): diff --git a/src/giskardpy/python_interface/python_interface.py b/src/giskardpy/python_interface/python_interface.py index bed53c8070..bb79123b44 100644 --- a/src/giskardpy/python_interface/python_interface.py +++ b/src/giskardpy/python_interface/python_interface.py @@ -4,6 +4,7 @@ import rospy from actionlib import SimpleActionClient from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped, QuaternionStamped +from nav_msgs.msg import Path from rospy import ServiceException from shape_msgs.msg import SolidPrimitive @@ -17,7 +18,7 @@ from giskardpy.exceptions import DuplicateNameException, UnknownGroupException from giskardpy.goals.align_planes import AlignPlanes from giskardpy.goals.align_to_push_door import AlignToPushDoor -from giskardpy.goals.base_traj_follower import CarryMyBullshit +from giskardpy.goals.base_traj_follower import CarryMyBullshit, FollowNavPath from giskardpy.goals.cartesian_goals import CartesianPose, DiffDriveBaseGoal, CartesianVelocityLimit, \ CartesianOrientation, CartesianPoseStraight, CartesianPosition, CartesianPositionStraight from giskardpy.goals.collision_avoidance import CollisionAvoidance @@ -1054,6 +1055,80 @@ def add_carry_my_luggage(self, hold_condition=hold_condition, end_condition=end_condition) + def add_follow_nav_path(self, + name: str, + path: Path, + laser_topics: Tuple[str] = ('/hsrb/base_scan',), + odom_joint_name: Optional[str] = None, + root_link: Optional[str] = None, + camera_link: str = 'head_rgbd_sensor_link', + distance_to_target_stop_threshold: float = 1, + laser_scan_age_threshold: float = 2, + laser_distance_threshold: float = 0.5, + laser_distance_threshold_width: float = 0.8, + laser_avoidance_angle_cutout: float = np.pi / 4, + laser_avoidance_sideways_buffer: float = 0.04, + base_orientation_threshold: float = np.pi / 16, + max_rotation_velocity: float = 0.5, + max_rotation_velocity_head: float = 1, + max_translation_velocity: float = 0.38, + traj_tracking_radius: float = 0.4, + height_for_camera_target: float = 1, + laser_frame_id: str = 'base_range_sensor_link', + start_condition: str = '', + hold_condition: str = '', + end_condition: str = ''): + """ + Will follow the path, orienting itself and the head towards the next points in the list. + At the end orient itself according to the final orientation in it. All other orientations will be ignored. + :param name: name of the goal + :param path: a nav path, make sure it's ordered correctly! + :param odom_joint_name: name of the odom joint + :param root_link: will use global reference frame + :param camera_link: link of the camera that will point to the tracked human + :param laser_scan_age_threshold: giskard will complain if scans are older than this many seconds + :param laser_distance_threshold: this and width are used to crate a stopping zone around the robot. + laser distance draws a circle around the robot and width lines to the left and right. + the stopping zone is the minimum of the two. + :param laser_distance_threshold_width: see laser_distance_threshold + :param laser_avoidance_angle_cutout: if something is in the stop zone in front of the robot in +/- this angle range + giskard will pause, otherwise it will try to dodge left or right + :param laser_avoidance_sideways_buffer: increase this if the robot is shaking too much if something is to its + left and right at the same time. + :param base_orientation_threshold: giskard will align the base of the robot to the target, this is a +/- buffer to avoid shaking + :param max_rotation_velocity: how quickly the base can change orientation + :param max_rotation_velocity_head: how quickly the head rotates + :param max_translation_velocity: how quickly the base drives + :param traj_tracking_radius: how close the robots root link will try to stick to the path in meter + :param height_for_camera_target: target tracking with head will ignore the published height, but use this instead + :param laser_frame_id: frame_id of the laser scanner + :param start_condition: + :param hold_condition: + :param end_condition: + """ + self.add_motion_goal(motion_goal_class=FollowNavPath.__name__, + name=name, + path=path, + laser_topics=laser_topics, + odom_joint_name=odom_joint_name, + root_link=root_link, + camera_link=camera_link, + laser_scan_age_threshold=laser_scan_age_threshold, + laser_distance_threshold=laser_distance_threshold, + laser_distance_threshold_width=laser_distance_threshold_width, + laser_avoidance_angle_cutout=laser_avoidance_angle_cutout, + laser_avoidance_sideways_buffer=laser_avoidance_sideways_buffer, + base_orientation_threshold=base_orientation_threshold, + max_rotation_velocity=max_rotation_velocity, + max_rotation_velocity_head=max_rotation_velocity_head, + max_translation_velocity=max_translation_velocity, + traj_tracking_radius=traj_tracking_radius, + height_for_camera_target=height_for_camera_target, + laser_frame_id=laser_frame_id, + start_condition=start_condition, + hold_condition=hold_condition, + end_condition=end_condition) + def add_cartesian_orientation(self, goal_orientation: QuaternionStamped, tip_link: str, diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py index 47087202b8..c5f6feac13 100644 --- a/test/test_integration_pr2.py +++ b/test/test_integration_pr2.py @@ -21,7 +21,7 @@ from giskardpy.configs.giskard import Giskard from giskardpy.configs.iai_robots.pr2 import PR2CollisionAvoidance, PR2StandaloneInterface, WorldWithPR2Config from giskardpy.configs.qp_controller_config import SupportedQPSolver, QPControllerConfig -from giskardpy.goals.base_traj_follower import FollowPointPath +from giskardpy.goals.base_traj_follower import FollowNavPath from giskardpy.goals.cartesian_goals import RelativePositionSequence from giskardpy.goals.caster import Circle, Wave from giskardpy.goals.collision_avoidance import CollisionAvoidanceHint @@ -1086,7 +1086,7 @@ def test_follow_nav_path(self, zero_pose: PR2TestWrapper): pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = \ pose_data['orientation'] path_msg.poses.append(pose) - zero_pose.motion_goals.add_motion_goal(motion_goal_class=FollowPointPath.__name__, + zero_pose.motion_goals.add_motion_goal(motion_goal_class=FollowNavPath.__name__, name='follow', camera_link='head_mount_kinect_rgb_optical_frame', laser_frame_id='base_laser_link', @@ -1112,12 +1112,11 @@ def test_follow_nav_path2(self, zero_pose: PR2TestWrapper): pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = \ pose_data['orientation'] path_msg.poses.append(pose) - zero_pose.motion_goals.add_motion_goal(motion_goal_class=FollowPointPath.__name__, - name='follow', - camera_link='head_mount_kinect_rgb_optical_frame', - laser_frame_id='base_laser_link', - # laser_topics=[], - path=path_msg) + zero_pose.motion_goals.add_follow_nav_path(name='follow', + camera_link='head_mount_kinect_rgb_optical_frame', + laser_frame_id='base_laser_link', + # laser_topics=[], + path=path_msg) zero_pose.execute(add_local_minimum_reached=False) # TODO write buggy constraints that test sanity checks From b50e4e1fcce384f39985dd350d9a5057fa5b8a7d Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 26 Jun 2024 15:32:51 +0200 Subject: [PATCH 09/10] turning of tf and js publishing in github workflow --- src/giskardpy/configs/behavior_tree_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/giskardpy/configs/behavior_tree_config.py b/src/giskardpy/configs/behavior_tree_config.py index 85c4407dba..ddea4aed6c 100644 --- a/src/giskardpy/configs/behavior_tree_config.py +++ b/src/giskardpy/configs/behavior_tree_config.py @@ -197,9 +197,9 @@ def __init__(self, """ self.include_prefix = include_prefix if is_running_in_pytest(): - # publish_tf = False - publish_js = False if god_map.is_in_github_workflow(): + publish_js = False + publish_tf = False debug_mode = False simulation_max_hz = None super().__init__(ControlModes.standalone, simulation_max_hz=simulation_max_hz) From 470f87fb8755509d918e90e73621ff97dc9145e2 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 26 Jun 2024 16:09:22 +0200 Subject: [PATCH 10/10] reverted default external collision avoidance thresholds in hsr config --- src/giskardpy/configs/iai_robots/hsr.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/giskardpy/configs/iai_robots/hsr.py b/src/giskardpy/configs/iai_robots/hsr.py index 5b56bba32d..ae1a2d3437 100644 --- a/src/giskardpy/configs/iai_robots/hsr.py +++ b/src/giskardpy/configs/iai_robots/hsr.py @@ -101,8 +101,8 @@ def __init__(self, drive_joint_name: str = 'brumbrum'): def setup(self): self.load_self_collision_matrix('package://giskardpy/self_collision_matrices/iai/hsrb.srdf') - self.set_default_external_collision_avoidance(soft_threshold=0.08, - hard_threshold=0.04) + self.set_default_external_collision_avoidance(soft_threshold=0.05, + hard_threshold=0.0) self.overwrite_external_collision_avoidance('wrist_roll_joint', number_of_repeller=4, soft_threshold=0.05,