class MoveitMoveGroupState(EventState):
"""
Uses moveit to plan to the given joint configuration and executes the resulting trajctory.
-- planning_group string Name of the planning group.
># target_joint_config float[] Target joint values to plan and move to.
<= reached Requested joint configuration has been reached successfully.
<= failed Failed to reach requested joint configuration.
"""
def __init__(self, planning_group):
"""Constructor"""
super(MoveitMoveGroupState,
self).__init__(outcomes=['reached', 'failed'],
input_keys=['target_joint_config'])
self._client = ProxyMoveitClient()
self._failed = False
self._planning_group = planning_group
def execute(self, userdata):
"""Execute this state"""
if self._failed:
return 'failed'
if self._client.finished():
if self._client.success():
return 'reached'
else:
Logger.logwarn('MoveIt failed with the following error: %s' %
self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
# create the motion goal
self._client.new_goal(self._planning_group)
self._client.add_joint_values(userdata.target_joint_config)
# execute the motion
try:
Logger.loginfo("Moving %s to: %s" %
(self._planning_group, ", ".join(
map(str, userdata.target_joint_config))))
self._client.start_execution()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\n%s' %
str(e))
self._failed = True
class MoveitMoveGroupState(EventState):
"""
Uses moveit to plan to the given joint configuration and executes the resulting trajctory.
-- planning_group string Name of the planning group.
># target_joint_config float[] Target joint values to plan and move to.
<= reached Requested joint configuration has been reached successfully.
<= failed Failed to reach requested joint configuration.
"""
def __init__(self, planning_group):
"""Constructor"""
super(MoveitMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
input_keys=['target_joint_config'])
self._client = ProxyMoveitClient()
self._failed = False
self._planning_group = planning_group
def execute(self, userdata):
"""Execute this state"""
if self._failed:
return 'failed'
if self._client.finished():
if self._client.success():
return 'reached'
else:
Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
# create the motion goal
self._client.new_goal(self._planning_group)
self._client.add_joint_values(userdata.target_joint_config)
# execute the motion
try:
Logger.loginfo("Moving %s to: %s" % (self._planning_group, ", ".join(map(str, userdata.target_joint_config))))
self._client.start_execution()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
self._failed = True
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 PlanEndeffectorCircularMotionState(EventState):
'''
Plans a circular motion of the endeffector.
-- 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?
-- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault")
># hand string One of the class constants determining the hand to be moved.
># rotation_axis PoseStamped Pose in the center of the rotation, can be retrieved from a template pose.
># rotation_angle float Angle of the rotation in radians.
#> 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, planner_id = "RRTConnectkConfigDefault"):
'''
Constructor
'''
super(PlanEndeffectorCircularMotionState, self).__init__(outcomes=['planned', 'failed'],
input_keys=['hand', 'rotation_axis', 'rotation_angle'],
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._failed = False
self._planner_id = planner_id
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._client.finished():
userdata.joint_trajectory = self._client.get_plan()
if self._client.success():
userdata.plan_fraction = 1
return 'planned'
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, 'rotation_axis') or userdata.rotation_axis is None \
or not hasattr(userdata, 'hand') or userdata.hand is None \
or not hasattr(userdata, 'rotation_angle') or userdata.rotation_angle is None:
self._failed = True
Logger.logwarn('Userdata key of state %s does not exist or is currently undefined!' % self.name)
return
center_pose = userdata.rotation_axis
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_circular_motion(userdata.rotation_angle)
self._client.set_keep_orientation(self._keep_endeffector_orientation)
self._client.add_endeffector_pose(userdata.rotation_axis.pose, userdata.rotation_axis.header.frame_id) # rotation center
self._client.set_planner_id(self._planner_id)
Logger.loginfo('Sending planning request for circular motion of %f rad...' % userdata.rotation_angle)
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 MoveitPredefinedPoseState(EventState):
"""
Uses moveit to go to one of the pre-defined poses.
-- target_pose int Identifier of the pre-defined pose to be used.
-- vel_scaling float Scales the velocity of the motion.
Lower values for slower trajectories.
-- ignore_collisions boolean Should collisions be ignored? Only pass True if you are sure that it is safe.
-- link_paddings dict link_name (str) : padding (float) pairs
-- is_cartesian boolean Execute as cartesian motion
># side string Arm side, turning direction, etc.
Possible values: {left, right, same}
<= done Successfully executed the motion.
<= failed Failed to execute the motion.
"""
# Arms
STAND_POSE = 0
SINGLE_ARM_STAND = 1 # for preventing unconstrained motion of the other
BOTH_ARMS_SIDES = 2 # for safely holding the tool at the robot's side
SINGLE_ARM_SIDE = 3
STAND_POSE_UP = 4
CALIBRATE_ARMS = 10 # for checking arm calibration with the head camera
WALK_POSE = 19
# Torso
TURN_TORSO_CENTER_POSE = 20
TURN_TORSO_SLIGHTLY = 21
TURN_TORSO_MORE = 22
TURN_TORSO_FULL = 23
# Head
HEAD_CENTER = 30
HEAD_SIDE = 31
# Task-specific poses
# Second digit is task number. Comment is pose in the Position widget.
CAR_ENTRY_ARMS_POSE = 111 # CarEntry
CAR_ENTRY_LEGS_POSE = 112 # CarEntry
CAR_ENTRY_FORE_POSE = 113 # CarEntryFore
CAR_PREDRIVE_LARM_POSE = 114 # pre_drive
CAR_DRIVE_LARM_POSE = 115 # drive
CAR_DRIVE_CAMERA_POSE = 116 # CarCamera
DOOR_READY_POSE = 131
DOOR_OPEN_POSE_TURNED = 132
DOOR_OPEN_POSE_STRAIGHT = 133
DOOR_OPEN_POSE_SIDE = 134 # push_door_3
DOOR_PUSH_SIDE = 136
DOOR_OPEN_TURN_MANIPULATE = 138
DOOR_OPEN_TURN_LIDAR = 139
POKE_READY_POSE = 151
PREP_CUT_WALL_1 = 155
PREP_CUT_WALL_2 = 156
PREP_CUT_WALL_3 = 157
LADDER_READY_POSE = 181
def __init__(self, target_pose,
vel_scaling = 0.1,
ignore_collisions = False,
link_paddings = {},
is_cartesian = False):
"""Constructor"""
super(MoveitPredefinedPoseState, self).__init__(outcomes=['done', 'failed'],
input_keys=['side'])
if not rospy.has_param("behavior/robot_namespace"):
Logger.logerr("Need to specify parameter behavior/robot_namespace at the parameter server")
return
self._robot = rospy.get_param("behavior/robot_namespace")
self._poses = dict()
self._poses['flor'] = dict()
# Position mode widget: src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch
self._poses['flor']['left'] = {
# Joint names: l_arm_shz, l_arm_shx, l_arm_ely, l_arm_elx, l_arm_wry, l_arm_wrx, l_arm_wry2
1: {'group': 'l_arm_group', 'joints': [-0.25, -1.34, 1.88, 0.49, 0.0, 0.0, 0.0]},
3: {'group': 'l_arm_group', 'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4, -0.50]},
10: {'group': 'l_arm_group', 'joints': [-1.0, 0.28, 1.2, 1.6, 0.3, 0.5 , 0.0]},
21: {'group': 'torso_group', 'joints': [+0.20, 0.00, 0.00]},
22: {'group': 'torso_group', 'joints': [+0.40, 0.00, 0.00]},
23: {'group': 'torso_group', 'joints': [+0.55, 0.00, 0.00]},
112: {'group': 'l_leg_group', 'joints': [+0.00, +0.00, -1.60, +1.40, -0.50, 0.00]}, # Safety pose
114: {'group': 'l_arm_group', 'joints': [+0.76, -0.94, 0.80, 2.00, +1.00, -0.20, -1.35]}, # pre_drive
115: {'group': 'l_arm_group', 'joints': [+0.11, -0.16, 1.75, 1.60, +1.00, -0.90, -1.00]}, # drive
116: {'group': 'l_arm_group', 'joints': []}, # We use the right hand for the camera!
131: {'group': 'l_arm_group', 'joints': [-0.29, -0.22, 1.87, 2.17, -0.17, 0.81, 0.12]},
132: {'group': 'l_arm_group', 'joints': [-0.70, -0.09, 1.93, 0.66, -0.15, 1.52, 0.12]},
133: {'group': 'l_arm_group', 'joints': [-1.38, -0.16, 1.82, 0.57, -0.19, 1.52, 0.12]},
134: {'group': 'l_arm_group', 'joints': []}, # Most probably will never be used
151: {'group': 'l_arm_group', 'joints': [-1.01, -0.43, +1.32, +1.67, -0.91, +1.46, +0.98]},
155: {'group': 'l_arm_group', 'joints': [0.0, -0.37, 2.65, 1.4, -0.2, 0.9 , -1.54]},
156: {'group': 'l_arm_group', 'joints': [-0.32, -0.9, 2.2, 1.3, 0.5, 1.0 , -1.8]},
157: {'group': 'l_arm_group', 'joints': [-0.45, -1.0, 2.1, 1.3, 0.5, 0.8 , -0.8]}
}
self._poses['flor']['same'] = {
0: {'group': 'both_arms_group', 'joints': [-0.25, -1.34, +1.88, +0.49, +0.00, +0.00, +0.00, \
+0.25, +1.34, +1.88, -0.49, +0.00, +0.00, +0.00]},
2: {'group': 'both_arms_group', 'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4 , -0.5, \
-0.72, 0.95, 2.7, -0.95, 0.0, 0.4 , -0.5]},
19: {'group': 'both_arms_group', 'joints': [-1.5, -1.5, +0.30, +0.50, +0.0, +0.8, +0.00, \
+1.5, +1.5, +0.30, -0.50, +0.0, -0.8, +0.00]},
20: {'group': 'torso_group', 'joints': [+0.00, 0.00, 0.00]},
111: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, +0.00, 0.0, \
+0.00, +1.50, +0.00, -1.72, 0.00, +0.00, 0.0]},
113: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, -1.57, 0.0, \
+0.00, +1.50, +0.00, -1.72, 0.00, +1.57, 0.0]},
181: {'group': 'both_arms_group', 'joints': [-1.53, -0.69, +0.12, +1.47, 0.00, +0.88, 0.00, \
+1.53, +0.69, +0.12, -1.47, 0.00, -0.88, 0.00]}
}
self._poses['flor']['right'] = {
# Joint names: r_arm_shz, r_arm_shx, r_arm_ely, r_arm_elx, r_arm_wry, r_arm_wrx, r_arm_wry2
1: {'group': 'r_arm_group', 'joints': [+0.25, 1.34, 1.88, -0.49, 0.0, 0.0, 0.0]},
3: {'group': 'r_arm_group', 'joints': [-0.72, 0.95, 2.7, -0.95, 0.0, 0.4, -0.50]},
10: {'group': 'r_arm_group', 'joints': [+1.0, -0.28, 1.2, -1.6, 0.3, -0.5 , 0.0]},
21: {'group': 'torso_group', 'joints': [-0.20, 0.00, 0.00]},
22: {'group': 'torso_group', 'joints': [-0.40, 0.00, 0.00]},
23: {'group': 'torso_group', 'joints': [-0.55, 0.00, 0.00]},
112: {'group': 'r_leg_group', 'joints': [+0.00, +0.00, -1.34, +1.23, 0.00, 0.00]},
115: {'group': 'r_arm_group', 'joints': []}, # Driving is done with the left arm!
116: {'group': 'r_arm_group', 'joints': [+0.90, 0.17, 0.50, -1.58, -0.70, +1.50, -0.34]}, # CarCamera
131: {'group': 'r_arm_group', 'joints': [+0.29, 0.22, 1.87, -2.17, -0.17, -0.81, 0.12]},
132: {'group': 'r_arm_group', 'joints': [+0.70, 0.09, 1.93, -0.66, -0.15, -1.52, 0.12]},
133: {'group': 'r_arm_group', 'joints': [+1.38, 0.16, 1.82, -0.57, -0.19, -1.52, 0.12]},
134: {'group': 'r_arm_group', 'joints': [+0.00, +0.54, +0.94, -1.04, 0.80, 0.5, 0.7]},
151: {'group': 'r_arm_group', 'joints': [+1.01, +0.43, +1.32, -1.67, -0.91, -1.46, +0.98]},
155: {'group': 'r_arm_group', 'joints': [+0.00, +0.37, +2.65, -1.40, -0.20, -0.90, -1.54]},
156: {'group': 'r_arm_group', 'joints': [+0.32, +0.90, +2.20, -1.30, +0.50, -1.00, -1.80]},
157: {'group': 'r_arm_group', 'joints': [0.45, 1.0, 2.1, -1.3, 0.5, -0.8 , -0.8]}
}
self._poses['thor_mang'] = dict()
self._poses['thor_mang']['left'] = {
1: {'group': 'l_arm_group', 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.57080884130538, -0.25205140042963, 0.01563815008, 0]},
3: {'group': 'l_arm_group', 'joints': [0.77, -0.27, 0.02, -1.21, -0.25, 0.02, 0]},
4: {'group': 'l_arm_group', 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.97080884130538, -0.25205140042963, 0.01563815008, 0]},
21: {'group': 'torso_group', 'joints': [+0.30, 0.03]},
22: {'group': 'torso_group', 'joints': [+0.60, 0.03]},
23: {'group': 'torso_group', 'joints': [+1.02, 0.03]},
31: {'group': 'head_group', 'joints': [1.57, 0.00]},
131: {'group': 'l_arm_group', 'joints': [-1.73, -0.69, 1.75, -1.86, 0.04, -0.72, 1.63]},
132: {'group': 'l_arm_group', 'joints': [-1.76, -1.13, 1.68, -0.55, 0.02, -1.81, 1.63]},
133: {'group': 'l_arm_group', 'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]},
134: {'group': 'l_arm_group', 'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]}, # placeholder
136: {'group': 'l_arm_group', 'joints': [-2.31, -2.2, 1.83, -1.22, 4.1, 0.14, -2.08]},
138: {'group': 'torso_group', 'joints': [0.92, 0]},
139: {'group': 'torso_group', 'joints': [1.3, 0]},
151: {'group': 'l_arm_group', 'joints': [-1.45, -1.26, 1.10, -2.02, -0.19, 0.04, -2.86]},
155: {'group': 'l_arm_group', 'joints': [-0.25, -0.75, 0.02, -1.21, -0.38, -1.36, -0.9]},
156: {'group': 'l_arm_group', 'joints': [-0.25, -0.25, 0.02, -1.21, -0.38, -1.36, -0.9]},
157: {'group': 'l_arm_group', 'joints': [-0.25, -0.13, 0.02, -1.21, -0.13, -1.55, 0.41]}
}
self._poses['thor_mang']['same'] = {
0: {'group': 'both_arms_group', 'joints': [0.79, -0.27, 0, -1.57, 1.55, 0, 0, -0.79, 0.27, 0, 1.57, -1.55, 0, 0]},
2: {'group': 'both_arms_group', 'joints': [-0.37, 0.0, 1.57, 0, 0, 0, 0, 0.37, 0.0, 1.57, 0, 0, 0, 0]},
20: {'group': 'torso_group', 'joints': [0.00, 0.03]},
30: {'group': 'head_group', 'joints': [0.00, 0.00]},
181: {'group': 'both_arms_group', 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.57080884130538, -0.25205140042963, 0.01563815008, 0, -0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.57080884130538, 0.25205140042963, -0.01563815008, 0]}
}
self._poses['thor_mang']['right'] = {
1: {'group': 'r_arm_group', 'joints': [-0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.57080884130538, 0.25205140042963, -0.01563815008, 0]},
3: {'group': 'r_arm_group', 'joints': [-0.77, 0.27, -0.02, 1.21, 0.25, -0.02, 0]},
4: {'group': 'r_arm_group', 'joints': [-0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.97080884130538, 0.25205140042963, -0.01563815008, 0]},
21: {'group': 'torso_group', 'joints': [-0.30, 0.03]},
22: {'group': 'torso_group', 'joints': [-0.60, 0.03]},
23: {'group': 'torso_group', 'joints': [-1.02, 0.03]},
31: {'group': 'head_group', 'joints': [-1.57, 0.00]},
131: {'group': 'r_arm_group', 'joints': [1.73, 0.69, -1.75, 1.86, -0.04, 0.72, -1.63]},
132: {'group': 'r_arm_group', 'joints': [1.76, 1.13, -1.68, 0.55, -0.02, 1.81, -1.63]},
133: {'group': 'r_arm_group', 'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]},
134: {'group': 'r_arm_group', 'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]}, # placeholder
136: {'group': 'r_arm_group', 'joints': [2.31, 2.2, -1.83, 1.22, -4.1, -0.14, 2.08]},
138: {'group': 'torso_group', 'joints': [-0.92, 0]},
139: {'group': 'torso_group', 'joints': [-1.3, 0]},
151: {'group': 'r_arm_group', 'joints': [1.45, 1.26, -1.10, 2.02, 0.19, -0.04, 2.86]},
155: {'group': 'r_arm_group', 'joints': [0.25, 0.75, -0.02, 1.21, 0.38, 1.36, 0.9]},
156: {'group': 'r_arm_group', 'joints': [0.25, 0.25, -0.02, 1.21, 0.38, 1.36, 0.9]},
157: {'group': 'r_arm_group', 'joints': [0.25, 0.13, -0.02, 1.21, 0.13, 1.55, -0.41]}
}
self._target_pose = target_pose
self._vel_scaling = vel_scaling
self._ignore_collisions = ignore_collisions
self._link_paddings = link_paddings
self._is_cartesian = is_cartesian
self._client = ProxyMoveitClient()
self._failed = False
def execute(self, userdata):
"""Execute this state"""
if self._failed:
return 'failed'
if self._client.finished():
if self._client.success():
return 'done'
else:
Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
planning_group = ""
joint_values = []
if userdata.side == 'left' and self._target_pose in self._poses[self._robot]['left'].keys():
planning_group = self._poses[self._robot]['left'][self._target_pose]['group']
joint_values = self._poses[self._robot]['left'][self._target_pose]['joints']
elif userdata.side == 'right' and self._target_pose in self._poses[self._robot]['right'].keys():
planning_group = self._poses[self._robot]['right'][self._target_pose]['group']
joint_values = self._poses[self._robot]['right'][self._target_pose]['joints']
elif self._target_pose in self._poses[self._robot]['same'].keys():
planning_group = self._poses[self._robot]['same'][self._target_pose]['group']
joint_values = self._poses[self._robot]['same'][self._target_pose]['joints']
else:
Logger.logwarn('Specified target pose %d is not specified for %s side' % (self._target_pose, userdata.side))
self._failed = True
return
# create the motion goal
self._client.new_goal(planning_group)
self._client.add_joint_values(joint_values)
self._client.set_velocity_scaling(self._vel_scaling)
self._client.set_collision_avoidance(self._ignore_collisions)
self._client.add_link_padding(link_paddings = self._link_paddings) #dict
if self._is_cartesian:
self._client.set_cartesian_motion()
# execute the motion
try:
self._client.start_execution()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\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 PlanEndeffectorPoseState(EventState):
'''
Plans to reach the given pose with the specified endeffector.
-- 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?
-- allowed_collisions tuple[] List of 2-tuples (strings) for each pair of links which is allowed to collide.
Set the second value to an empty string if collision to any other link is allowed.
-- planner_id string Sets the ID of the planner to use (MoveIt planner id or "drake" - default = "RRTConnectkConfigDefault")
># target_pose PoseStamped Target pose to reach.
># hand string One of the class constants determining the hand to be moved.
#> joint_trajectory JointTrajectory Trajectory of the end effector to perform the requested motion.
<= planned Was able to generate a valid motion plan.
<= failed Failed to create a motion plan at all.
'''
LEFT_HAND = 'left'
RIGHT_HAND = 'right'
def __init__(self, ignore_collisions = False, include_torso = False, allowed_collisions = [], planner_id = "RRTConnectkConfigDefault"):
'''
Constructor
'''
super(PlanEndeffectorPoseState, self).__init__(outcomes=['planned', 'failed'],
input_keys=['target_pose', 'hand'],
output_keys=['joint_trajectory'])
self._client = ProxyMoveitClient()
self._ignore_collisions = ignore_collisions
self._include_torso = include_torso
self._allowed_collisions = allowed_collisions
self._failed = False
self._planner_id = planner_id
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._client.finished():
userdata.joint_trajectory = self._client.get_plan()
if self._client.success():
return 'planned'
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, 'target_pose') or userdata.target_pose is None:
self._failed = True
Logger.logwarn('Userdata "target_pose" of state %s does not exist or is currently undefined!' % self.name)
return
if not hasattr(userdata, 'hand') or userdata.hand is None:
self._failed = True
Logger.logwarn('Userdata "hand" of state %s does not exist or is currently undefined!' % self.name)
return
self._failed = 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.add_endeffector_pose(userdata.target_pose.pose, userdata.target_pose.header.frame_id)
self._client.set_collision_avoidance(self._ignore_collisions)
self._client.set_planner_id(self._planner_id)
for link, target in self._allowed_collisions:
self._client.set_allowed_collision(link, target)
# for later use
#for link, target in self._allowed_collisions:
Logger.loginfo('Sending planning request to reach point (%f, %f, %f) in frame %s...' % (userdata.target_pose.pose.position.x, userdata.target_pose.pose.position.y, userdata.target_pose.pose.position.z, userdata.target_pose.header.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 MoveitPredefinedPoseState(EventState):
"""
Uses moveit to go to one of the pre-defined poses.
-- target_pose int Identifier of the pre-defined pose to be used.
-- vel_scaling float Scales the velocity of the motion.
Lower values for slower trajectories.
-- ignore_collisions boolean Should collisions be ignored? Only pass True if you are sure that it is safe.
-- link_paddings dict link_name (str) : padding (float) pairs
-- is_cartesian boolean Execute as cartesian motion
># side string Arm side, turning direction, etc.
Possible values: {left, right, same}
<= done Successfully executed the motion.
<= failed Failed to execute the motion.
"""
# Arms
STAND_POSE = 0
SINGLE_ARM_STAND = 1 # for preventing unconstrained motion of the other
BOTH_ARMS_SIDES = 2 # for safely holding the tool at the robot's side
SINGLE_ARM_SIDE = 3
STAND_POSE_UP = 4
CALIBRATE_ARMS = 10 # for checking arm calibration with the head camera
WALK_POSE = 19
# Torso
TURN_TORSO_CENTER_POSE = 20
TURN_TORSO_SLIGHTLY = 21
TURN_TORSO_MORE = 22
TURN_TORSO_FULL = 23
# Head
HEAD_CENTER = 30
HEAD_SIDE = 31
# Task-specific poses
# Second digit is task number. Comment is pose in the Position widget.
CAR_ENTRY_ARMS_POSE = 111 # CarEntry
CAR_ENTRY_LEGS_POSE = 112 # CarEntry
CAR_ENTRY_FORE_POSE = 113 # CarEntryFore
CAR_PREDRIVE_LARM_POSE = 114 # pre_drive
CAR_DRIVE_LARM_POSE = 115 # drive
CAR_DRIVE_CAMERA_POSE = 116 # CarCamera
DOOR_READY_POSE = 131
DOOR_OPEN_POSE_TURNED = 132
DOOR_OPEN_POSE_STRAIGHT = 133
DOOR_OPEN_POSE_SIDE = 134 # push_door_3
DOOR_PUSH_SIDE = 136
DOOR_OPEN_TURN_MANIPULATE = 138
DOOR_OPEN_TURN_LIDAR = 139
POKE_READY_POSE = 151
PREP_CUT_WALL_1 = 155
PREP_CUT_WALL_2 = 156
PREP_CUT_WALL_3 = 157
LADDER_READY_POSE = 181
def __init__(self,
target_pose,
vel_scaling=0.1,
ignore_collisions=False,
link_paddings={},
is_cartesian=False):
"""Constructor"""
super(MoveitPredefinedPoseState,
self).__init__(outcomes=['done', 'failed'], input_keys=['side'])
if not rospy.has_param("behavior/robot_namespace"):
Logger.logerr(
"Need to specify parameter behavior/robot_namespace at the parameter server"
)
return
self._robot = rospy.get_param("behavior/robot_namespace")
self._poses = dict()
self._poses['flor'] = dict()
# Position mode widget: src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch
self._poses['flor']['left'] = {
# Joint names: l_arm_shz, l_arm_shx, l_arm_ely, l_arm_elx, l_arm_wry, l_arm_wrx, l_arm_wry2
1: {
'group': 'l_arm_group',
'joints': [-0.25, -1.34, 1.88, 0.49, 0.0, 0.0, 0.0]
},
3: {
'group': 'l_arm_group',
'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4, -0.50]
},
10: {
'group': 'l_arm_group',
'joints': [-1.0, 0.28, 1.2, 1.6, 0.3, 0.5, 0.0]
},
21: {
'group': 'torso_group',
'joints': [+0.20, 0.00, 0.00]
},
22: {
'group': 'torso_group',
'joints': [+0.40, 0.00, 0.00]
},
23: {
'group': 'torso_group',
'joints': [+0.55, 0.00, 0.00]
},
112: {
'group': 'l_leg_group',
'joints': [+0.00, +0.00, -1.60, +1.40, -0.50, 0.00]
}, # Safety pose
114: {
'group': 'l_arm_group',
'joints': [+0.76, -0.94, 0.80, 2.00, +1.00, -0.20, -1.35]
}, # pre_drive
115: {
'group': 'l_arm_group',
'joints': [+0.11, -0.16, 1.75, 1.60, +1.00, -0.90, -1.00]
}, # drive
116: {
'group': 'l_arm_group',
'joints': []
}, # We use the right hand for the camera!
131: {
'group': 'l_arm_group',
'joints': [-0.29, -0.22, 1.87, 2.17, -0.17, 0.81, 0.12]
},
132: {
'group': 'l_arm_group',
'joints': [-0.70, -0.09, 1.93, 0.66, -0.15, 1.52, 0.12]
},
133: {
'group': 'l_arm_group',
'joints': [-1.38, -0.16, 1.82, 0.57, -0.19, 1.52, 0.12]
},
134: {
'group': 'l_arm_group',
'joints': []
}, # Most probably will never be used
151: {
'group': 'l_arm_group',
'joints': [-1.01, -0.43, +1.32, +1.67, -0.91, +1.46, +0.98]
},
155: {
'group': 'l_arm_group',
'joints': [0.0, -0.37, 2.65, 1.4, -0.2, 0.9, -1.54]
},
156: {
'group': 'l_arm_group',
'joints': [-0.32, -0.9, 2.2, 1.3, 0.5, 1.0, -1.8]
},
157: {
'group': 'l_arm_group',
'joints': [-0.45, -1.0, 2.1, 1.3, 0.5, 0.8, -0.8]
}
}
self._poses['flor']['same'] = {
0: {'group': 'both_arms_group', 'joints': [-0.25, -1.34, +1.88, +0.49, +0.00, +0.00, +0.00, \
+0.25, +1.34, +1.88, -0.49, +0.00, +0.00, +0.00]},
2: {'group': 'both_arms_group', 'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4 , -0.5, \
-0.72, 0.95, 2.7, -0.95, 0.0, 0.4 , -0.5]},
19: {'group': 'both_arms_group', 'joints': [-1.5, -1.5, +0.30, +0.50, +0.0, +0.8, +0.00, \
+1.5, +1.5, +0.30, -0.50, +0.0, -0.8, +0.00]},
20: {'group': 'torso_group', 'joints': [+0.00, 0.00, 0.00]},
111: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, +0.00, 0.0, \
+0.00, +1.50, +0.00, -1.72, 0.00, +0.00, 0.0]},
113: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, -1.57, 0.0, \
+0.00, +1.50, +0.00, -1.72, 0.00, +1.57, 0.0]},
181: {'group': 'both_arms_group', 'joints': [-1.53, -0.69, +0.12, +1.47, 0.00, +0.88, 0.00, \
+1.53, +0.69, +0.12, -1.47, 0.00, -0.88, 0.00]}
}
self._poses['flor']['right'] = {
# Joint names: r_arm_shz, r_arm_shx, r_arm_ely, r_arm_elx, r_arm_wry, r_arm_wrx, r_arm_wry2
1: {
'group': 'r_arm_group',
'joints': [+0.25, 1.34, 1.88, -0.49, 0.0, 0.0, 0.0]
},
3: {
'group': 'r_arm_group',
'joints': [-0.72, 0.95, 2.7, -0.95, 0.0, 0.4, -0.50]
},
10: {
'group': 'r_arm_group',
'joints': [+1.0, -0.28, 1.2, -1.6, 0.3, -0.5, 0.0]
},
21: {
'group': 'torso_group',
'joints': [-0.20, 0.00, 0.00]
},
22: {
'group': 'torso_group',
'joints': [-0.40, 0.00, 0.00]
},
23: {
'group': 'torso_group',
'joints': [-0.55, 0.00, 0.00]
},
112: {
'group': 'r_leg_group',
'joints': [+0.00, +0.00, -1.34, +1.23, 0.00, 0.00]
},
115: {
'group': 'r_arm_group',
'joints': []
}, # Driving is done with the left arm!
116: {
'group': 'r_arm_group',
'joints': [+0.90, 0.17, 0.50, -1.58, -0.70, +1.50, -0.34]
}, # CarCamera
131: {
'group': 'r_arm_group',
'joints': [+0.29, 0.22, 1.87, -2.17, -0.17, -0.81, 0.12]
},
132: {
'group': 'r_arm_group',
'joints': [+0.70, 0.09, 1.93, -0.66, -0.15, -1.52, 0.12]
},
133: {
'group': 'r_arm_group',
'joints': [+1.38, 0.16, 1.82, -0.57, -0.19, -1.52, 0.12]
},
134: {
'group': 'r_arm_group',
'joints': [+0.00, +0.54, +0.94, -1.04, 0.80, 0.5, 0.7]
},
151: {
'group': 'r_arm_group',
'joints': [+1.01, +0.43, +1.32, -1.67, -0.91, -1.46, +0.98]
},
155: {
'group': 'r_arm_group',
'joints': [+0.00, +0.37, +2.65, -1.40, -0.20, -0.90, -1.54]
},
156: {
'group': 'r_arm_group',
'joints': [+0.32, +0.90, +2.20, -1.30, +0.50, -1.00, -1.80]
},
157: {
'group': 'r_arm_group',
'joints': [0.45, 1.0, 2.1, -1.3, 0.5, -0.8, -0.8]
}
}
self._poses['thor_mang'] = dict()
self._poses['thor_mang']['left'] = {
1: {
'group':
'l_arm_group',
'joints': [
0.785385646194622, -0.281153767716932,
0.000600782658167331, -1.57080884130538, -0.25205140042963,
0.01563815008, 0
]
},
3: {
'group': 'l_arm_group',
'joints': [0.77, -0.27, 0.02, -1.21, -0.25, 0.02, 0]
},
4: {
'group':
'l_arm_group',
'joints': [
0.785385646194622, -0.281153767716932,
0.000600782658167331, -1.97080884130538, -0.25205140042963,
0.01563815008, 0
]
},
21: {
'group': 'torso_group',
'joints': [+0.30, 0.03]
},
22: {
'group': 'torso_group',
'joints': [+0.60, 0.03]
},
23: {
'group': 'torso_group',
'joints': [+1.02, 0.03]
},
31: {
'group': 'head_group',
'joints': [1.57, 0.00]
},
131: {
'group': 'l_arm_group',
'joints': [-1.73, -0.69, 1.75, -1.86, 0.04, -0.72, 1.63]
},
132: {
'group': 'l_arm_group',
'joints': [-1.76, -1.13, 1.68, -0.55, 0.02, -1.81, 1.63]
},
133: {
'group': 'l_arm_group',
'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]
},
134: {
'group': 'l_arm_group',
'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]
}, # placeholder
136: {
'group': 'l_arm_group',
'joints': [-2.31, -2.2, 1.83, -1.22, 4.1, 0.14, -2.08]
},
138: {
'group': 'torso_group',
'joints': [0.92, 0]
},
139: {
'group': 'torso_group',
'joints': [1.3, 0]
},
151: {
'group': 'l_arm_group',
'joints': [-1.45, -1.26, 1.10, -2.02, -0.19, 0.04, -2.86]
},
155: {
'group': 'l_arm_group',
'joints': [-0.25, -0.75, 0.02, -1.21, -0.38, -1.36, -0.9]
},
156: {
'group': 'l_arm_group',
'joints': [-0.25, -0.25, 0.02, -1.21, -0.38, -1.36, -0.9]
},
157: {
'group': 'l_arm_group',
'joints': [-0.25, -0.13, 0.02, -1.21, -0.13, -1.55, 0.41]
}
}
self._poses['thor_mang']['same'] = {
0: {
'group':
'both_arms_group',
'joints': [
0.79, -0.27, 0, -1.57, 1.55, 0, 0, -0.79, 0.27, 0, 1.57,
-1.55, 0, 0
]
},
2: {
'group':
'both_arms_group',
'joints':
[-0.37, 0.0, 1.57, 0, 0, 0, 0, 0.37, 0.0, 1.57, 0, 0, 0, 0]
},
20: {
'group': 'torso_group',
'joints': [0.00, 0.03]
},
30: {
'group': 'head_group',
'joints': [0.00, 0.00]
},
181: {
'group':
'both_arms_group',
'joints': [
0.785385646194622, -0.281153767716932,
0.000600782658167331, -1.57080884130538, -0.25205140042963,
0.01563815008, 0, -0.785385646194622, 0.281153767716932,
-0.000600782658167331, 1.57080884130538, 0.25205140042963,
-0.01563815008, 0
]
}
}
self._poses['thor_mang']['right'] = {
1: {
'group':
'r_arm_group',
'joints': [
-0.785385646194622, 0.281153767716932,
-0.000600782658167331, 1.57080884130538, 0.25205140042963,
-0.01563815008, 0
]
},
3: {
'group': 'r_arm_group',
'joints': [-0.77, 0.27, -0.02, 1.21, 0.25, -0.02, 0]
},
4: {
'group':
'r_arm_group',
'joints': [
-0.785385646194622, 0.281153767716932,
-0.000600782658167331, 1.97080884130538, 0.25205140042963,
-0.01563815008, 0
]
},
21: {
'group': 'torso_group',
'joints': [-0.30, 0.03]
},
22: {
'group': 'torso_group',
'joints': [-0.60, 0.03]
},
23: {
'group': 'torso_group',
'joints': [-1.02, 0.03]
},
31: {
'group': 'head_group',
'joints': [-1.57, 0.00]
},
131: {
'group': 'r_arm_group',
'joints': [1.73, 0.69, -1.75, 1.86, -0.04, 0.72, -1.63]
},
132: {
'group': 'r_arm_group',
'joints': [1.76, 1.13, -1.68, 0.55, -0.02, 1.81, -1.63]
},
133: {
'group': 'r_arm_group',
'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]
},
134: {
'group': 'r_arm_group',
'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]
}, # placeholder
136: {
'group': 'r_arm_group',
'joints': [2.31, 2.2, -1.83, 1.22, -4.1, -0.14, 2.08]
},
138: {
'group': 'torso_group',
'joints': [-0.92, 0]
},
139: {
'group': 'torso_group',
'joints': [-1.3, 0]
},
151: {
'group': 'r_arm_group',
'joints': [1.45, 1.26, -1.10, 2.02, 0.19, -0.04, 2.86]
},
155: {
'group': 'r_arm_group',
'joints': [0.25, 0.75, -0.02, 1.21, 0.38, 1.36, 0.9]
},
156: {
'group': 'r_arm_group',
'joints': [0.25, 0.25, -0.02, 1.21, 0.38, 1.36, 0.9]
},
157: {
'group': 'r_arm_group',
'joints': [0.25, 0.13, -0.02, 1.21, 0.13, 1.55, -0.41]
}
}
self._target_pose = target_pose
self._vel_scaling = vel_scaling
self._ignore_collisions = ignore_collisions
self._link_paddings = link_paddings
self._is_cartesian = is_cartesian
self._client = ProxyMoveitClient()
self._failed = False
def execute(self, userdata):
"""Execute this state"""
if self._failed:
return 'failed'
if self._client.finished():
if self._client.success():
return 'done'
else:
Logger.logwarn('MoveIt failed with the following error: %s' %
self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
planning_group = ""
joint_values = []
if userdata.side == 'left' and self._target_pose in self._poses[
self._robot]['left'].keys():
planning_group = self._poses[self._robot]['left'][
self._target_pose]['group']
joint_values = self._poses[self._robot]['left'][
self._target_pose]['joints']
elif userdata.side == 'right' and self._target_pose in self._poses[
self._robot]['right'].keys():
planning_group = self._poses[self._robot]['right'][
self._target_pose]['group']
joint_values = self._poses[self._robot]['right'][
self._target_pose]['joints']
elif self._target_pose in self._poses[self._robot]['same'].keys():
planning_group = self._poses[self._robot]['same'][
self._target_pose]['group']
joint_values = self._poses[self._robot]['same'][
self._target_pose]['joints']
else:
Logger.logwarn(
'Specified target pose %d is not specified for %s side' %
(self._target_pose, userdata.side))
self._failed = True
return
# create the motion goal
self._client.new_goal(planning_group)
self._client.add_joint_values(joint_values)
self._client.set_velocity_scaling(self._vel_scaling)
self._client.set_collision_avoidance(self._ignore_collisions)
self._client.add_link_padding(link_paddings=self._link_paddings) #dict
if self._is_cartesian:
self._client.set_cartesian_motion()
# execute the motion
try:
self._client.start_execution()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\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 MoveitPlanningState(EventState):
"""
Uses moveit to plan to the given joint configuration.
-- planning_group string Name of the planning group.
-- vel_scaling string Scales the velocity of the planned trajectory, lower values for slower trajectories.
># joint_values float[] Target joint values to plan to.
#> joint_trajectory JointTrajectory Planned joint trajectory to reach the goal.
<= planned Requested joint trajectory has been plannd.
<= failed Failed to plan to the requested joint configuration.
"""
def __init__(self, planning_group, vel_scaling=0.1):
"""Constructor"""
super(MoveitPlanningState, self).__init__(outcomes=['planned', 'failed'],
input_keys=['joint_values'],
output_keys=['joint_trajectory'])
self._client = ProxyMoveitClient()
self._planning_group = planning_group
self._vel_scaling = vel_scaling
self._failed = False
def execute(self, userdata):
"""Execute this state"""
if self._failed:
return 'failed'
if self._client.finished():
if self._client.success():
userdata.joint_trajectory = self._client.get_plan()
return 'planned'
else:
Logger.logwarn('MoveIt failed with the following error: %s' % self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
# create the motion goal
self._client.new_goal(self._planning_group)
self._client.add_joint_values(userdata.joint_values)
self._client.set_velocity_scaling(self._vel_scaling)
# execute the motion
try:
self._client.start_planning()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\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 MoveitPlanningState(EventState):
"""
Uses moveit to plan to the given joint configuration.
-- planning_group string Name of the planning group.
-- vel_scaling string Scales the velocity of the planned trajectory, lower values for slower trajectories.
># joint_values float[] Target joint values to plan to.
#> joint_trajectory JointTrajectory Planned joint trajectory to reach the goal.
<= planned Requested joint trajectory has been plannd.
<= failed Failed to plan to the requested joint configuration.
"""
def __init__(self, planning_group, vel_scaling=0.1):
"""Constructor"""
super(MoveitPlanningState,
self).__init__(outcomes=['planned', 'failed'],
input_keys=['joint_values'],
output_keys=['joint_trajectory'])
self._client = ProxyMoveitClient()
self._planning_group = planning_group
self._vel_scaling = vel_scaling
self._failed = False
def execute(self, userdata):
"""Execute this state"""
if self._failed:
return 'failed'
if self._client.finished():
if self._client.success():
userdata.joint_trajectory = self._client.get_plan()
return 'planned'
else:
Logger.logwarn('MoveIt failed with the following error: %s' %
self._client.error_msg())
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
# create the motion goal
self._client.new_goal(self._planning_group)
self._client.add_joint_values(userdata.joint_values)
self._client.set_velocity_scaling(self._vel_scaling)
# execute the motion
try:
self._client.start_planning()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\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 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 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.")