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 __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 __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 __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
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
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
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
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 __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 __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
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.")
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
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.")
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
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 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 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.")