Skip to content

Commit

Permalink
fixed carry my bs
Browse files Browse the repository at this point in the history
  • Loading branch information
@ichumuh
ichumuh committed Jan 10, 2025
1 parent f3d80b5 commit 14af2a7
Show file tree
Hide file tree
Showing 3 changed files with 49 additions and 21 deletions.
36 changes: 19 additions & 17 deletions src/giskardpy_ros/goals/realtime_goals.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions src/giskardpy_ros/python_interface/python_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
33 changes: 29 additions & 4 deletions test/test_integration_pr2_mujoco_closed_loop.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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):
Expand Down Expand Up @@ -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)

Expand Down

0 comments on commit 14af2a7

Please sign in to comment.