Skip to content

Commit

Permalink
fixed error messages and other stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
@JustMiep
JustMiep committed Jun 21, 2024
1 parent 5be887b commit 7ec1662
Show file tree
Hide file tree
Showing 2 changed files with 20 additions and 17 deletions.
13 changes: 7 additions & 6 deletions src/giskardpy/python_interface/old_python_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
from tmc_manipulation_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint

from giskard_msgs.msg import MoveResult, CollisionEntry, MoveGoal, WorldResult
from giskard_msgs.msg import MoveResult, CollisionEntry, MoveGoal, WorldResult, GiskardError
from giskard_msgs.srv import DyeGroupResponse, GetGroupInfoResponse
from giskardpy.data_types import goal_parameter
from giskardpy.goals.suturo import MoveAroundDishwasher, Placing, Reaching
Expand Down Expand Up @@ -1470,7 +1470,8 @@ def set_grasp_bar_offset_goal(self,
**kwargs)

def monitor_placing_in_old(self,
context,
grasp: str,
align: str,
goal_pose: PoseStamped,
threshold_name: str = "",
object_type: str = "",
Expand All @@ -1489,15 +1490,16 @@ def monitor_placing_in_old(self,
object_type=object_type)

self.motion_goals.add_motion_goal(motion_goal_class=Placing.__name__,
context=context,
goal_pose=goal_pose,
align=align,
grasp=grasp,
tip_link=tip_link,
velocity=velocity,
end_condition=f'{force_torque_trigger} and {sleep}')

local_min = self.monitors.add_local_minimum_reached()

self.monitors.add_cancel_motion(local_min, "HSR IS UNABLE TO PLACE OBJECT", 810)
self.monitors.add_cancel_motion(local_min, "", GiskardError.FORCE_TORQUE_MONITOR_PLACING_MISSED_PLACING_LOCATION)
self.monitors.add_end_motion(start_condition=f'{force_torque_trigger} or {local_min}')
self.monitors.add_max_trajectory_length(100)

Expand All @@ -1522,7 +1524,6 @@ def monitor_grasp_carefully_in_old(self,
force_torque_trigger = self.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
name=PayloadForceTorque.__name__,
start_condition=gripper_closed,
# add gripper monitor as start condition
threshold_name=threshold_name,
object_type=object_type)

Expand All @@ -1538,6 +1539,6 @@ def monitor_grasp_carefully_in_old(self,

local_min = self.monitors.add_local_minimum_reached()

self.monitors.add_cancel_motion(local_min, "", 811)
self.monitors.add_cancel_motion(local_min, "", GiskardError.FORCE_TORQUE_MONITOR_GRASPING_MISSED_OBJECT)
self.monitors.add_end_motion(start_condition=f'{force_torque_trigger} or {local_min}')
self.monitors.add_max_trajectory_length(100)
24 changes: 13 additions & 11 deletions src/giskardpy/python_interface/python_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2047,7 +2047,8 @@ def _feedback_cb(self, msg: MoveFeedback):

# SuTuRo Goals start here! (Only add SuTuRo goals which actually need monitors for stopping conditions etc.)
def monitor_placing(self,
context,
align: str,
grasp: str,
goal_pose: PoseStamped,
threshold_name: str = "",
object_type: str = "",
Expand All @@ -2066,20 +2067,19 @@ def monitor_placing(self,
object_type=object_type)

self.motion_goals.add_motion_goal(motion_goal_class=Placing.__name__,
context=context,
goal_pose=goal_pose,
align=align,
grasp=grasp,
tip_link=tip_link,
velocity=velocity,
end_condition=f'{force_torque_trigger} and {sleep}')

local_min = self.monitors.add_local_minimum_reached()

self.monitors.add_cancel_motion(local_min, "HSR IS UNABLE TO PLACE OBJECT", )
self.monitors.add_end_motion(start_condition=f'{force_torque_trigger} or {local_min}')
self.monitors.add_cancel_motion(local_min, "", GiskardError.FORCE_TORQUE_MONITOR_PLACING_MISSED_PLACING_LOCATION)
self.monitors.add_end_motion(start_condition=force_torque_trigger)
self.monitors.add_max_trajectory_length(100)

# TODO: Add gripper monitor as start condition when it's merged in

def monitor_grasp_carefully(self,
goal_pose: PoseStamped,
align: str,
Expand All @@ -2097,10 +2097,10 @@ def monitor_grasp_carefully(self,
to open doors or fails to properly grip an object.
"""
sleep = self.monitors.add_sleep(1.5)
gripper_closed = self.monitors.add_close_hsr_gripper()
gripper_open = self.monitors.add_open_hsr_gripper()
force_torque_trigger = self.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
name=PayloadForceTorque.__name__,
start_condition=gripper_closed, # add gripper monitor as start condition
start_condition=gripper_open,
threshold_name=threshold_name,
object_type=object_type)

Expand All @@ -2116,17 +2116,19 @@ def monitor_grasp_carefully(self,

local_min = self.monitors.add_local_minimum_reached()

self.monitors.add_cancel_motion(local_min, "", 811)
self.monitors.add_end_motion(start_condition=f'{force_torque_trigger} or {local_min}')
self.monitors.add_cancel_motion(local_min, "", GiskardError.FORCE_TORQUE_MONITOR_GRASPING_MISSED_OBJECT)
self.monitors.add_end_motion(start_condition=force_torque_trigger)
self.monitors.add_max_trajectory_length(100)

def monitor_transport_check(self,
object_type: str = "",
threshold_name: str = ""):

gripper_closed = self.monitors.add_close_hsr_gripper()

self.monitors.add_monitor(monitor_class=PayloadForceTorque.__name__,
name=PayloadForceTorque.__name__,
start_condition='',
start_condition=gripper_closed,
threshold_name=threshold_name,
object_type=object_type)

Expand Down

0 comments on commit 7ec1662

Please sign in to comment.