class PlanAffordanceState(EventState):
'''
Plans for a given affordance without collision avoidance.
-- vel_scaling float Scales the velocity of the planned trajectory,
lower values for slower trajectories.
-- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault")
-- drake_sample_rate float Sample rate for Drake's result trajectory (in Hz, default = 4.0)
-- drake_orientation_type int How should the eef-orientation be handled (see ExtendedPlanningOptions, 0 = full, 1 = axis only, 2 = ignore)
-- drake_link_axis float[] Target axis to keep (only for drake_orientation_type == ORIENTATION_AXIS_ONLY
># affordance Affordance A message as defined in vigir_object_template_msgs containing all required information.
># hand string The hand which should execute the given affordance.
># reference_point PoseStamped A point of interest on the template w.r.t.
which planning will take place (defaults to palm if not set).
A template's 'usability' pose would go here.
#> joint_trajectory JointTrajectory Trajectory of the end effector to perform the requested motion.
#> plan_fraction float Fraction of the requested motion which could be planned.
A value below 1 indicates a partial motion.
<= done Successfully planned the given affordance.
<= failed Failed to plan the affordance.
'''
LEFT_HAND = 'left'
RIGHT_HAND = 'right'
def __init__(self,
vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault",
drake_sample_rate=4.0,
drake_orientation_type=2,
drake_link_axis=[0, 0, 1]):
'''Constructor'''
super(PlanAffordanceState, self).__init__(
outcomes=['done', 'incomplete', 'failed'],
input_keys=['affordance', 'hand', 'reference_point'],
output_keys=['joint_trajectory', 'plan_fraction'])
self._client = ProxyMoveitClient()
try:
self.set_up_affordance_service() # topics and clients are set here
except Exception as e:
Logger.logerr(
'Failed to set up GetAffordanceInWristFrame service: \n%s' %
str(e))
return
self._vel_scaling = vel_scaling
self._planner_id = planner_id
self._drake_sample_rate = drake_sample_rate
self._drake_orientation_type = drake_orientation_type
self._drake_link_axis = Point(x=drake_link_axis[0],
y=drake_link_axis[1],
z=drake_link_axis[2])
self._failed = False
self._done = False
self._incomplete = False
def execute(self, userdata):
'''Wait for result and determine outcome based on fraction thresholds'''
fail_threshold = 0.05
success_threshold = 0.95
if self._failed:
return 'failed'
if self._done:
return 'done'
if self._incomplete:
return 'incomplete'
if self._client.finished():
userdata.joint_trajectory = self._client.get_plan()
if self._client.success():
plan_fraction = self._client.get_plan_fraction()
Logger.loginfo('Plan completion fraction = %f%%' %
(plan_fraction * 100))
userdata.plan_fraction = plan_fraction
if plan_fraction < fail_threshold:
Logger.logwarn(
'Failure! The plan fraction was below %d%%' %
(fail_threshold * 100))
self._failed = True
return 'failed'
elif plan_fraction > success_threshold:
Logger.loginfo(
'Success! The plan fraction was above %d%%' %
(success_threshold * 100))
self._done = True
return 'done'
else:
Logger.logwarn(
'Partial success.The plan fraction was between %d%% and %d%%'
% (fail_threshold * 100, success_threshold * 100))
self._incomplete = True
return 'incomplete'
else:
Logger.logwarn('MoveIt failed with the following error: %s' %
self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''Handle cartesian/circular affordance, set options, and send goal.'''
self._failed = False
self._done = False
self._incomplete = False
planning_group_str = 'l_arm_group' if userdata.hand == PlanAffordanceState.LEFT_HAND else 'r_arm_group'
affordance = userdata.affordance
print affordance #TODO: remove temp debug statement
# Create the manipulation action goal
self._client.new_goal(move_group=planning_group_str)
if affordance.type == "circular":
self._client.set_circular_motion(angle=affordance.displacement,
pitch=affordance.pitch)
for waypoint in affordance.waypoints: # There should only be one
self._client.add_endeffector_pose(waypoint.pose,
waypoint.header.frame_id)
print waypoint #TODO: remove temp debug statement
elif affordance.type == "cartesian":
self._client.set_cartesian_motion()
try:
srv_request = GetAffordanceInWristFrameRequest(affordance)
srv_result = self._srv.call(
self._service_topics[userdata.hand], srv_request)
wrist_affordance = srv_result.wrist_affordance # PoseStamped
print wrist_affordance #TODO: remove temp debug statement
self._client.add_endeffector_pose(
wrist_affordance.pose, wrist_affordance.header.frame_id)
except Exception as e:
Logger.logwarn(
'Failed to transform affordance to wrist frame: \n%s' %
str(e))
self._failed = True
return
else:
Logger.logwarn("Unknown affordance type: %s" % affordance.type)
self._failed = True
return
# Set planning options
self._client.set_planner_id(self._planner_id)
self._client.set_velocity_scaling(self._vel_scaling)
self._client.set_collision_avoidance(True)
self._client.set_execute_incomplete_plans(True)
self._client.set_keep_orientation(affordance.keep_orientation)
#Set reference_point and frame, if provided. Defaults to palm.
if userdata.reference_point is not None:
self._client.set_reference_point(
pose=userdata.reference_point.pose,
frame_id=userdata.reference_point.header.frame_id)
#Set Drake specific options
self._client.set_drake_target_orientation_type(
self._drake_orientation_type)
self._client.set_drake_target_link_axis(self._drake_link_axis)
self._client.set_drake_trajectory_sample_rate(self._drake_sample_rate)
try:
self._client.start_planning()
except Exception as e:
Logger.logwarn('Could not request a plan for the affordance!\n%s' %
str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.finished():
self._client.cancel()
Logger.loginfo("Cancelled active action goal.")
############################################################################
def set_up_affordance_service(self):
hands_in_use = self.determine_hands_in_use()
# Set up services for transforming cartesian affordances to wrist frame
self._service_topics = dict()
for hand in hands_in_use:
topic = '/manipulation_control/%s_hand/wrist_affordance_srv' % hand[
0]
self._service_topics[hand] = topic
self._srv = ProxyServiceCaller({
t: GetAffordanceInWristFrame
for t in self._service_topics.values()
})
def determine_hands_in_use(self):
'''Determine which hand types and sides have been sourced'''
if not rospy.has_param("behavior/hand_type_prefix"):
Logger.logerr(
"Need to specify behavior/parameter hand_type_prefix at the parameter server"
)
return
hand_type_prefix = rospy.get_param("behavior/hand_type_prefix")
hands_in_use = list()
LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE']
RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE']
if LEFT_HAND not in ['l_stump', 'l_hook']:
hands_in_use.append('left')
if RIGHT_HAND not in ['r_stump', 'r_hook']:
hands_in_use.append('right')
if len(hands_in_use) == 0:
Logger.logerr(
'No hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s'
% (LEFT_HAND, RIGHT_HAND))
return hands_in_use
class PlanEndeffectorCartesianWaypointsState(EventState):
'''
Plans a cartesian endeffector trajectory passing all given waypoints.
-- ignore_collisions boolean Should collisions be ignored? Only pass True if you are sure that it is safe.
-- include_torso boolean Should the torso be included in the trajectory or stay fixed?
-- keep_endeffector_orientation boolean Should the initial orientation of the endeffector be kept?
-- allow_incomplete_plans boolean Should incomplete plans be allowed?
-- vel_scaling string Scales the velocity of the motion, lower values for slower trajectories.
-- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault")
># waypoints Pose[] Waypoints for the endeffector motion.
># hand string One of the class constants determining the hand to be moved.
># frame_id string Frame in which the waypoints are given.
#> joint_trajectory JointTrajectory Trajectory of the endeffector to perform the requested motion.
#> plan_fraction float Fraction of the requested motion which could be planned.
A value below 1 indicates a partial motion.
<= planned Was able to generate a valid motion plan.
This plan is not necessarily complete.
<= failed Failed to create a motion plan at all.
'''
LEFT_HAND = 'left'
RIGHT_HAND = 'right'
def __init__(self, ignore_collisions = False,
include_torso = False,
keep_endeffector_orientation = False,
allow_incomplete_plans = False,
vel_scaling = 0.1,
planner_id = "RRTConnectkConfigDefault"):
'''Constructor'''
super(PlanEndeffectorCartesianWaypointsState, self).__init__(outcomes=['planned', 'incomplete', 'failed'],
input_keys=['waypoints', 'hand', 'frame_id'],
output_keys=['joint_trajectory', 'plan_fraction'])
self._client = ProxyMoveitClient()
self._ignore_collisions = ignore_collisions
self._include_torso = include_torso
self._keep_endeffector_orientation = keep_endeffector_orientation
self._allow_incomplete_plans = allow_incomplete_plans
self._vel_scaling = vel_scaling
self._planner_id = planner_id
self._failed = False
self._done = False
self._incomplete = False
def execute(self, userdata):
'''
Execute this state
'''
fail_threshold = 0.05
success_threshold = 0.95
if self._failed:
return 'failed'
if self._done:
return 'planned'
if self._incomplete:
return 'incomplete'
if self._client.finished():
userdata.joint_trajectory = self._client.get_plan()
if self._client.success():
# Incomplete plans are considered a SUCCESS, if we allow them
plan_fraction = self._client.get_plan_fraction()
Logger.loginfo('Plan completion fraction = %.2f%%' % (plan_fraction*100))
userdata.plan_fraction = plan_fraction
if plan_fraction < fail_threshold:
rospy.logwarn('Failure! The plan fraction was below %d%%' % (fail_threshold*100))
self._failed = True
return 'failed'
elif plan_fraction > success_threshold:
rospy.loginfo('Success! The plan fraction was above %d%%' % (success_threshold*100))
self._done = True
return 'planned'
else:
rospy.logwarn('Partial success.The plan fraction was between %d%% and %d%%' % (fail_threshold*100, success_threshold*100))
self._incomplete = True
return 'incomplete'
else:
Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''...'''
if not hasattr(userdata, 'waypoints') or userdata.waypoints is None \
or not hasattr(userdata, 'hand') or userdata.hand is None \
or not hasattr(userdata, 'frame_id') or userdata.frame_id is None:
self._failed = True
Logger.logwarn('Userdata key of state %s does not exist or is currently undefined!' % self.name)
return
self._failed = False
self._done = False
self._incomplete = False
planning_group_str = \
('l' if userdata.hand == self.LEFT_HAND else 'r') + \
'_arm' + \
('_with_torso' if self._include_torso else '') + \
'_group'
# create the motion goal
self._client.new_goal(planning_group_str)
self._client.set_collision_avoidance(self._ignore_collisions)
self._client.set_cartesian_motion()
self._client.set_keep_orientation(self._keep_endeffector_orientation)
self._client.set_execute_incomplete_plans(self._allow_incomplete_plans)
self._client.set_velocity_scaling(self._vel_scaling)
self._client.set_planner_id(self._planner_id)
for pose in userdata.waypoints:
self._client.add_endeffector_pose(pose, userdata.frame_id)
rospy.loginfo('Sending planning request to pass %d waypoints in frame %s...' % (len(userdata.waypoints), userdata.frame_id))
try:
self._client.start_planning()
except Exception as e:
Logger.logwarn('Could not request a plan!\n%s' % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.finished():
self._client.cancel()
Logger.loginfo("Cancelled active action goal.")
class PlanAffordanceState(EventState):
'''
Plans for a given affordance without collision avoidance.
-- vel_scaling float Scales the velocity of the planned trajectory,
lower values for slower trajectories.
-- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault")
-- drake_sample_rate float Sample rate for Drake's result trajectory (in Hz, default = 4.0)
-- drake_orientation_type int How should the eef-orientation be handled (see ExtendedPlanningOptions, 0 = full, 1 = axis only, 2 = ignore)
-- drake_link_axis float[] Target axis to keep (only for drake_orientation_type == ORIENTATION_AXIS_ONLY
># affordance Affordance A message as defined in vigir_object_template_msgs containing all required information.
># hand string The hand which should execute the given affordance.
># reference_point PoseStamped A point of interest on the template w.r.t.
which planning will take place (defaults to palm if not set).
A template's 'usability' pose would go here.
#> joint_trajectory JointTrajectory Trajectory of the end effector to perform the requested motion.
#> plan_fraction float Fraction of the requested motion which could be planned.
A value below 1 indicates a partial motion.
<= done Successfully planned the given affordance.
<= failed Failed to plan the affordance.
'''
LEFT_HAND = 'left'
RIGHT_HAND = 'right'
def __init__(self, vel_scaling = 0.1, planner_id = "RRTConnectkConfigDefault", drake_sample_rate = 4.0, drake_orientation_type = 2, drake_link_axis = [0, 0, 1]):
'''Constructor'''
super(PlanAffordanceState, self).__init__(outcomes = ['done', 'incomplete', 'failed'],
input_keys = ['affordance', 'hand', 'reference_point'],
output_keys = ['joint_trajectory', 'plan_fraction'])
self._client = ProxyMoveitClient()
try:
self.set_up_affordance_service() # topics and clients are set here
except Exception as e:
Logger.logerr('Failed to set up GetAffordanceInWristFrame service: \n%s' % str(e))
return
self._vel_scaling = vel_scaling
self._planner_id = planner_id
self._drake_sample_rate = drake_sample_rate
self._drake_orientation_type = drake_orientation_type
self._drake_link_axis = Point( x = drake_link_axis[0], y = drake_link_axis[1], z = drake_link_axis[2] )
self._failed = False
self._done = False
self._incomplete = False
def execute(self, userdata):
'''Wait for result and determine outcome based on fraction thresholds'''
fail_threshold = 0.05
success_threshold = 0.95
if self._failed:
return 'failed'
if self._done:
return 'done'
if self._incomplete:
return 'incomplete'
if self._client.finished():
userdata.joint_trajectory = self._client.get_plan()
if self._client.success():
plan_fraction = self._client.get_plan_fraction()
Logger.loginfo('Plan completion fraction = %f%%' % (plan_fraction*100))
userdata.plan_fraction = plan_fraction
if plan_fraction < fail_threshold:
Logger.logwarn('Failure! The plan fraction was below %d%%' % (fail_threshold*100))
self._failed = True
return 'failed'
elif plan_fraction > success_threshold:
Logger.loginfo('Success! The plan fraction was above %d%%' % (success_threshold*100))
self._done = True
return 'done'
else:
Logger.logwarn('Partial success.The plan fraction was between %d%% and %d%%' % (fail_threshold*100, success_threshold*100))
self._incomplete = True
return 'incomplete'
else:
Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''Handle cartesian/circular affordance, set options, and send goal.'''
self._failed = False
self._done = False
self._incomplete = False
planning_group_str = 'l_arm_group' if userdata.hand == PlanAffordanceState.LEFT_HAND else 'r_arm_group'
affordance = userdata.affordance
print affordance #TODO: remove temp debug statement
# Create the manipulation action goal
self._client.new_goal(move_group = planning_group_str)
if affordance.type == "circular":
self._client.set_circular_motion(angle = affordance.displacement,
pitch = affordance.pitch)
for waypoint in affordance.waypoints: # There should only be one
self._client.add_endeffector_pose(waypoint.pose,
waypoint.header.frame_id)
print waypoint #TODO: remove temp debug statement
elif affordance.type == "cartesian":
self._client.set_cartesian_motion()
try:
srv_request = GetAffordanceInWristFrameRequest(affordance)
srv_result = self._srv.call(self._service_topics[userdata.hand],
srv_request)
wrist_affordance = srv_result.wrist_affordance # PoseStamped
print wrist_affordance #TODO: remove temp debug statement
self._client.add_endeffector_pose(wrist_affordance.pose,
wrist_affordance.header.frame_id)
except Exception as e:
Logger.logwarn('Failed to transform affordance to wrist frame: \n%s' % str(e))
self._failed = True
return
else:
Logger.logwarn("Unknown affordance type: %s" % affordance.type)
self._failed = True
return
# Set planning options
self._client.set_planner_id(self._planner_id)
self._client.set_velocity_scaling(self._vel_scaling)
self._client.set_collision_avoidance(True)
self._client.set_execute_incomplete_plans(True)
self._client.set_keep_orientation(affordance.keep_orientation)
#Set reference_point and frame, if provided. Defaults to palm.
if userdata.reference_point is not None:
self._client.set_reference_point(
pose = userdata.reference_point.pose,
frame_id = userdata.reference_point.header.frame_id)
#Set Drake specific options
self._client.set_drake_target_orientation_type(self._drake_orientation_type)
self._client.set_drake_target_link_axis(self._drake_link_axis)
self._client.set_drake_trajectory_sample_rate(self._drake_sample_rate)
try:
self._client.start_planning()
except Exception as e:
Logger.logwarn('Could not request a plan for the affordance!\n%s' % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.finished():
self._client.cancel()
Logger.loginfo("Cancelled active action goal.")
############################################################################
def set_up_affordance_service(self):
hands_in_use = self.determine_hands_in_use()
# Set up services for transforming cartesian affordances to wrist frame
self._service_topics = dict()
for hand in hands_in_use:
topic = '/manipulation_control/%s_hand/wrist_affordance_srv' % hand[0]
self._service_topics[hand] = topic
self._srv = ProxyServiceCaller(
{t: GetAffordanceInWristFrame for t in self._service_topics.values()})
def determine_hands_in_use(self):
'''Determine which hand types and sides have been sourced'''
if not rospy.has_param("behavior/hand_type_prefix"):
Logger.logerr("Need to specify behavior/parameter hand_type_prefix at the parameter server")
return
hand_type_prefix = rospy.get_param("behavior/hand_type_prefix")
hands_in_use = list()
LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE']
RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE']
if LEFT_HAND not in ['l_stump', 'l_hook']:
hands_in_use.append('left')
if RIGHT_HAND not in ['r_stump', 'r_hook']:
hands_in_use.append('right')
if len(hands_in_use) == 0:
Logger.logerr('No hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (LEFT_HAND, RIGHT_HAND))
return hands_in_use
class PlanEndeffectorCartesianWaypointsState(EventState):
'''
Plans a cartesian endeffector trajectory passing all given waypoints.
-- ignore_collisions boolean Should collisions be ignored? Only pass True if you are sure that it is safe.
-- include_torso boolean Should the torso be included in the trajectory or stay fixed?
-- keep_endeffector_orientation boolean Should the initial orientation of the endeffector be kept?
-- allow_incomplete_plans boolean Should incomplete plans be allowed?
-- vel_scaling string Scales the velocity of the motion, lower values for slower trajectories.
-- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault")
># waypoints Pose[] Waypoints for the endeffector motion.
># hand string One of the class constants determining the hand to be moved.
># frame_id string Frame in which the waypoints are given.
#> joint_trajectory JointTrajectory Trajectory of the endeffector to perform the requested motion.
#> plan_fraction float Fraction of the requested motion which could be planned.
A value below 1 indicates a partial motion.
<= planned Was able to generate a valid motion plan.
This plan is not necessarily complete.
<= failed Failed to create a motion plan at all.
'''
LEFT_HAND = 'left'
RIGHT_HAND = 'right'
def __init__(self,
ignore_collisions=False,
include_torso=False,
keep_endeffector_orientation=False,
allow_incomplete_plans=False,
vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"):
'''Constructor'''
super(PlanEndeffectorCartesianWaypointsState,
self).__init__(outcomes=['planned', 'incomplete', 'failed'],
input_keys=['waypoints', 'hand', 'frame_id'],
output_keys=['joint_trajectory', 'plan_fraction'])
self._client = ProxyMoveitClient()
self._ignore_collisions = ignore_collisions
self._include_torso = include_torso
self._keep_endeffector_orientation = keep_endeffector_orientation
self._allow_incomplete_plans = allow_incomplete_plans
self._vel_scaling = vel_scaling
self._planner_id = planner_id
self._failed = False
self._done = False
self._incomplete = False
def execute(self, userdata):
'''
Execute this state
'''
fail_threshold = 0.05
success_threshold = 0.95
if self._failed:
return 'failed'
if self._done:
return 'planned'
if self._incomplete:
return 'incomplete'
if self._client.finished():
userdata.joint_trajectory = self._client.get_plan()
if self._client.success():
# Incomplete plans are considered a SUCCESS, if we allow them
plan_fraction = self._client.get_plan_fraction()
Logger.loginfo('Plan completion fraction = %.2f%%' %
(plan_fraction * 100))
userdata.plan_fraction = plan_fraction
if plan_fraction < fail_threshold:
rospy.logwarn('Failure! The plan fraction was below %d%%' %
(fail_threshold * 100))
self._failed = True
return 'failed'
elif plan_fraction > success_threshold:
rospy.loginfo('Success! The plan fraction was above %d%%' %
(success_threshold * 100))
self._done = True
return 'planned'
else:
rospy.logwarn(
'Partial success.The plan fraction was between %d%% and %d%%'
% (fail_threshold * 100, success_threshold * 100))
self._incomplete = True
return 'incomplete'
else:
Logger.logwarn('MoveIt failed with the following error: %s' %
self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''...'''
if not hasattr(userdata, 'waypoints') or userdata.waypoints is None \
or not hasattr(userdata, 'hand') or userdata.hand is None \
or not hasattr(userdata, 'frame_id') or userdata.frame_id is None:
self._failed = True
Logger.logwarn(
'Userdata key of state %s does not exist or is currently undefined!'
% self.name)
return
self._failed = False
self._done = False
self._incomplete = False
planning_group_str = \
('l' if userdata.hand == self.LEFT_HAND else 'r') + \
'_arm' + \
('_with_torso' if self._include_torso else '') + \
'_group'
# create the motion goal
self._client.new_goal(planning_group_str)
self._client.set_collision_avoidance(self._ignore_collisions)
self._client.set_cartesian_motion()
self._client.set_keep_orientation(self._keep_endeffector_orientation)
self._client.set_execute_incomplete_plans(self._allow_incomplete_plans)
self._client.set_velocity_scaling(self._vel_scaling)
self._client.set_planner_id(self._planner_id)
for pose in userdata.waypoints:
self._client.add_endeffector_pose(pose, userdata.frame_id)
rospy.loginfo(
'Sending planning request to pass %d waypoints in frame %s...' %
(len(userdata.waypoints), userdata.frame_id))
try:
self._client.start_planning()
except Exception as e:
Logger.logwarn('Could not request a plan!\n%s' % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.finished():
self._client.cancel()
Logger.loginfo("Cancelled active action goal.")
