diff --git a/src/giskardpy_ros/goals/realtime_goals.py b/src/giskardpy_ros/goals/realtime_goals.py index 0e9181358..d059d6a01 100644 --- a/src/giskardpy_ros/goals/realtime_goals.py +++ b/src/giskardpy_ros/goals/realtime_goals.py @@ -135,11 +135,11 @@ def __init__(self, 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 = cas.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 = cas.Vector3() self.tip_V_pointing_axis.x = 1 self.max_rotation_velocity = max_rotation_velocity self.max_rotation_velocity_head = max_rotation_velocity_head @@ -197,27 +197,27 @@ def __init__(self, root_T_camera = god_map.world.compose_fk_expression(self.root, self.camera_link) root_P_bf = root_T_bf.to_position() - min_left_violation1 = symbol_manager.get_symbol(self + '.closest_laser_left') - min_right_violation1 = symbol_manager.get_symbol(self + '.closest_laser_right') - closest_laser_reading1 = symbol_manager.get_symbol(self + '.closest_laser_reading') - min_left_violation2 = symbol_manager.get_symbol(self + '.closest_laser_left_pc') - min_right_violation2 = symbol_manager.get_symbol(self + '.closest_laser_right_pc') - closest_laser_reading2 = symbol_manager.get_symbol(self + '.closest_laser_reading_pc') + min_left_violation1 = symbol_manager.get_symbol(self.ref_str + '.closest_laser_left') + min_right_violation1 = symbol_manager.get_symbol(self.ref_str + '.closest_laser_right') + closest_laser_reading1 = symbol_manager.get_symbol(self.ref_str + '.closest_laser_reading') + min_left_violation2 = symbol_manager.get_symbol(self.ref_str + '.closest_laser_left_pc') + min_right_violation2 = symbol_manager.get_symbol(self.ref_str + '.closest_laser_right_pc') + closest_laser_reading2 = symbol_manager.get_symbol(self.ref_str + '.closest_laser_reading_pc') closest_laser_left = cas.min(min_left_violation1, min_left_violation2) closest_laser_right = cas.max(min_right_violation1, min_right_violation2) closest_laser_reading = cas.min(closest_laser_reading1, closest_laser_reading2) if not self.drive_back: - last_target_age = symbol_manager.get_symbol(self + '.last_target_age') + last_target_age = symbol_manager.get_symbol(self.ref_str + '.last_target_age') target_lost = ExpressionMonitor(name='target out of sight') self.add_monitor(target_lost) target_lost.expression = cas.greater_equal(last_target_age, self.target_age_threshold) - 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\']') + next_x = symbol_manager.get_symbol(self.ref_str + '.get_current_target()[\'next_x\']') + next_y = symbol_manager.get_symbol(self.ref_str + '.get_current_target()[\'next_y\']') + closest_x = symbol_manager.get_symbol(self.ref_str + '.get_current_target()[\'closest_x\']') + closest_y = symbol_manager.get_symbol(self.ref_str + '.get_current_target()[\'closest_y\']') # tangent_x = god_map.to_expr(self._get_identifier() + ['get_current_target', tuple(), 'tangent_x']) # tangent_y = god_map.to_expr(self._get_identifier() + ['get_current_target', tuple(), 'tangent_y']) clear_memo(self.get_current_target) @@ -230,8 +230,10 @@ def __init__(self, if self.drive_back: map_P_human = root_P_goal_point else: - map_P_human = cas.Point3(symbol_manager.get_expr(self + '.human_point', input_type_hint=PointStamped)) - map_P_human_projected = cas.Point3(map_P_human) + map_P_human = cas.Point3((symbol_manager.get_symbol(self.ref_str + '.human_point.point.x'), + symbol_manager.get_symbol(self.ref_str + '.human_point.point.y'), + symbol_manager.get_symbol(self.ref_str + '.human_point.point.z'))) + map_P_human_projected = map_P_human map_P_human_projected.z = 0 # %% orient to goal @@ -351,8 +353,8 @@ def __init__(self, name='move sideways') if self.drive_back: - first_traj_x = symbol_manager.get_symbol(self + '.get_first_traj_point()[0]') - first_traj_y = symbol_manager.get_symbol(self + '.get_first_traj_point()[1]') + first_traj_x = symbol_manager.get_symbol(self.ref_str + '.get_first_traj_point()[0]') + first_traj_y = symbol_manager.get_symbol(self.ref_str + '.get_first_traj_point()[1]') first_point = cas.Point3([first_traj_x, first_traj_y, 0]) goal_reached = ExpressionMonitor(name='goal reached?') self.add_monitor(goal_reached) diff --git a/src/giskardpy_ros/python_interface/python_interface.py b/src/giskardpy_ros/python_interface/python_interface.py index ef29292b9..4325db0e7 100644 --- a/src/giskardpy_ros/python_interface/python_interface.py +++ b/src/giskardpy_ros/python_interface/python_interface.py @@ -35,6 +35,7 @@ from giskardpy.motion_graph.monitors.overwrite_state_monitors import SetOdometry, SetSeedConfiguration from giskardpy.motion_graph.monitors.payload_monitors import Print, Sleep, SetMaxTrajectoryLength, \ PayloadAlternator +from giskardpy_ros.goals.realtime_goals import CarryMyBullshit, FollowNavPath, RealTimePointing from giskardpy_ros.ros1 import msg_converter from giskardpy_ros.ros1.msg_converter import kwargs_to_json from giskardpy_ros.utils.utils import make_world_body_box diff --git a/test/test_integration_pr2_mujoco_closed_loop.py b/test/test_integration_pr2_mujoco_closed_loop.py index 41e41fb0c..98acb7605 100644 --- a/test/test_integration_pr2_mujoco_closed_loop.py +++ b/test/test_integration_pr2_mujoco_closed_loop.py @@ -14,6 +14,7 @@ import giskardpy_ros.ros1.tfwrapper as tf from giskard_msgs.msg import MoveGoal, GiskardError +from giskardpy.data_types.exceptions import PreemptedException from giskardpy_ros.configs.behavior_tree_config import ClosedLoopBTConfig from giskardpy_ros.configs.giskard import Giskard from giskardpy_ros.configs.iai_robots.pr2 import PR2CollisionAvoidance, PR2VelocityMujocoInterface, WorldWithPR2Config @@ -27,6 +28,28 @@ class PR2TestWrapperMujoco(PR2TestWrapper): + default_pose = { + 'r_elbow_flex_joint': -0.15, + 'r_forearm_roll_joint': 0, + 'r_shoulder_lift_joint': 0, + 'r_shoulder_pan_joint': 0, + 'r_upper_arm_roll_joint': 0, + 'r_wrist_flex_joint': -0.10001, + 'r_wrist_roll_joint': 0, + 'l_elbow_flex_joint': -0.15, + 'l_forearm_roll_joint': 0, + 'l_shoulder_lift_joint': 0, + 'l_shoulder_pan_joint': 0, + 'l_upper_arm_roll_joint': 0, + 'l_wrist_flex_joint': -0.10001, + 'l_wrist_roll_joint': 0, + # 'torso_lift_joint': 0.2, + 'head_pan_joint': 0, + 'head_tilt_joint': 0, + 'l_gripper_l_finger_joint': 0.55, + 'r_gripper_l_finger_joint': 0.55 + } + better_pose = {'r_shoulder_pan_joint': -1.7125, 'r_shoulder_lift_joint': -0.25672, 'r_upper_arm_roll_joint': -1.46335, @@ -41,7 +64,7 @@ class PR2TestWrapperMujoco(PR2TestWrapper): 'l_forearm_roll_joint': 16.99, 'l_wrist_flex_joint': - 0.10001, 'l_wrist_roll_joint': 0, - 'torso_lift_joint': 0.2, + # 'torso_lift_joint': 0.2, # 'l_gripper_l_finger_joint': 0.55, # 'r_gripper_l_finger_joint': 0.55, 'head_pan_joint': 0, @@ -62,8 +85,10 @@ def __init__(self): giskard = Giskard(world_config=WorldWithPR2Config(), collision_avoidance_config=PR2CollisionAvoidance(), robot_interface_config=PR2VelocityMujocoInterface(), - behavior_tree_config=ClosedLoopBTConfig(debug_mode=True, control_loop_max_hz=50), - qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.qpSWIFT)) + behavior_tree_config=ClosedLoopBTConfig(debug_mode=True), + qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.qpSWIFT, + mpc_dt=0.0125, + control_dt=0.0125)) super().__init__(giskard) def reset_base(self): @@ -237,7 +262,7 @@ def test_carry_my_bs(self, zero_pose: PR2TestWrapper): zero_pose.allow_all_collisions() # zero_pose.execute(expected_error_code=GiskardError.EXECUTION_ERROR, # add_local_minimum_reached=False) - zero_pose.execute(expected_error_code=GiskardError.PREEMPTED, + zero_pose.execute(expected_error_type=PreemptedException, stop_after=20, add_local_minimum_reached=False)