def create_basic_mp_position_request(
planning_frame,
link_name,
target_point_offset,
planner_id):
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = move_group
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = 5.0
motion_plan_request.workspace_parameters = WorkspaceParameters()
motion_plan_request.max_velocity_scaling_factor = 0.5
motion_plan_request.max_acceleration_scaling_factor = 0.5
motion_plan_request.planner_id = planner_id
position_constraints = []
position_constraint = PositionConstraint()
header = std_msgs.msg.Header()
header.frame_id = planning_frame
position_constraint.header = header
position_constraint.link_name = link_name
position_constraint.target_point_offset = target_point_offset
position_constraints = [position_constraint]
constraint = Constraints()
constraint.position_constraints = position_constraints
constraints = [constraint]
motion_plan_request.goal_constraints = constraints
return motion_plan_request
def _cmd_to_request(self, robot):
"""Transforms the gripper command to a MotionPlanRequest."""
req = MotionPlanRequest()
# Set general info
req.planner_id = "PTP"
req.group_name = self._planning_group
req.max_velocity_scaling_factor = self._vel_scale * robot.global_motion_factor
req.max_acceleration_scaling_factor = self._acc_scale * robot.global_motion_factor
req.allowed_planning_time = 1.0
# Set an empty diff as start_state => the current state is used by the planner
req.start_state.is_diff = True
# create goal constraints
goal_constraints = Constraints()
if isinstance(self._goal, (float, int, long)):
joint_names = robot._robot_commander.get_group(self._planning_group).get_active_joints()
if len(joint_names) != 1:
raise IndexError("PG70 should have only one joint. But group " + req.group_name +
" contains " + str(len(joint_names)) + " joints.")
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[0]
joint_constraint.position = float(self._goal)
joint_constraint.weight = 1
goal_constraints.joint_constraints.append(joint_constraint)
else:
raise NotImplementedError("Unknown type of goal is given.")
req.goal_constraints.append(goal_constraints)
return req
def _cmd_to_request(self, robot):
"""Transforms the given command to a MotionPlanRequest."""
req = MotionPlanRequest()
# Set general info
req.planner_id = self._planner_id
req.group_name = self._planning_group
req.max_velocity_scaling_factor = self._vel_scale
req.max_acceleration_scaling_factor = self._acc_scale
req.allowed_planning_time = 1.0
# Set an empty diff as start_state => the current state is used by the planner
req.start_state.is_diff = True
# Set goal constraint
if self._goal is None:
raise NameError("Goal is not given.")
goal_constraints = Constraints()
# goal as Pose in Cartesian space
if isinstance(self._goal, Pose):
goal_pose = self._get_goal_pose(robot)
robot_reference_frame = robot._robot_commander.get_planning_frame()
goal_constraints.orientation_constraints.append(
_to_ori_constraint(goal_pose, robot_reference_frame,
self._target_link))
goal_constraints.position_constraints.append(
_to_pose_constraint(goal_pose, robot_reference_frame,
self._target_link))
# goal as list of int or float in joint space
elif isinstance(self._goal, list):
joint_names = robot._robot_commander.get_group(
self._planning_group).get_active_joints()
joint_values = self._get_joint_pose(robot)
if len(joint_names) != len(joint_values):
raise IndexError(
"Given joint goal does not match the planning group " +
req.group_name + ".")
for joint_name, joint_value in zip(joint_names, joint_values):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_name
joint_constraint.position = joint_value
joint_constraint.weight = 1
goal_constraints.joint_constraints.append(joint_constraint)
else:
raise NotImplementedError("Unknown type of goal is given.")
req.goal_constraints.append(goal_constraints)
return req
def constraint_planner(start_robot_state, goal_config, group_handle, planner_name, planning_attemps, planning_time): planning_workspace = WorkspaceParameters(); planning_workspace.header.frame_id = "/base_link"; planning_workspace.header.stamp = rospy.Time.now(); planning_workspace.min_corner.x = -1; planning_workspace.min_corner.y = -1; planning_workspace.min_corner.z = -1; planning_workspace.max_corner.x = 1; planning_workspace.max_corner.y = 1; planning_workspace.max_corner.z = 1; # Set Start start_state = RobotState(); start_state = start_robot_state; print ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"; #print start_robot_state; # Set Goal goal_state = Constraints(); Jnt_constraint = setJointConstraints(goal_config, group_handle); goal_state.joint_constraints = Jnt_constraint; #print Jnt_constraint; # Set Constraints des_w = -0.070099; des_x = 0.41382; des_y = -0.57302; des_z = 0.70391; rotation_constraints = setOrientationConstraints(des_w,des_x,des_y,des_z,group_handle, weight = 1.0); # Generating Request planningRequest = MotionPlanRequest(); planningRequest.workspace_parameters = planning_workspace; planningRequest.start_state = start_robot_state; planningRequest.goal_constraints.append(goal_state); # Setting Constraint planningRequest.path_constraints.name = 'scoop_constraint'; planningRequest.path_constraints.orientation_constraints.append(rotation_constraints); #traj_constraint = Constraints(); #traj_constraint.orientation_constraints.append(rotation_constraints); #planningRequest.trajectory_constraints.constraints.append(traj_constraint); planningRequest.planner_id = "RRTConnectkConfigDefault"; planningRequest.group_name = group_handle.get_name(); planningRequest.num_planning_attempts = planning_attemps; planningRequest.allowed_planning_time = planning_time; planningRequest.max_velocity_scaling_factor = 1.0; Planning_req.publish(planningRequest);
def set_global_defaults( self, group ):
"""
Initializes the proxy with default parameters to use with connections
@param group : string move group name
"""
# Default planning parameters for new connections
num_planning_attempts=5
allowed_planning_time=10.0
plan_only=True
planner_id='RRTConnectkConfigDefault'
# Set the default plan request data
plan_request = MotionPlanRequest()
plan_request.start_state.is_diff = True # Flags start state as empty to use current state on server
plan_request.num_planning_attempts = num_planning_attempts
plan_request.allowed_planning_time = allowed_planning_time
plan_request.planner_id = planner_id
plan_request.group_name = group
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._motion_plan_requests[group] = plan_request
ProxyMoveItClient._default_motion_plan_requests[group] = copy.deepcopy(plan_request)
planning_options = PlanningOptions()
planning_options.plan_only = plan_only
planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._planning_options[group] = planning_options
ProxyMoveItClient._default_planning_options[group] = copy.deepcopy(planning_options)
# Set up the default joint constraints
joint_constraints = {}
for name in ProxyMoveItClient._joint_names[group]:
joint_constraints[name] = JointConstraint(joint_name=name, tolerance_above=0.05, tolerance_below=0.05,weight=1.0)
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._joint_constraints[group] = joint_constraints
ProxyMoveItClient._default_joint_constraints[group] = copy.deepcopy(joint_constraints)
# @TODO - add real constraints
ProxyMoveItClient._position_constraints[group] = []
ProxyMoveItClient._default_position_constraints[group] = []
ProxyMoveItClient._orientation_constraints[group] = []
ProxyMoveItClient._default_orientation_constraints[group] = []
ProxyMoveItClient._visibility_constraints[group] = []
ProxyMoveItClient._default_visibility_constraints[group] = []
ProxyMoveItClient._robot_states[group] = RobotState()
ProxyMoveItClient._default_robot_states[group] = RobotState()
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 move_to_state(self, joint_states):
if self.verbose:
print 'Moving to next state'
mpr = MotionPlanRequest()
con = Constraints()
for name, val in joint_states.items():
if name not in self.joints_to_exclude:
jc = JointConstraint()
jc.joint_name = name
jc.position = val
jc.tolerance_above = self.joint_angle_tolerance
jc.tolerance_below = self.joint_angle_tolerance
jc.weight = 1.0
con.joint_constraints.append(jc)
mpr.goal_constraints = [con]
mpr.group_name = self.planning_group_name
mpr.allowed_planning_time = 3.0 # [s]
# print mpr
try:
req = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan)
res = req(mpr)
traj = res.motion_plan_response.trajectory
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def create_basic_mp_joint_request(joint_names, joint_values, planner_id):
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = move_group
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = 5.0
motion_plan_request.workspace_parameters = WorkspaceParameters()
motion_plan_request.max_velocity_scaling_factor = 0.5
motion_plan_request.max_acceleration_scaling_factor = 0.5
motion_plan_request.planner_id = planner_id
joint_constraints = []
for i in range(len(joint_names)):
joint_con = JointConstraint()
joint_con.joint_name = joint_names[i]
joint_con.position = joint_values[i]
joint_con.tolerance_above = 0.0001
joint_con.tolerance_below = 0.0001
joint_con.weight = 0.0001
joint_constraints.append(joint_con)
constraint = Constraints()
constraint.joint_constraints = joint_constraints
constraints = [constraint]
motion_plan_request.goal_constraints = constraints
return motion_plan_request
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 motionPlanToJointPosition(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",
"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()
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.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
jc0.tolerance_below = 0.0001
jc0.weight = 1.0
cons = Constraints()
cons.name = ""
cons.joint_constraints.append(jc2)
cons.joint_constraints.append(jc3)
cons.joint_constraints.append(jc4)
cons.joint_constraints.append(jc5)
cons.joint_constraints.append(jc1)
cons.joint_constraints.append(jc0)
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 = "manipulator"
move_group_goal.request = msg
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 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
jc0.position = -4.43859335239
jc0.tolerance_above = 0.0001
jc0.tolerance_below = 0.0001
jc0.weight = 1.0
cons = Constraints()
cons.name = ""
cons.joint_constraints.append(jc2)
cons.joint_constraints.append(jc3)
cons.joint_constraints.append(jc4)
cons.joint_constraints.append(jc5)
cons.joint_constraints.append(jc1)
cons.joint_constraints.append(jc0)
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 = "manipulator"
move_group_goal.request = msg
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 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