def send_move_group_joint_space_goal(self, move_group, joint_names, joint_values, plan_only=None):
'''
Retrieve the current move group action configuration and send the move request
@param move_group : string specifying a particular move group
@param joint_names : list of joint names for command
@param joint_values : list of joint values for target
'''
# We should have already set this pairing up during initialization
action_topic = ProxyMoveItClient._move_group_clients_[move_group]
# Get the latest setup of of the MoveGroup goal data
goal = MoveGroupGoal()
goal.request = copy.deepcopy(ProxyMoveItClient._motion_plan_requests[move_group])
goal.planning_options = copy.deepcopy(ProxyMoveItClient._planning_options[move_group])
if (plan_only is not None):
goal.planning_options.plan_only = plan_only
goal_constraints = Constraints()
for i in range(len(joint_names)):
jc = ProxyMoveItClient._joint_constraints[move_group][joint_names[i]]
goal_constraints.joint_constraints.append(JointConstraint(
joint_name=joint_names[i],
position=joint_values[i],
tolerance_above=jc.tolerance_above,
tolerance_below=jc.tolerance_above,
weight=jc.weight))
goal.request.goal_constraints.append(goal_constraints)
ProxyMoveItClient._action_clients[action_topic].send_goal(action_topic, goal)
def make_moveit_action_goal(joint_names, joint_positions):
goal_config_constraint = Constraints()
for name, position in zip(joint_names, joint_positions):
joint_constraint = JointConstraint()
joint_constraint.joint_name = name
joint_constraint.position = position
goal_config_constraint.joint_constraints.append(joint_constraint)
req = MotionPlanRequest()
req.group_name = 'both_arms'
req.goal_constraints.append(goal_config_constraint)
goal = MoveGroupGoal()
goal.request = req
return goal
def setUp(self):
# create a action client of move group
self._move_client = SimpleActionClient('move_group', MoveGroupAction)
self._move_client.wait_for_server()
moveit_commander.roscpp_initialize(sys.argv)
group_name = moveit_commander.RobotCommander().get_group_names()[0]
group = moveit_commander.MoveGroupCommander(group_name)
# prepare a joint goal
self._goal = MoveGroupGoal()
self._goal.request.group_name = group_name
self._goal.request.max_velocity_scaling_factor = 0.1
self._goal.request.max_acceleration_scaling_factor = 0.1
self._goal.request.start_state.is_diff = True
goal_constraint = Constraints()
joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
joint_names = group.get_active_joints()
for i in range(len(joint_names)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_values[i]
joint_constraint.weight = 1.0
goal_constraint.joint_constraints.append(joint_constraint)
self._goal.request.goal_constraints.append(goal_constraint)
self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def append_pose_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
""" Appends a pose to the given move_group goal, returns it appended
Goals for now are for the same link TODO: let it be for different links"""
if goal_to_append == None:
rospy.logerr("append_pose_to_move_group_goal needs a goal!")
return
goal_to_append = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = goal_to_append.request.goal_constraints[0].header
position_c.link_name = goal_to_append.request.goal_constraints[0].link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = goal_to_append.request.goal_constraints[0].header
orientation_c.link_name = goal_to_append.request.goal_constraints[0].link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
goal_to_append.request.goal_constraints.append(goal_c)
return goal_to_append
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._config_name = userdata.config_name # Currently not used
self._robot_name = userdata.robot_name # Currently not used
self._move_group = userdata.move_group
self._action_topic = userdata.action_topic
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
userdata.action_topic = self._action_topic # Save action topic to output key
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def execute(self, userdata):
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
userdata.move_it_joint_goal = MoveGroupGoal()
goal_c = Constraints()
for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose):
joint_c = JointConstraint()
joint_c.joint_name = name
joint_c.position = value
joint_c.tolerance_above = 0.01
joint_c.tolerance_below = 0.01
joint_c.weight = 1.0
goal_c.joint_constraints.append(joint_c)
userdata.move_it_joint_goal.request.goal_constraints.append(goal_c)
userdata.move_it_joint_goal.request.num_planning_attempts = 5
userdata.move_it_joint_goal.request.allowed_planning_time = 5.0
userdata.move_it_joint_goal.planning_options.plan_only = False #False = Plan + Execute ; True = Plan only
userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True
userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group
rospy.loginfo('Group Name: ' + userdata.manip_joint_group)
rospy.loginfo('Joints name: ' + str(userdata.manip_joint_names))
rospy.loginfo('Joints Values: ' + str(userdata.manip_goal_joint_pose))
rospy.loginfo('GOAL TO SEND IS:... ' + str(userdata.move_it_joint_goal))
return 'succeeded'
def __init__(self, frame, ns = ''):
# self.scene_pub = PlanningScenePublisher(ns)
self.move_group_client = SimpleActionClient("/{}/move_group".format(ns), MoveGroupAction)
self.move_group_client.wait_for_server()
self.move_group_msg = MoveGroupGoal()
#Create the message for the request
self.move_group_msg.request = MotionPlanRequest()
self.move_group_msg.request.workspace_parameters.header.frame_id = frame
self.move_group_msg.request.workspace_parameters.min_corner.x = -50
self.move_group_msg.request.workspace_parameters.min_corner.y = -50
self.move_group_msg.request.workspace_parameters.min_corner.z = 0
self.move_group_msg.request.workspace_parameters.max_corner.x = 50
self.move_group_msg.request.workspace_parameters.max_corner.y = 50
self.move_group_msg.request.workspace_parameters.max_corner.z = 50
# Target definition
self.move_group_msg.request.goal_constraints.append(Constraints())
self.constraints = JointConstraint()
self.constraints.tolerance_above = 0.001
self.constraints.tolerance_below = 0.001
self.constraints.weight = 1.0
for i in range(0,7):
self.move_group_msg.request.goal_constraints[0].joint_constraints.append(copy.deepcopy(self.constraints))
self.move_group_msg.planning_options.planning_scene_diff.is_diff = True
self.move_group_msg.planning_options.plan_only = True
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._action_topic = userdata.move_group_prefix + userdata.action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._move_group = userdata.move_group
self._joint_names = None
self._joint_names = userdata.joint_names
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def create_move_group_joints_goal(self,
joint_names,
joint_values,
group="right_arm",
plan_only=False):
""" Creates a move_group goal based on pose.
@arg joint_names list of strings of the joint names
@arg joint_values list of digits with the joint values
@arg group string representing the move_group group to use
@arg plan_only bool to for only planning or planning and executing
@return MoveGroupGoal with the desired contents"""
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
for name, value in zip(joint_names, joint_values):
joint_c = JointConstraint()
joint_c.joint_name = name
joint_c.position = value
joint_c.tolerance_above = 0.01
joint_c.tolerance_below = 0.01
joint_c.weight = 1.0
goal_c.joint_constraints.append(joint_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True
moveit_goal.request.group_name = group
return moveit_goal
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._sub = ProxySubscriberCached({self._position_topic: JointState})
self._current_position = []
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def on_enter(self, userdata):
self._param_error = False
self._planning_failed = False
self._control_failed = False
self._success = False
#Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib['name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
return 'param_error'
try:
self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
return 'param_error'
#Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=self._joint_config[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def create_move_group_pose_goal(goal_pose=Pose(),
group="right_arm_torso",
end_link_name=None,
plan_only=True):
""" Creates a move_group goal based on pose.
@arg group string representing the move_group group to use
@arg end_link_name string representing the ending link to use
@arg goal_pose Pose() representing the goal pose
@arg plan_only bool to for only planning or planning and executing
@returns MoveGroupGoal with the data given on the arguments"""
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
# We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message
# http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html
# http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[
0.01
])) # how big is the area where the end effector can be
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if end_link_name != None:
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported.
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
moveit_goal.request.group_name = group
return moveit_goal
def make_default_action_goal(object, pose, orientation=True):
action_goal = MoveGroupGoal()
action_goal.request.group_name = object._move_group
action_goal.request.start_state.is_diff = True
action_goal.request.planner_id = "PRMkConfigDefault"
goal_constraint = Constraints()
goal_constraint.position_constraints.append(PositionConstraint())
goal_constraint.position_constraints[
0].header.frame_id = object.planning_frame
goal_constraint.position_constraints[0].link_name = object.eef_link
bounding_volume = BoundingVolume()
solid_primitive = SolidPrimitive()
solid_primitive.dimensions = [object._tolerance * object._tolerance]
solid_primitive.type = solid_primitive.SPHERE
bounding_volume.primitives.append(solid_primitive)
bounding_volume.primitive_poses.append(pose)
goal_constraint.position_constraints[0].constraint_region = bounding_volume
goal_constraint.position_constraints[0].weight = 1.0
if orientation is not False:
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = object.planning_frame
orientation_constraint.orientation = pose.orientation
orientation_constraint.link_name = object.eef_link
orientation_constraint.absolute_x_axis_tolerance = object._tolerance
orientation_constraint.absolute_y_axis_tolerance = object._tolerance
orientation_constraint.absolute_z_axis_tolerance = object._tolerance
orientation_constraint.weight = 1.0
goal_constraint.orientation_constraints.append(orientation_constraint)
action_goal.request.goal_constraints.append(goal_constraint)
action_goal.request.num_planning_attempts = 5
action_goal.request.allowed_planning_time = 5
action_goal.request.workspace_parameters.header.frame_id = object.planning_frame
action_goal.request.workspace_parameters.min_corner.x = -20
action_goal.request.workspace_parameters.min_corner.y = -20
action_goal.request.workspace_parameters.min_corner.z = -20
action_goal.request.workspace_parameters.max_corner.x = 20
action_goal.request.workspace_parameters.max_corner.y = 20
action_goal.request.workspace_parameters.min_corner.z = 20
action_goal.planning_options.look_around = False
action_goal.planning_options.replan = True
return action_goal
def create_move_group_pose_goal(self,
goal_pose=Pose(),
group="right_arm",
end_link_name="arm_right_tool_link",
plan_only=True):
""" Creates a move_group goal based on pose.
@arg group string representing the move_group group to use
@arg end_link_name string representing the ending link to use
@arg goal_pose Pose() representing the goal pose"""
# Specifying the header makes the planning fail...
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 3
moveit_goal.request.allowed_planning_time = 1.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True
moveit_goal.request.group_name = group
return moveit_goal
def createMoveGroupGoal(move_group="both_arms", pose_from_params={}, tolerances=0.1):
"""Create a moveit_msgs/MoveGroupGoal with the pose passed in pose_from_params with some default values
@param move_group the move_group from MoveIt! which needs to execute the movement of the joints, defaults to both_arms
@param pose_from_params the pose as imported from the param server in the reem_tutorials containing joints and positions
@param tolerances tolerances to all the joints above and below, defaults to 0.1
@return moveit_msgs/MoveGroupGoal with the goal contained in the pose_from_params
"""
mg_g = MoveGroupGoal()
mg_g.request.group_name = move_group
mg_g.request.allowed_planning_time = 5.0
mg_g.request.num_planning_attempts = 3
c = Constraints()
joint_constraints = createJointConstraints(pose_params, tolerances=0.1)
c.joint_constraints.extend(joint_constraints)
mg_g.request.goal_constraints.append(c)
mg_g.planning_options.plan_only = False
# Next planning options are... optional
mg_g.planning_options.replan = True
mg_g.planning_options.replan_attempts = 3
mg_g.planning_options.replan_delay = 0.3
return mg_g
def create_move_group_pose_goal(self,
goal_pose=Pose(),
group="manipulator",
end_link_name=None,
plan_only=True):
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None:
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.1]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if end_link_name != None:
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 0.01
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
moveit_goal.request.group_name = group
return moveit_goal
def move_group_client():
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
goal = MoveGroupGoal()
goal.request.group_name = "right_arm"
goal_constraints = Constraints()
goal_constraints.name = 'goal_test'
#goal_constraints.joint_constraints.extend(createJointConstraints())
#goal_constraints.joint_constraints.extend(createJointConstraintsZero())
goal_constraints.position_constraints.extend(createPositionConstraint())
goal_constraints.orientation_constraints.extend(
createOrientationConstraint())
goal.request.goal_constraints.append(goal_constraints)
client.send_goal(goal)
client.wait_for_result()
return client.get_result()
def moveToJointPosition(self,
joints,
positions,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_scene_diff",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in workspace_parameters
# 2. fill in start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in goal_constraints
c1 = Constraints()
for i in range(len(joints)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = joints[i]
c1.joint_constraints[i].position = positions[i]
c1.joint_constraints[i].tolerance_above = tolerance
c1.joint_constraints[i].tolerance_below = tolerance
c1.joint_constraints[i].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in path constraints
# 5. fill in trajectory constraints
# 6. fill in planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in group name
g.request.group_name = self._group
# 8. fill in number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
try:
g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
except KeyError:
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
def moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
def build(self, tf_timeout=rospy.Duration(5.0)):
"""Builds the goal message.
To set a pose or joint goal, call set_pose_goal or set_joint_goal
before calling build. To add a path orientation constraint, call
add_path_orientation_constraint first.
Args:
tf_timeout: rospy.Duration. How long to wait for a TF message. Only
used with pose goals.
Returns: moveit_msgs/MoveGroupGoal
"""
goal = MoveGroupGoal()
# Set start state
goal.request.start_state = copy.deepcopy(self.start_state)
# Set goal constraint
if self._pose_goal is not None:
self._tf_listener.waitForTransform(self._pose_goal.header.frame_id,
self.fixed_frame,
rospy.Time.now(), tf_timeout)
try:
pose_transformed = self._tf_listener.transformPose(
self.fixed_frame, self._pose_goal)
except (tf.LookupException, tf.ConnectivityException):
return None
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self.fixed_frame
c1.position_constraints[0].link_name = self.gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [self.tolerance * self.tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(self._pose_goal.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self.fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = self.gripper_frame
c1.orientation_constraints[
0].absolute_x_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_y_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_z_axis_tolerance = self.tolerance
c1.orientation_constraints[0].weight = 1.0
goal.request.goal_constraints.append(c1)
elif self._joint_names is not None:
c1 = Constraints()
for i in range(len(self._joint_names)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = self._joint_names[i]
c1.joint_constraints[i].position = self._joint_positions[i]
c1.joint_constraints[i].tolerance_above = self.tolerance
c1.joint_constraints[i].tolerance_below = self.tolerance
c1.joint_constraints[i].weight = 1.0
goal.request.goal_constraints.append(c1)
# Set path constraints
goal.request.path_constraints.orientation_constraints = self._orientation_contraints
# Set trajectory constraints
# Set planner ID (name of motion planner to use)
goal.request.planner_id = self.planner_id
# Set group name
goal.request.group_name = self.group_name
# Set planning attempts
goal.request.num_planning_attempts = self.num_planning_attempts
# Set planning time
goal.request.allowed_planning_time = self.allowed_planning_time
# Set scaling factors
goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor
goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor
# Set planning scene diff
goal.planning_options.planning_scene_diff = copy.deepcopy(
self.planning_scene_diff)
# Set is plan only
goal.planning_options.plan_only = self.plan_only
# Set look around
goal.planning_options.look_around = self.look_around
# Set replanning options
goal.planning_options.replan = self.replan
goal.planning_options.replan_attempts = self.replan_attempts
goal.planning_options.replan_delay = self.replan_delay
return goal
def on_enter(self, userdata):
# Clear return information
self.action_client = None # Clear prior connection in case we changed topics
self.request_time = None
self.robot_trajectory = None
self.return_code = None
# We require action_topic and move_group to be set to use this state
if (self.given_action_topic is None) and (not hasattr(
userdata, 'action_topic') or userdata.action_topic is None):
self.return_code = 'param_error'
Logger.logwarn(
'Userdata action topic of state %s does not exist or is currently undefined!'
% self.name)
return
if not hasattr(userdata, 'move_group') or userdata.move_group is None:
self.return_code = 'param_error'
Logger.logwarn(
'Userdata move_group of state %s does not exist or is currently undefined!'
% self.name)
return
if self.moveit_client is None:
try:
self.moveit_client = ProxyMoveItClient(None)
except Exception as e:
self.moveit_client = None
self.return_code = 'param_error'
Logger.logerr(str(e))
Logger.logwarn(
'No MoveItClient configured - state %s does not exist or is currently undefined!'
% self.name)
return
try:
if (self.given_action_topic is None):
if (self.current_action_topic != userdata.action_topic) or \
(self.move_group != userdata.move_group) :
# Make sure connection is established for change in topics
self.current_action_topic = userdata.action_topic
self.move_group = userdata.move_group
Logger.loginfo(
"%s - trying to connect to %s on moveit client ..." %
(self.name, self.current_action_topic))
self.moveit_client.setup_action_client(
self.current_action_topic, "MoveGroupAction",
self.enter_wait_duration)
else:
self.current_action_topic = self.given_action_topic
if not self.moveit_client.is_available(self.current_action_topic):
# Try to re-connect if not available
ret = False
if self.enter_wait_duration >= 0.0:
Logger.loginfo(
"%s - trying to reconnect to %s on moveit client ..."
% (self.name, self.current_action_topic))
ret = self.moveit_client.connect_action_server(
self.current_action_topic, "MoveGroupAction",
self.enter_wait_duration)
if not ret:
Logger.logwarn(
'State %s -action topic %s connection to action server is not available'
% (self.name, current_action_topic))
self.return_code = 'topics_unavailable'
return
# Retrieve reference to the relevant action client
self.action_client = self.moveit_client._action_clients[
self.current_action_topic]
except Exception as e:
Logger.logwarn('State %s - invalid action connection !\n%s' %
(self.name, str(e)))
self.return_code = 'param_error'
return
try:
joint_values = userdata.joint_values
joint_names = ProxyMoveItClient._joint_names[userdata.move_group]
if hasattr(userdata, 'joint_names'):
joint_names = userdata.joint_names
if len(joint_names) != len(joint_values):
Logger.logwarn(
'%s - Joint values mismatch %d vs. %d' %
(self.name, len(joint_names), len(joint_values)))
self.return_code = 'param_error'
return
# Grab the current data and create a deep copy for local mods
action_goal = MoveGroupGoal(
request=copy.deepcopy(ProxyMoveItClient._motion_plan_requests[
userdata.move_group]),
planning_options=copy.deepcopy(
ProxyMoveItClient._planning_options[userdata.move_group]))
action_goal.planning_options.plan_only = True
Logger.loginfo('State %s - set goal joint constraints !' %
(self.name))
constraints = Constraints()
constraints.joint_constraints = self.moveit_client.get_goal_joint_constraints(
self.move_group, joint_values, joint_names)
action_goal.request.goal_constraints.append(constraints)
self.request_time = rospy.Time.now()
print "------------ Planning Goal -----"
print str(action_goal)
self.action_client.send_goal(self.current_action_topic,
action_goal)
if self.timeout_duration > rospy.Duration(0.0):
self.timeout_target = self.request_time + self.timeout_duration + rospy.Duration(
action_goal.request.allowed_planning_time)
else:
self.timeout_target = None
except Exception as e:
Logger.logwarn('Could not request a plan!\n%s' % str(e))
self.return_code = 'param_error'
print('planning_frame:', planning_frame)
# ('planning_frame:', '/panda_link0')
eef_link = group.get_end_effector_link()
print('eef_link:', eef_link)
# ('eef_link:', 'panda_link8')
tolerance = 0.01
pose_goal = Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 1.0
pose_goal.position.y = 0.15
pose_goal.position.z = 0.4
action_goal = MoveGroupGoal()
action_goal.request.group_name = move_group
goal_constraint = Constraints()
goal_constraint.position_constraints.append(PositionConstraint())
goal_constraint.position_constraints[0].header.frame_id = planning_frame
goal_constraint.position_constraints[0].link_name = eef_link
solid_primitive = SolidPrimitive()
solid_primitive.dimensions = [tolerance * tolerance]
solid_primitive.type = solid_primitive.SPHERE
bounding_volume = BoundingVolume()
bounding_volume.primitives.append(solid_primitive)
bounding_volume.primitive_poses.append(pose_goal)
goal_constraint.position_constraints[0].constraint_region = bounding_volume
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s"%e
def on_enter(self, userdata):
self._param_error = False
self._planning_failed = False
self._control_failed = False
self._success = False
self._config_name = userdata.config_name
self._move_group = userdata.move_group
self._robot_name = userdata.robot_name
self._action_topic = userdata.move_group_prefix + userdata.action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param(userdata.move_group_prefix +
'/robot_description_semantic'):
self._srdf_param = rospy.get_param(userdata.move_group_prefix +
'/robot_description_semantic')
else:
Logger.logerr('Unable to get parameter: %s' % srdf_param)
return
self._param_error = False
self._srdf = None
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn(
'Unable to parse given SRDF parameter: /robot_description_semantic'
)
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib[
'name']:
robot = r
userdata.robot_name_out = robot # Save robot name to output key
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' %
self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib[
'group'] # Set move group name in case it was not defined
userdata.config_name_out = config # Save configuration name to output key
userdata.move_group_out = self._move_group # Save move_group to output key
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' %
self._config_name)
return 'param_error'
try:
self._joint_config = [
float(j.attrib['value']) for j in config.iter('joint')
]
self._joint_names = [
str(j.attrib['name']) for j in config.iter('joint')
]
userdata.joint_values = self._joint_config # Save joint configuration to output key
userdata.joint_names = self._joint_names # Save joint names to output key
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' %
str(e))
return 'param_error'
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
userdata.action_topic_out = self._action_topic # Save action topic to output key
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
from moveit_msgs.msg import RobotState
from moveit_msgs.msg import WorkspaceParameters
from moveit_msgs.msg import JointConstraint
from moveit_msgs.msg import Constraints
from moveit_msgs.msg import MoveGroupAction
from moveit_msgs.msg import MoveGroupGoal
from moveit_msgs.msg import MoveGroupActionGoal
from moveit_msgs.msg import MotionPlanRequest
from geometry_msgs.msg import Vector3
if __name__ == '__main__':
rospy.init_node('moveit_publisher')
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = "/BASE"
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = "/BASE"
start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"]
start_state.joint_state.position = [-0.2569046038066598, -0.8442722962923348, 1.849082034218144, 0.26825374068443164, -0.04090683809444329, 5.745512865657193]
def on_enter(self, userdata):
self._param_error = False
self._planning_failed = False
self._control_failed = False
self._success = False
#self._position_topic = "/m1n6s200_driver/joint_states"
#self._delta = 0.0000001
self._sub = ProxySubscriberCached({self._position_topic: JointState})
self._current_position= []
#Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib['name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib['group'] # Set move group name in case it was not defined
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
return 'param_error'
try:
self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
return 'param_error'
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(
joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
userdata.action_topic = self._action_topic # Save action topic to output key
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
movement_params = rospy.get_param(global_name + '/' + name_of_movement)
joints = movement_params['joints']
positions_and_times = movement_params['points']
# connect to action server
rospy.loginfo("Connecting to move_group")
client = actionlib.SimpleActionClient('/move_group', MoveGroupAction)
client.wait_for_server()
# foreach joint config
step_num = 1
for position_and_time in positions_and_times:
rospy.loginfo("Executing step: " + str(step_num) + "/" + str(len(positions_and_times)))
step_num += 1
# create goal for action server
mg_g = MoveGroupGoal()
mg_g.request.group_name = "both_arms"
mg_g.request.allowed_planning_time = 5.0
mg_g.request.num_planning_attempts = 3
c = Constraints()
joint_constraints = createJointConstraints(joints, position_and_time['positions'], tolerances=0.1)
c.joint_constraints.extend(joint_constraints)
mg_g.request.goal_constraints.append(c)
mg_g.planning_options.plan_only = False
mg_g.planning_options.replan = True
mg_g.planning_options.replan_attempts = 3
mg_g.planning_options.replan_delay = 0.3
#print mg_g
# send goal to action server
client.send_goal(mg_g)
def main():
#Initialize the node
rospy.init_node('moveit_client')
# Create the SimpleActionClient, passing the type of the action
# (MoveGroupAction) to the constructor.
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
# Wait until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = MoveGroupGoal()
#----------------Construct the goal message (start)----------------
joint_names = [
'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1',
'right_w0', 'right_w1', 'right_w2'
]
#Set parameters for the planner
goal.request.group_name = 'both_arms'
goal.request.num_planning_attempts = 1
goal.request.allowed_planning_time = 5.0
#Define the workspace in which the planner will search for solutions
goal.request.workspace_parameters.min_corner.x = -1
goal.request.workspace_parameters.min_corner.y = -1
goal.request.workspace_parameters.min_corner.z = -1
goal.request.workspace_parameters.max_corner.x = 1
goal.request.workspace_parameters.max_corner.y = 1
goal.request.workspace_parameters.max_corner.z = 1
goal.request.start_state.joint_state.header.frame_id = "base"
#Set the start state for the trajectory
goal.request.start_state.joint_state.name = joint_names
'''
changed by zishu yu start
'''
# goal.request.start_state.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# goal.request.start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position = raw_input("please input %d start position for " %
(len(joint_names), ) + str(joint_names))
position = position.split()
while (not (len(position) == len(joint_names))):
print("error")
position = raw_input("please input %d start position for " %
(len(joint_names), ) + str(joint_names))
position = position.split()
velocity = raw_input("please input %d start velocity for " %
(len(joint_names), ) + str(joint_names))
velocity = velocity.split()
while (not (len(velocity) == len(joint_names))):
print("error")
velocity = raw_input("please input %d start velocity for " %
(len(joint_names), ) + str(joint_names))
velocity = velocity.split()
position = [float(position[i]) for i in range(len(position))]
velocity = [float(velocity[i]) for i in range(len(velocity))]
goal.request.start_state.joint_state.position = position
goal.request.start_state.joint_state.velocity = velocity
'''
changed by zishu yu end
'''
#Tell MoveIt whether to execute the trajectory after planning it
goal.planning_options.plan_only = True
#Set the goal position of the robot
#Note that the goal is specified with a collection of individual
#joint constraints, rather than a vector of joint angles
arm_joint_names = joint_names[1:]
'''
changed by zishu yu start
'''
# target_joint_angles = [0.5, -0.1, 0.1, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
target_joint_angles = raw_input("please input %d end position for " %
(len(arm_joint_names), ) +
str(joint_names))
target_joint_angles = target_joint_angles.split()
while (not (len(target_joint_angles) == len(arm_joint_names))):
target_joint_angles = raw_input("please input %d end position for " %
(len(arm_joint_names), ) +
str(joint_names))
target_joint_angles = target_joint_angles.split()
target_joint_angles = [
float(target_joint_angles[i]) for i in range(len(target_joint_angles))
]
'''
changed by zishu yu end
'''
tolerance = 0.0001
consts = []
for i in range(len(arm_joint_names)):
const = JointConstraint()
const.joint_name = arm_joint_names[i]
const.position = target_joint_angles[i]
const.tolerance_above = tolerance
const.tolerance_below = tolerance
const.weight = 1.0
consts.append(const)
goal.request.goal_constraints.append(
Constraints(name='', joint_constraints=consts))
#---------------Construct the goal message (end)-----------------
# Send the goal to the action server.
client.send_goal(goal)
# Wait for the server to finish performing the action.
client.wait_for_result()
# Print out the result of executing the action
print(client.get_result())
def moveToJointPosition(self,
joints,
positions,
tolerance=0.01,
wait=True,
**kwargs):
'''
Move the arm to set of joint position goals
:param joints: joint names for which the target position
is specified.
:param positions: target joint positions
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type joints: list of string element type
:type positions: list of float element type
:type tolerance: float
:type wait: bool
'''
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_scene_diff",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in workspace_parameters
# 2. fill in start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in goal_constraints
c1 = Constraints()
for i in range(len(joints)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = joints[i]
c1.joint_constraints[i].position = positions[i]
c1.joint_constraints[i].tolerance_above = tolerance
c1.joint_constraints[i].tolerance_below = tolerance
c1.joint_constraints[i].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in path constraints
# 5. fill in trajectory constraints
# 6. fill in planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in group name
g.request.group_name = self._group
# 8. fill in number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
try:
g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
except KeyError:
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
result = self._action.get_result()
return processResult(result)
else:
rospy.loginfo('Failed while waiting for action result.')
return False
def main():
#Initialize the node
rospy.init_node('moveit_client')
# Create the SimpleActionClient, passing the type of the action
# (MoveGroupAction) to the constructor.
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
# Wait until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = MoveGroupGoal()
#----------------Construct the goal message (start)----------------
joint_names = [
'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1',
'right_w0', 'right_w1', 'right_w2'
]
#Set parameters for the planner
goal.request.group_name = 'both_arms'
goal.request.num_planning_attempts = 1
goal.request.allowed_planning_time = 5.0
#Define the workspace in which the planner will search for solutions
goal.request.workspace_parameters.min_corner.x = -1
goal.request.workspace_parameters.min_corner.y = -1
goal.request.workspace_parameters.min_corner.z = -1
goal.request.workspace_parameters.max_corner.x = 1
goal.request.workspace_parameters.max_corner.y = 1
goal.request.workspace_parameters.max_corner.z = 1
goal.request.start_state.joint_state.header.frame_id = "base"
#Set the start state for the trajectory
goal.request.start_state.joint_state.name = joint_names
goal.request.start_state.joint_state.position = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
goal.request.start_state.joint_state.velocity = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
#Tell MoveIt whether to execute the trajectory after planning it
goal.planning_options.plan_only = True
#Set the goal position of the robot
#Note that the goal is specified with a collection of individual
#joint constraints, rather than a vector of joint angles
bagel = raw_input('Please enter in 15 joint angles and press enter: ')
bagel = bagel.split()
i = 0
for x in bagel:
bagel[i] = float(x)
i += 1
arm_joint_names = joint_names[1:]
target_joint_angles = bagel
tolerance = 0.0001
consts = []
for i in range(len(arm_joint_names)):
const = JointConstraint()
const.joint_name = arm_joint_names[i]
const.position = target_joint_angles[i]
const.tolerance_above = tolerance
const.tolerance_below = tolerance
const.weight = 1.0
consts.append(const)
goal.request.goal_constraints.append(
Constraints(name='', joint_constraints=consts))
#---------------Construct the goal message (end)-----------------
# Send the goal to the action server.
client.send_goal(goal)
# Wait for the server to finish performing the action.
client.wait_for_result()
# Print out the result of executing the action
print(client.get_result())
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
# TODO check userdata
# if not isinstance(userdata.pose, PoseStamped):
#Logger.logwarn('userdata.pose must be geomery_msgs.msg.PoseStamped. `%s` received' % str(type(userdata.pose)))
#self._planning_failed = True
#return
check_type('pose', 'geometry_msgs/PoseStamped', userdata.pose)
# request planing and execution
action_goal = MoveGroupGoal()
# set palnning options
action_goal.planning_options.plan_only = self._plan_only
action_goal.planning_options.replan = False
# action_goal.planning_options.planning_scene_diff.is_diff = True
# action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
# setup request
action_goal.request.group_name = self._move_group
action_goal.request.num_planning_attempts = 3
action_goal.request.allowed_planning_time = 1.0
action_goal.request.max_velocity_scaling_factor = 1.0
action_goal.request.max_acceleration_scaling_factor = 1.0
# start pose
action_goal.request.start_state.is_diff = True
# pose constraint
goal_constraint = Constraints(name='')
# set target position
constraint = PositionConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.constraint_region = BoundingVolume()
constraint.constraint_region.primitives = [
SolidPrimitive(type=SolidPrimitive.SPHERE,
dimensions=[self._position_tolerance])
]
constraint.constraint_region.primitive_poses = [
Pose(position=userdata.pose.pose.position)
]
constraint.weight = 1.0
goal_constraint.position_constraints.append(constraint)
# set target orientation
constraint = OrientationConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.orientation = userdata.pose.pose.orientation
constraint.absolute_x_axis_tolerance = self._orientation_tolerance
constraint.absolute_y_axis_tolerance = self._orientation_tolerance
constraint.absolute_z_axis_tolerance = self._orientation_tolerance
constraint.weight = 0.1
goal_constraint.orientation_constraints.append(constraint)
# add goal_constraint
action_goal.request.goal_constraints.append(goal_constraint)
try:
self._client.send_goal('move_group', action_goal)
except Exception as e:
Logger.logwarn(
'MoveitToPose: Failed to send MoveGroupAction goal for group: %s\n%s'
% (self._move_group, str(e)))
self._planning_failed = True
def moveToPose(self,
pose,
tolerance=0.01,
wait=True,
**kwargs):
'''
Move the arm, based on a goal pose_stamped for the end effector.
:param pose: target pose to which we want to move
specified link to
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type pose_stamped: ros message object of type PoseStamped
:type gripper_frame: string
:type tolerance: float
:type wait: bool
'''
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToPose: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = self._gripper_frame
# c1.position_constraints[0].target_point_offset
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose.orientation
c1.orientation_constraints[0].link_name = self._gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
result = self._action.get_result()
return processResult(result)
else:
rospy.loginfo('Failed while waiting for action result.')
return False
