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 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):
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 motionPlanToPose(self,
pose,
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",
"max_acceleration_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("motionPlanToPose: unsupported argument: %s",
arg)
# Create goal
g = MotionPlanRequest()
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.start_state = kwargs["start_state"]
except KeyError:
g.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.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.planner_id = self.planner_id
# 7. fill in request group name
g.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.allowed_planning_time = self.planning_time
# 10. Fill in velocity scaling factor
try:
g.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 11. Fill in acceleration scaling factor
try:
g.max_velocity_scaling_factor = kwargs["max_acceleration_scaling_factor"]
except KeyError:
pass # do not fill in at all
result = self._mp_service(g)
traj = result.motion_plan_response.trajectory.joint_trajectory.points
if len(traj) < 1:
rospy.logwarn('No motion plan found.')
return None
return traj
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
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
goal_constraint.orientation_constraints.append(OrientationConstraint())
goal_constraint.orientation_constraints[0].header.frame_id = planning_frame
goal_constraint.orientation_constraints[0].link_name = eef_link
goal_constraint.orientation_constraints[0].orientation = pose_goal.orientation
goal_constraint.orientation_constraints[
0].absolute_x_axis_tolerance = tolerance
goal_constraint.orientation_constraints[
0].absolute_y_axis_tolerance = tolerance
goal_constraint.orientation_constraints[
0].absolute_z_axis_tolerance = tolerance
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 = "RRTkConfigDefault" # PRMkConfigDefault, PRMstarkConfigDefault, RRTkConfigDefault, RRTstarkConfigDefault
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]
solid_primitive.type = solid_primitive.SPHERE
bounding_volume.primitives.append(solid_primitive)
# if object.selected_object_name[:8] == "postcard":
# p_update = pose
# p_update.position.z = 0.75
# bounding_volume.primitive_poses.append(p_update)
#elif pose.position.z <= 0.1: #地面に当たらないように
# if pose.position.z <= 0.1: #地面に当たらないように
# p_update = pose
# p_update.position.z = 0.13
# bounding_volume.primitive_poses.append(p_update)
# else:
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
if pose.position.z <= 0.2: # Stage 2 (Grasp)
rospy.loginfo("Planning num:0-0 (Grasp)")
orientation_constraint.orientation.x = 0.675
orientation_constraint.orientation.y = 0.737
orientation_constraint.orientation.z = 0.018
orientation_constraint.orientation.w = -0.019
#goal_constraint.position_constraints[0].constraint_region.primitive_poses[0].position.z -= 0.01 # Stage1
# elif object.selected_object_name[:8] == "postcard":
# #elif pose.position.z < 0.02: # Stage 1
# object.
# orientation_constraint.orientation.x = 0.0#0.904
# orientation_constraint.orientation.y = 0.0#-0.166
# orientation_constraint.orientation.z = -0.676#-0.068
# orientation_constraint.orientation.w = 0.736#0.387
# #orientation_constraint.link_name = "hand_palm_link"
elif object.count_num == 1: # Stage 2 (Sofa)
rospy.loginfo("Planning num:1 (Put on sofa)")
orientation_constraint.orientation.x = -0.019
orientation_constraint.orientation.y = -0.707
orientation_constraint.orientation.z = -0.202
orientation_constraint.orientation.w = 0.706
elif object.count_num == 2: # Stage 2 (Work desk, Shelf)
rospy.loginfo("Planning num:2 (Put on shelf or desk)")
orientation_constraint.orientation.x = 0.51
orientation_constraint.orientation.y = -0.489
orientation_constraint.orientation.z = 0.509
orientation_constraint.orientation.w = 0.489
elif object.count_num == 3: # Stage 2 (White table)
rospy.loginfo("Planning num:3 (Put on table)")
orientation_constraint.orientation.x = 0.51
orientation_constraint.orientation.y = 0.489
orientation_constraint.orientation.z = 0.501
orientation_constraint.orientation.w = -0.497
else:
rospy.loginfo("Planning num:0-1 (Put)")
orientation_constraint.orientation = pose.orientation
orientation_constraint.link_name = object.eef_link
orientation_constraint.absolute_x_axis_tolerance = object._tolerance * 100
orientation_constraint.absolute_y_axis_tolerance = object._tolerance * 100
orientation_constraint.absolute_z_axis_tolerance = object._tolerance * 100
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 = 10.0
action_goal.request.workspace_parameters.header.frame_id = object.planning_frame
action_goal.request.workspace_parameters.min_corner.y = -5.0 #-20.0
action_goal.request.workspace_parameters.min_corner.x = -5.0 #-20.0
action_goal.request.workspace_parameters.min_corner.z = -5.0 #-20.0
action_goal.request.workspace_parameters.max_corner.x = 5.0 #20.0
action_goal.request.workspace_parameters.max_corner.y = 5.0 #20.0
action_goal.request.workspace_parameters.min_corner.z = 5.0 #20.0
action_goal.planning_options.look_around = False
action_goal.planning_options.replan = True
action_goal.planning_options.replan_attempts = 5
action_goal.planning_options.replan_delay = 10.0
return action_goal
