def __init__(self):
self.current_diff = SetPlanningSceneDiffRequest()
self.set_scene = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff)
rospy.loginfo('Waiting for planning scene service')
self.set_scene.wait_for_service()
self._get_world_robot = rospy.ServiceProxy('/environment_server/get_robot_state', GetRobotState)
rospy.loginfo('Waiting for get robot state service')
self._get_world_robot.wait_for_service()
self.transformer = TransformState()
arms = ['arm'] #could somehow get these off the parameter server I guess
self.hands = {}
#this is unfortunately necessary for dealing with attached objects
for arm in arms:
self.hands[arm] = HandDescription(arm)
self.send_diff()
self.world_frame = '/odom_combined'
self.robot_frame = '/base_footprint'
rs = self.get_robot_state()
if rs.multi_dof_joint_state.frame_ids:
self.world_frame = rs.multi_dof_joint_state.frame_ids[0]
self.robot_frame = rs.multi_dof_joint_state.child_frame_ids[0]
rospy.loginfo('Frame '+str(self.world_frame)+' is world frame and '+
str(self.robot_frame)+' is robot root frame')
rospy.loginfo('Planning scene interface created')
def __init__(self):
self.current_diff = SetPlanningSceneDiffRequest()
self.set_scene = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff)
rospy.loginfo('Waiting for planning scene service')
self.set_scene.wait_for_service()
self._get_world_robot = rospy.ServiceProxy('/environment_server/get_robot_state', GetRobotState)
rospy.loginfo('Waiting for get robot state service')
self._get_world_robot.wait_for_service()
self.transformer = TransformState()
arms = ['left_arm', 'right_arm'] #could somehow get these off the parameter server I guess
self.hands = {}
#this is unfortunately necessary for dealing with attached objects
for arm in arms:
self.hands[arm] = HandDescription(arm)
self.send_diff()
self.world_frame = '/odom_combined'
self.robot_frame = '/base_footprint'
rs = self.get_robot_state()
if rs.multi_dof_joint_state.frame_ids:
self.world_frame = rs.multi_dof_joint_state.frame_ids[0]
self.robot_frame = rs.multi_dof_joint_state.child_frame_ids[0]
rospy.loginfo('Frame '+str(self.world_frame)+' is world frame and '+
str(self.robot_frame)+' is robot root frame')
rospy.loginfo('Planning scene interface created')
class PlanningSceneInterface:
'''
This class is a wrapper for the ROS planning scene.
The planning scene is used to allow the robot to plan
in a different version of the world than the one that currently exists. For example, if we wish to plan
a series of arm movements, the second arm movement starts from the end of the first one rather than from the
robot's current pose in the world. More importantly, we may wish to plan a sequence of manipulations, which not
only change the state of the robot but also the state of objects in the world. All planners in ROS plan in
the planning scene rather than in the current state of the world.
However, the ROS planning scene is not set up for planning sequences of actions. The ROS service call to
get_planning_scene returns the current state of the world, NOT the scene planners are currently using. Likewise
the service call to set_planning_scene_diff sets the planning scene from the current state of the world, not
current planning scene. This makes it very difficult to plan consecutive sequences of actions as it is
incumbent on the user to maintain the diff from the current state of the world throughout the entire set
of plans.
Instead, this class maintains a running diff, allowing the user to continually update the state of the planning
scene instead of remembering all changes from the current state of the world. It is easy to reset the diff
to reflect exactly the state of the world, but for every function call we have also provided a function call
that can exactly undo it. Note that although this class keeps track of your running changes, it is, in the
end, still sending a diff from the current world. Therefore, if you make a change to the world (by publishing
on the appropriate topic or using a WorldInterface) the planning scene will reflect that.
When planning we must also be careful not to call TF as that reflects only the current state of the world.
This inteface has a state transformer (see transform_state.py) that transforms frames according to the current
robot state in the planning scene rather than in the world. When planning you should always use the transform
functions in the planning scene rather than calling TF.
In many ways, this class looks like world_interface.py. This was intentional to highlight that the planning
scene operates just like the world will. When making calls like add_object or attach_object_to_gripper to the
planning scene during planning, you will make a lot of those same calls to the world interface during execution.
The ROS planning scene is global; whenever it is set all planners are immediately updated to the new planning
scene. For this reason, we recommend that your planning scene interface also be global and that you use
the get_planning_scene_interface call whenever you need an interface. However, if you are very comfortable with
the planning scene mechanism, you can maintain multiple interfaces corresponding to different sets of plans.
For an example of using the planning scene interface, see arm_planner.py.
Please note that this class is **NOT CURRENTLY THREADSAFE!**
**Attributes:**
**world_frame (string):** The frame in which objects are added to the collision map and also the frame in
which planning is done. Usually this is /map, but if not using a map it will be /odom_combined. It is
found by looking at the parent of the multi DOF joint in the robot state.
**robot_frame (string):** The root frame of the robot's link tree. This has the same orientation as the
world frame but moves with the robot. Usually it is /base_footprint. It is found by looking at the child
of the multi DOF joint in the robot state.
**hands (dictionary):** Hand descriptions for the robot indexed by 'left_arm' and 'right_arm'. For more
information about hand descriptions, see hand_description.py
**current_diff (arm_navigation_msgs.srv.SetPlanningSceneDiffRequest):** The current diff between the actual
state of the world and the planning scene.
**set_scene (rospy.ServiceProxy):** The set_planning_scene_diff service
**transformer (transform_state.TransformState):** The state transformer used for transforming between frames
because TF reflects only the current state of the world.
'''
def __init__(self):
self.current_diff = SetPlanningSceneDiffRequest()
self.set_scene = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff)
rospy.loginfo('Waiting for planning scene service')
self.set_scene.wait_for_service()
self._get_world_robot = rospy.ServiceProxy('/environment_server/get_robot_state', GetRobotState)
rospy.loginfo('Waiting for get robot state service')
self._get_world_robot.wait_for_service()
self.transformer = TransformState()
arms = ['left_arm', 'right_arm'] #could somehow get these off the parameter server I guess
self.hands = {}
#this is unfortunately necessary for dealing with attached objects
for arm in arms:
self.hands[arm] = HandDescription(arm)
self.send_diff()
self.world_frame = '/odom_combined'
self.robot_frame = '/base_footprint'
rs = self.get_robot_state()
if rs.multi_dof_joint_state.frame_ids:
self.world_frame = rs.multi_dof_joint_state.frame_ids[0]
self.robot_frame = rs.multi_dof_joint_state.child_frame_ids[0]
rospy.loginfo('Frame '+str(self.world_frame)+' is world frame and '+
str(self.robot_frame)+' is robot root frame')
rospy.loginfo('Planning scene interface created')
def send_diff(self):
'''
Sends the current diff.
All functions call this to set the diff. If you set the diff yourself, call this to
send it along to the planning scene. In general use, you should call the helper functions rather than
calling this directly.
**Returns:**
An arm_navigation_msgs.srv.SetPlanningSceneDiffResponse that is the current planning scene
'''
return self.set_scene(self.current_diff)
def current_planning_scene(self):
'''
Returns the current planning scene.
**Returns:**
An arm_navigation_msgs.mgs.PlanningScene that is the current planning scene
'''
return self.send_diff().planning_scene
def reset(self):
'''
Resets the planning scene to the current state of the world.
This does NOT empty the planning scene; it is
unfortunately not possible to do that. Calling this will wipe out your current diff and any changes you
have applied to the planning scene.
'''
self.current_diff = SetPlanningSceneDiffRequest()
self.send_diff()
rospy.sleep(0.2)
return True
def set_collisions(self, ordered_collisions):
'''
Sets the ordered collisions in the scene, removing any ordered collisions you passed previously.
If you pass in None, this removes all ordered collisions.
**Args:**
**ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions
'''
if not ordered_collisions:
self.current_diff.operations = OrderedCollisionOperations()
else:
self.current_diff.operations = ordered_collisions
self.send_diff()
return True
def add_ordered_collisions(self, ordered_collisions):
'''
Adds ordered collisions on top of whatever ordered collisions are already in the diff.
To exactly counter this effect, call remove_ordered collisions.
**Args:**
**ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions
'''
if not ordered_collisions or\
not ordered_collisions.collision_operations:
return
self.current_diff.operations.collision_operations +=\
ordered_collisions.collision_operations
self.send_diff()
return True
def add_object(self, co):
'''
Adds an object to the planning scene.
This can also be used to move an object in the diff by passing in a new pose and the same id. If you
previously removed an object with this ID, this function will undo that removal.
**Args:**
**co (arm_navigation_msgs.msg.CollisionObject):** The object to add
'''
for o in self.current_diff.planning_scene_diff.collision_objects:
if o.id == co.id:
self.current_diff.planning_scene_diff.collision_objects.remove(o)
break
co.operation.operation = co.operation.ADD
self.current_diff.planning_scene_diff.collision_objects.append(co)
self.send_diff()
return True
def remove_object(self, co):
'''
Removes an object from the planning scene.
If you previously added the object using add_object, this
function will undo that. The full collision object is needed because the object may not actually exist
in the world but only in the planning scene.
**Args:**
**co (arm_navigation_msgs.msg.CollisionObject):** Obbject to remove
**TODO:**
* Is it possible to do this by ID only?
* Does this cause warnings if the object was initially added by the planning scene and not the world?
'''
for o in self.current_diff.planning_scene_diff.collision_objects:
if o.id == co.id:
self.current_diff.planning_scene_diff.collision_objects.remove(o)
break
co.operation.operation = co.operation.REMOVE
self.current_diff.planning_scene_diff.collision_objects.append(co)
self.send_diff()
def attach_object_to_gripper(self, arm_name, object_id):
'''
Attaches an object to the robot's end effector.
The object must already exist in the planning scene
(although it does not need to exist in the current state of the world). This does NOT "snap" the object to
the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect
to the robot's end effector instead of the world. Collisions will be checked between the object and the
world as the object moves, but collisions between the object and the end effector will be ignored.
The link the object is attached to and the links with which collisions are ignored are defined by the hand
description (see hand_description.py).
The object will be attached to the robot according to the current state of the robot and object in the
planning scene as maintained by this class (i.e. with any prior changes you made to the diff).
To undo this function, you can use detach_object and add_object but you must remember the position of the
object where it was originally attached! If you call detach_and_add_back_attached_object, it will add
the object back at its current location in the planning scene, NOT at the location at which it was
originally attached.
The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing
an attached object with operation ADD and a collision object with operation REMOVE.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to
**object_id (string):** The ID of the object to attach
**Returns:**
False if the object did not previously exist in the world or planning scene diff and therefore cannot be
attached; True otherwise.
'''
#Note: It is important to send the full collision object (not just the id) in the attached collision object.
#This was quite complicated to figure out the first time.
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
obj.touch_links = self.hands[arm_name].touch_links
diff_obj = None
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
rospy.loginfo('Object was added to or modified in planning scene.')
diff_obj = co
break
if not diff_obj:
rospy.loginfo('Object was not previously added to or modified in planning scene.')
diff_obj = self.collision_object(object_id)
if not diff_obj:
rospy.logerr('Cannot attach object '+object_id+'. This object does not exist')
return False
self.current_diff.planning_scene_diff.collision_objects.append(diff_obj)
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
#convert it into the frame of the hand - it remains stationary with respect to this
for p in range(len(diff_obj.poses)):
diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p])
diff_obj.header.frame_id = obj.link_name
diff_obj.operation.operation = diff_obj.operation.REMOVE
obj.object = copy.deepcopy(diff_obj)
obj.object.operation.operation = obj.object.operation.ADD
self.send_diff()
return True
def detach_object(self, arm_name, object_id):
'''
Detaches a single object from the arm and removes it from the collision space entirely.
The object must have been attached in the world or planning scene diff previously. This removes the object
from the planning scene even if it currently exists in the world.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
**Returns:**
False if the object was not previously attached in the world or the diff and therefore cannot be
detached; True otherwise.
'''
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
#did the object ever exist in the world?
#if so, we need to continually remove it after detaching
#but it will always be in
#corresponding collision object if it exists
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
#if the planning scene originally added this object, we could
#just remove it from the list of collision objects but we have
#no way of knowing that at this point
co.operation.operation = co.operation.REMOVE
for p in range(len(co.poses)):
co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p])
co.header.frame_id = self.world_frame
break
for ao in self.current_diff.planning_scene_diff.attached_collision_objects:
if ao.object.id == object_id:
rospy.loginfo('Object was attached by the planning scene interface')
self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao)
self.send_diff()
return True
rospy.loginfo('Object was not attached by the planning scene interface')
aos = self.attached_collision_objects()
for ao in aos:
if ao.object.id == object_id:
obj.object = ao.object
obj.object.operation.operation = obj.object.operation.REMOVE
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
self.send_diff()
return True
rospy.logwarn('Object '+object_id+' not attached to gripper')
return False
def detach_and_add_back_attached_object(self, arm_name, object_id):
'''
Detaches a single object from the gripper and adds it back to the planning scene at its current location in
the diff.
From here on, it is assumed that the object remains stationary with respect to the world. The
object must have been attached to the robot in the world or in the diff previously.
As always, this function is done with respect to the running diff. Therefore, if you have used
set_robot_state to change the robot state in the diff, this will respect that change and add the object back
at the location corresponding to the robot's new state.
**Args:**
**arm_name (string):** The arm ('right_arm' or 'left_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
**Returns:**
False if the object was not attached to the robot previously; True otherwise.
'''
#could save some work by doing this all in one step :)
ao = self.attached_collision_object(object_id)
if not self.detach_object(arm_name, object_id): return False
#find its current pose in the world frame
for p in range(len(ao.object.poses)):
ao.object.poses[p] = self.transform_pose(self.world_frame, ao.object.header.frame_id, ao.object.poses[p])
ao.object.header.frame_id = self.world_frame
return self.add_object(ao.object)
def set_robot_state(self, robot_state):
'''
Sets the robot state in the diff.
This will also update the position of all attached objects as their
positions are defined relative to the position of the robot. When planning, you should call this function
rather than trying to set starting states for the planner. All planners will plan assuming the state in
the diff is the starting state.
**Args:**
**robot_state (arm_navigation_msgs.msg.RobotState):** New robot state
'''
self.current_diff.planning_scene_diff.robot_state = robot_state
self.send_diff()
return True
def remove_ordered_collisions(self, ordered_collisions):
'''
Removes the ordered collisions from the current diff.
This will ONLY remove ordered collisions by removing
them from the diff. It can be used to exactly undo the effects of add_ordered_collisions, but cannot be
used to change any collisions that were not set using add_ordered_collisions. To enable or disable
collisions in the scene, use add_ordered_collisions.
For example, if you used add_ordered_collisions to add a collision operation that removed all collisions with
the right gripper, passing the same collision operation to this function will reset the collisions to exactly
what they were before. It will NOT enable the collisions of the gripper with everything.
**Args:**
**ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions to remove
'''
if not ordered_collisions or\
not ordered_collisions.collision_operations:
return
newops = OrderedCollisionOperations()
for o in self.current_diff.operations.collision_operations:
doadd = True
for a in ordered_collisions.collision_operations:
if o.object1 == a.object1 and o.object2 == a.object2 and\
o.operation == a.operation:
doadd = False
break
if doadd:
newops.collision_operations.append(o)
self.set_collisions(newops)
def get_robot_state(self):
'''
Returns the current robot state in the planning scene including the current diff
**Returns:**
The robot state as an arm_navigation_msgs.msg.RobotState in the current diff of the planning scene.
This is the state that all planners are assuming is the starting state. If you have not set the robot
state in the diff, this is the current robot state in the world.
'''
#note: done this way because it's much faster than sending the whole planning scene back and forth
if self.current_diff.planning_scene_diff.robot_state.joint_state.name:
return self.current_diff.planning_scene_diff.robot_state
state = self._get_world_robot()
return state.robot_state
def collision_objects(self):
'''
Returns the list of collision objects in the planning scene including the current diff
**Returns:**
A list of arm_navigation_msgs.msg.CollisionObject in the planning scene the planners are using. This
includes any changes you have made using add_object, remove_object, etc.
'''
scene = self.current_planning_scene()
return scene.collision_objects
def attached_collision_objects(self):
'''
Returns the list of attached collision objects in the planning scene including the current diff
**Returns:**
A list of arm_navigation_msgs.msg.AttachedCollisionObject in the planning scene the planners are using.
This includes any changes you have made using attach_object_to_gripper, detach_object, etc.
'''
scene = self.current_planning_scene()
return scene.attached_collision_objects
def collision_object(self, object_id):
'''
Returns the collision object with this ID
**Args:**
**object_id (string):** The ID of a collision object
**Returns:**
The arm_navigation_msgs.msg.CollisionObject corresponding to object_id or None if no such object exists.
This will reflect any changes in the collision objects (position or existence) you have made.
'''
cos = self.collision_objects()
for co in cos:
if co.id == object_id:
return co
return None
def attached_collision_object(self, object_id):
'''
Returns the attached collision object with this ID
**Args:**
**object_id (string):** The ID of an attached collision object
**Returns:**
The arm_navigation_msgs.msg.AttachedCollisionObject corresponding to object_id or None if no such object
exists. This will reflect any changes in the attached collision objects (position or existence) you
have made.
'''
aos = self.attached_collision_objects()
for ao in aos:
if ao.object.id == object_id:
return ao
return None
def get_transform(self, to_frame, from_frame):
'''
All of the transform functions transform according to the robot state in the diff (or the current state of
the world if you have not set a state in the diff).
This is because while planning, you cannot use TF as it
reflects only the current state of the world. Instead, you want to know what the transform will be given
the state we expect the robot to be in. You should always use these functions for transformation rather
than TF while planning!
This function follows the conventions of TF meaning that the transform returned will take the origin of
from_frame and return the origin of to_frame in from_frame. For example, calling
get_transform(r_wrist_roll_link, base_link) will return the current position of the PR2's right wrist in the
base_link frame.
Note that this is the INVERSE of the transform you would use to transform a point from from_frame to
to_frame. Because this gets complicated to keep straight, we have also provided a number of
transfrom_DATATYPE functions.
**Args:**
**from_frame (string):** Frame to transform from
**to_frame (string):** Frame to transform to
**Returns:**
A geometry_msgs.msg.TransformStamped (the timestamp is left at zero as it is irrelevant)
'''
return self.transformer.get_transform(to_frame, from_frame, self.get_robot_state())
def transform_point(self, new_frame_id, old_frame_id, point):
'''
Transforms a point defined in old_frame_id into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**point (geometry_msgs.msg.Point):** point defined in old_frame_id
**Returns:**
A geometry_msgs.msg.Point defined in new_frame_id
'''
return self.transformer.transform_point(new_frame_id, old_frame_id, point, self.get_robot_state())
def transform_quaternion(self, new_frame_id, old_frame_id, quat):
'''
Transforms a quaternion defined in old_frame_id into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**quat (geometry_msgs.msg.Quaternion):** quaternion defined in old_frame_id
**Returns:**
A geometry_msgs.msg.Quaternion defined in new_frame_id
'''
return self.transformer.transform_quaternion(new_frame_id, old_frame_id, quat, self.get_robot_state())
def transform_pose(self, new_frame_id, old_frame_id, pose):
'''
Transforms a pose defined in old_frame_id into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**pose (geometry_msgs.msg.Pose):** point defined in old_frame_id
**Returns:**
A geometry_msgs.msg.Pose defined in new_frame_id
'''
return self.transformer.transform_pose(new_frame_id, old_frame_id, pose, self.get_robot_state())
def transform_point_stamped(self, new_frame_id, point_stamped):
'''
Transforms a point into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**point_stamped (geometry_msgs.msg.PointStamped):** point stamped (current frame is defined by header)
**Returns:**
A geometry_msgs.msg.PointStamped defined in new_frame_id
'''
return self.transformer.transform_point_stamped(new_frame_id, point_stamped, self.get_robot_state())
def transform_quaternion_stamped(self, new_frame_id, quat_stamped):
'''
Transforms a quaternion into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**quat_stamped (geometry_msgs.msg.QuaternionStamped):** quaternion stamped (current frame is defined by
header)
**Returns:**
A geometry_msgs.msg.QuaternionStamped defined in new_frame_id
'''
return self.transformer.transform_quaternion_stamped(new_frame_id, quat_stamped, self.get_robot_state())
def transform_pose_stamped(self, new_frame_id, pose_stamped):
'''
Transforms a pose into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**pose_stamped (geometry_msgs.msg.PoseStamped):** pose stamped (current frame is defined by header)
**Returns:**
A geometry_msgs.msg.PoseStamped defined in new_frame_id
'''
return self.transformer.transform_pose_stamped(new_frame_id, pose_stamped, self.get_robot_state())
def transform_constraints(self, new_frame_id, constraints):
'''
Transforms constraints into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**constraints (arm_navigation_msgs.msg.Constraints):** constraints to transform
**Returns:**
An arm_navigation_msgs.msg.Constraints defined in new_frame_id
'''
return self.transformer.transform_constraints(new_frame_id, constraints, self.get_robot_state())
class PlanningSceneInterface:
'''
This class is a wrapper for the ROS planning scene.
The planning scene is used to allow the robot to plan
in a different version of the world than the one that currently exists. For example, if we wish to plan
a series of arm movements, the second arm movement starts from the end of the first one rather than from the
robot's current pose in the world. More importantly, we may wish to plan a sequence of manipulations, which not
only change the state of the robot but also the state of objects in the world. All planners in ROS plan in
the planning scene rather than in the current state of the world.
However, the ROS planning scene is not set up for planning sequences of actions. The ROS service call to
get_planning_scene returns the current state of the world, NOT the scene planners are currently using. Likewise
the service call to set_planning_scene_diff sets the planning scene from the current state of the world, not
current planning scene. This makes it very difficult to plan consecutive sequences of actions as it is
incumbent on the user to maintain the diff from the current state of the world throughout the entire set
of plans.
Instead, this class maintains a running diff, allowing the user to continually update the state of the planning
scene instead of remembering all changes from the current state of the world. It is easy to reset the diff
to reflect exactly the state of the world, but for every function call we have also provided a function call
that can exactly undo it. Note that although this class keeps track of your running changes, it is, in the
end, still sending a diff from the current world. Therefore, if you make a change to the world (by publishing
on the appropriate topic or using a WorldInterface) the planning scene will reflect that.
When planning we must also be careful not to call TF as that reflects only the current state of the world.
This inteface has a state transformer (see transform_state.py) that transforms frames according to the current
robot state in the planning scene rather than in the world. When planning you should always use the transform
functions in the planning scene rather than calling TF.
In many ways, this class looks like world_interface.py. This was intentional to highlight that the planning
scene operates just like the world will. When making calls like add_object or attach_object_to_gripper to the
planning scene during planning, you will make a lot of those same calls to the world interface during execution.
The ROS planning scene is global; whenever it is set all planners are immediately updated to the new planning
scene. For this reason, we recommend that your planning scene interface also be global and that you use
the get_planning_scene_interface call whenever you need an interface. However, if you are very comfortable with
the planning scene mechanism, you can maintain multiple interfaces corresponding to different sets of plans.
For an example of using the planning scene interface, see arm_planner.py.
Please note that this class is **NOT CURRENTLY THREADSAFE!**
**Attributes:**
**world_frame (string):** The frame in which objects are added to the collision map and also the frame in
which planning is done. Usually this is /map, but if not using a map it will be /odom_combined. It is
found by looking at the parent of the multi DOF joint in the robot state.
**robot_frame (string):** The root frame of the robot's link tree. This has the same orientation as the
world frame but moves with the robot. Usually it is /base_footprint. It is found by looking at the child
of the multi DOF joint in the robot state.
**hands (dictionary):** Hand descriptions for the robot indexed by 'left_arm' and 'right_arm'. For more
information about hand descriptions, see hand_description.py
**current_diff (arm_navigation_msgs.srv.SetPlanningSceneDiffRequest):** The current diff between the actual
state of the world and the planning scene.
**set_scene (rospy.ServiceProxy):** The set_planning_scene_diff service
**transformer (transform_state.TransformState):** The state transformer used for transforming between frames
because TF reflects only the current state of the world.
'''
def __init__(self):
self.current_diff = SetPlanningSceneDiffRequest()
self.set_scene = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff)
rospy.loginfo('Waiting for planning scene service')
self.set_scene.wait_for_service()
self._get_world_robot = rospy.ServiceProxy('/environment_server/get_robot_state', GetRobotState)
rospy.loginfo('Waiting for get robot state service')
self._get_world_robot.wait_for_service()
self.transformer = TransformState()
arms = ['arm'] #could somehow get these off the parameter server I guess
self.hands = {}
#this is unfortunately necessary for dealing with attached objects
for arm in arms:
self.hands[arm] = HandDescription(arm)
self.send_diff()
self.world_frame = '/odom_combined'
self.robot_frame = '/base_footprint'
rs = self.get_robot_state()
if rs.multi_dof_joint_state.frame_ids:
self.world_frame = rs.multi_dof_joint_state.frame_ids[0]
self.robot_frame = rs.multi_dof_joint_state.child_frame_ids[0]
rospy.loginfo('Frame '+str(self.world_frame)+' is world frame and '+
str(self.robot_frame)+' is robot root frame')
rospy.loginfo('Planning scene interface created')
def send_diff(self):
'''
Sends the current diff.
All functions call this to set the diff. If you set the diff yourself, call this to
send it along to the planning scene. In general use, you should call the helper functions rather than
calling this directly.
**Returns:**
An arm_navigation_msgs.srv.SetPlanningSceneDiffResponse that is the current planning scene
'''
return self.set_scene(self.current_diff)
def current_planning_scene(self):
'''
Returns the current planning scene.
**Returns:**
An arm_navigation_msgs.mgs.PlanningScene that is the current planning scene
'''
return self.send_diff().planning_scene
def reset(self):
'''
Resets the planning scene to the current state of the world.
This does NOT empty the planning scene; it is
unfortunately not possible to do that. Calling this will wipe out your current diff and any changes you
have applied to the planning scene.
'''
self.current_diff = SetPlanningSceneDiffRequest()
self.send_diff()
rospy.sleep(0.2)
return True
def set_collisions(self, ordered_collisions):
'''
Sets the ordered collisions in the scene, removing any ordered collisions you passed previously.
If you pass in None, this removes all ordered collisions.
**Args:**
**ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions
'''
if not ordered_collisions:
self.current_diff.operations = OrderedCollisionOperations()
else:
self.current_diff.operations = ordered_collisions
self.send_diff()
return True
def add_ordered_collisions(self, ordered_collisions):
'''
Adds ordered collisions on top of whatever ordered collisions are already in the diff.
To exactly counter this effect, call remove_ordered collisions.
**Args:**
**ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions
'''
if not ordered_collisions or\
not ordered_collisions.collision_operations:
return
self.current_diff.operations.collision_operations +=\
ordered_collisions.collision_operations
self.send_diff()
return True
def add_object(self, co):
'''
Adds an object to the planning scene.
This can also be used to move an object in the diff by passing in a new pose and the same id. If you
previously removed an object with this ID, this function will undo that removal.
**Args:**
**co (arm_navigation_msgs.msg.CollisionObject):** The object to add
'''
for o in self.current_diff.planning_scene_diff.collision_objects:
if o.id == co.id:
self.current_diff.planning_scene_diff.collision_objects.remove(o)
break
co.operation.operation = co.operation.ADD
self.current_diff.planning_scene_diff.collision_objects.append(co)
self.send_diff()
return True
def remove_object(self, co):
'''
Removes an object from the planning scene.
If you previously added the object using add_object, this
function will undo that. The full collision object is needed because the object may not actually exist
in the world but only in the planning scene.
**Args:**
**co (arm_navigation_msgs.msg.CollisionObject):** Obbject to remove
**TODO:**
* Is it possible to do this by ID only?
* Does this cause warnings if the object was initially added by the planning scene and not the world?
'''
for o in self.current_diff.planning_scene_diff.collision_objects:
if o.id == co.id:
self.current_diff.planning_scene_diff.collision_objects.remove(o)
break
co.operation.operation = co.operation.REMOVE
self.current_diff.planning_scene_diff.collision_objects.append(co)
self.send_diff()
def attach_object_to_gripper(self, arm_name, object_id):
'''
Attaches an object to the robot's end effector.
The object must already exist in the planning scene
(although it does not need to exist in the current state of the world). This does NOT "snap" the object to
the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect
to the robot's end effector instead of the world. Collisions will be checked between the object and the
world as the object moves, but collisions between the object and the end effector will be ignored.
The link the object is attached to and the links with which collisions are ignored are defined by the hand
description (see hand_description.py).
The object will be attached to the robot according to the current state of the robot and object in the
planning scene as maintained by this class (i.e. with any prior changes you made to the diff).
To undo this function, you can use detach_object and add_object but you must remember the position of the
object where it was originally attached! If you call detach_and_add_back_attached_object, it will add
the object back at its current location in the planning scene, NOT at the location at which it was
originally attached.
The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing
an attached object with operation ADD and a collision object with operation REMOVE.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to
**object_id (string):** The ID of the object to attach
**Returns:**
False if the object did not previously exist in the world or planning scene diff and therefore cannot be
attached; True otherwise.
'''
#Note: It is important to send the full collision object (not just the id) in the attached collision object.
#This was quite complicated to figure out the first time.
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
obj.touch_links = self.hands[arm_name].touch_links
diff_obj = None
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
rospy.loginfo('Object was added to or modified in planning scene.')
diff_obj = co
break
if not diff_obj:
rospy.loginfo('Object was not previously added to or modified in planning scene.')
diff_obj = self.collision_object(object_id)
if not diff_obj:
rospy.logerr('Cannot attach object '+object_id+'. This object does not exist')
return False
self.current_diff.planning_scene_diff.collision_objects.append(diff_obj)
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
#convert it into the frame of the hand - it remains stationary with respect to this
for p in range(len(diff_obj.poses)):
diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p])
diff_obj.header.frame_id = obj.link_name
diff_obj.operation.operation = diff_obj.operation.REMOVE
obj.object = copy.deepcopy(diff_obj)
obj.object.operation.operation = obj.object.operation.ADD
self.send_diff()
return True
def detach_object(self, arm_name, object_id):
'''
Detaches a single object from the arm and removes it from the collision space entirely.
The object must have been attached in the world or planning scene diff previously. This removes the object
from the planning scene even if it currently exists in the world.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
**Returns:**
False if the object was not previously attached in the world or the diff and therefore cannot be
detached; True otherwise.
'''
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
#did the object ever exist in the world?
#if so, we need to continually remove it after detaching
#but it will always be in
#corresponding collision object if it exists
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
#if the planning scene originally added this object, we could
#just remove it from the list of collision objects but we have
#no way of knowing that at this point
co.operation.operation = co.operation.REMOVE
for p in range(len(co.poses)):
co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p])
co.header.frame_id = self.world_frame
break
for ao in self.current_diff.planning_scene_diff.attached_collision_objects:
if ao.object.id == object_id:
rospy.loginfo('Object was attached by the planning scene interface')
self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao)
self.send_diff()
return True
rospy.loginfo('Object was not attached by the planning scene interface')
aos = self.attached_collision_objects()
for ao in aos:
if ao.object.id == object_id:
obj.object = ao.object
obj.object.operation.operation = obj.object.operation.REMOVE
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
self.send_diff()
return True
rospy.logwarn('Object '+object_id+' not attached to gripper')
return False
def detach_and_add_back_attached_object(self, arm_name, object_id):
'''
Detaches a single object from the gripper and adds it back to the planning scene at its current location in
the diff.
From here on, it is assumed that the object remains stationary with respect to the world. The
object must have been attached to the robot in the world or in the diff previously.
As always, this function is done with respect to the running diff. Therefore, if you have used
set_robot_state to change the robot state in the diff, this will respect that change and add the object back
at the location corresponding to the robot's new state.
**Args:**
**arm_name (string):** The arm ('right_arm' or 'left_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
**Returns:**
False if the object was not attached to the robot previously; True otherwise.
'''
#could save some work by doing this all in one step :)
ao = self.attached_collision_object(object_id)
if not self.detach_object(arm_name, object_id): return False
#find its current pose in the world frame
for p in range(len(ao.object.poses)):
ao.object.poses[p] = self.transform_pose(self.world_frame, ao.object.header.frame_id, ao.object.poses[p])
ao.object.header.frame_id = self.world_frame
return self.add_object(ao.object)
def set_robot_state(self, robot_state):
'''
Sets the robot state in the diff.
This will also update the position of all attached objects as their
positions are defined relative to the position of the robot. When planning, you should call this function
rather than trying to set starting states for the planner. All planners will plan assuming the state in
the diff is the starting state.
**Args:**
**robot_state (arm_navigation_msgs.msg.RobotState):** New robot state
'''
self.current_diff.planning_scene_diff.robot_state = robot_state
self.send_diff()
return True
def remove_ordered_collisions(self, ordered_collisions):
'''
Removes the ordered collisions from the current diff.
This will ONLY remove ordered collisions by removing
them from the diff. It can be used to exactly undo the effects of add_ordered_collisions, but cannot be
used to change any collisions that were not set using add_ordered_collisions. To enable or disable
collisions in the scene, use add_ordered_collisions.
For example, if you used add_ordered_collisions to add a collision operation that removed all collisions with
the right gripper, passing the same collision operation to this function will reset the collisions to exactly
what they were before. It will NOT enable the collisions of the gripper with everything.
**Args:**
**ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions to remove
'''
if not ordered_collisions or\
not ordered_collisions.collision_operations:
return
newops = OrderedCollisionOperations()
for o in self.current_diff.operations.collision_operations:
doadd = True
for a in ordered_collisions.collision_operations:
if o.object1 == a.object1 and o.object2 == a.object2 and\
o.operation == a.operation:
doadd = False
break
if doadd:
newops.collision_operations.append(o)
self.set_collisions(newops)
def get_robot_state(self):
'''
Returns the current robot state in the planning scene including the current diff
**Returns:**
The robot state as an arm_navigation_msgs.msg.RobotState in the current diff of the planning scene.
This is the state that all planners are assuming is the starting state. If you have not set the robot
state in the diff, this is the current robot state in the world.
'''
#note: done this way because it's much faster than sending the whole planning scene back and forth
if self.current_diff.planning_scene_diff.robot_state.joint_state.name:
return self.current_diff.planning_scene_diff.robot_state
state = self._get_world_robot()
return state.robot_state
def collision_objects(self):
'''
Returns the list of collision objects in the planning scene including the current diff
**Returns:**
A list of arm_navigation_msgs.msg.CollisionObject in the planning scene the planners are using. This
includes any changes you have made using add_object, remove_object, etc.
'''
scene = self.current_planning_scene()
return scene.collision_objects
def attached_collision_objects(self):
'''
Returns the list of attached collision objects in the planning scene including the current diff
**Returns:**
A list of arm_navigation_msgs.msg.AttachedCollisionObject in the planning scene the planners are using.
This includes any changes you have made using attach_object_to_gripper, detach_object, etc.
'''
scene = self.current_planning_scene()
return scene.attached_collision_objects
def collision_object(self, object_id):
'''
Returns the collision object with this ID
**Args:**
**object_id (string):** The ID of a collision object
**Returns:**
The arm_navigation_msgs.msg.CollisionObject corresponding to object_id or None if no such object exists.
This will reflect any changes in the collision objects (position or existence) you have made.
'''
cos = self.collision_objects()
for co in cos:
if co.id == object_id:
return co
return None
def attached_collision_object(self, object_id):
'''
Returns the attached collision object with this ID
**Args:**
**object_id (string):** The ID of an attached collision object
**Returns:**
The arm_navigation_msgs.msg.AttachedCollisionObject corresponding to object_id or None if no such object
exists. This will reflect any changes in the attached collision objects (position or existence) you
have made.
'''
aos = self.attached_collision_objects()
for ao in aos:
if ao.object.id == object_id:
return ao
return None
def get_transform(self, to_frame, from_frame):
'''
All of the transform functions transform according to the robot state in the diff (or the current state of
the world if you have not set a state in the diff).
This is because while planning, you cannot use TF as it
reflects only the current state of the world. Instead, you want to know what the transform will be given
the state we expect the robot to be in. You should always use these functions for transformation rather
than TF while planning!
This function follows the conventions of TF meaning that the transform returned will take the origin of
from_frame and return the origin of to_frame in from_frame. For example, calling
get_transform(r_wrist_roll_link, base_link) will return the current position of the PR2's right wrist in the
base_link frame.
Note that this is the INVERSE of the transform you would use to transform a point from from_frame to
to_frame. Because this gets complicated to keep straight, we have also provided a number of
transfrom_DATATYPE functions.
**Args:**
**from_frame (string):** Frame to transform from
**to_frame (string):** Frame to transform to
**Returns:**
A geometry_msgs.msg.TransformStamped (the timestamp is left at zero as it is irrelevant)
'''
return self.transformer.get_transform(to_frame, from_frame, self.get_robot_state())
def transform_point(self, new_frame_id, old_frame_id, point):
'''
Transforms a point defined in old_frame_id into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**point (geometry_msgs.msg.Point):** point defined in old_frame_id
**Returns:**
A geometry_msgs.msg.Point defined in new_frame_id
'''
return self.transformer.transform_point(new_frame_id, old_frame_id, point, self.get_robot_state())
def transform_quaternion(self, new_frame_id, old_frame_id, quat):
'''
Transforms a quaternion defined in old_frame_id into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**quat (geometry_msgs.msg.Quaternion):** quaternion defined in old_frame_id
**Returns:**
A geometry_msgs.msg.Quaternion defined in new_frame_id
'''
return self.transformer.transform_quaternion(new_frame_id, old_frame_id, quat, self.get_robot_state())
def transform_pose(self, new_frame_id, old_frame_id, pose):
'''
Transforms a pose defined in old_frame_id into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**pose (geometry_msgs.msg.Pose):** point defined in old_frame_id
**Returns:**
A geometry_msgs.msg.Pose defined in new_frame_id
'''
return self.transformer.transform_pose(new_frame_id, old_frame_id, pose, self.get_robot_state())
def transform_point_stamped(self, new_frame_id, point_stamped):
'''
Transforms a point into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**point_stamped (geometry_msgs.msg.PointStamped):** point stamped (current frame is defined by header)
**Returns:**
A geometry_msgs.msg.PointStamped defined in new_frame_id
'''
return self.transformer.transform_point_stamped(new_frame_id, point_stamped, self.get_robot_state())
def transform_quaternion_stamped(self, new_frame_id, quat_stamped):
'''
Transforms a quaternion into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**quat_stamped (geometry_msgs.msg.QuaternionStamped):** quaternion stamped (current frame is defined by
header)
**Returns:**
A geometry_msgs.msg.QuaternionStamped defined in new_frame_id
'''
return self.transformer.transform_quaternion_stamped(new_frame_id, quat_stamped, self.get_robot_state())
def transform_pose_stamped(self, new_frame_id, pose_stamped):
'''
Transforms a pose into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**pose_stamped (geometry_msgs.msg.PoseStamped):** pose stamped (current frame is defined by header)
**Returns:**
A geometry_msgs.msg.PoseStamped defined in new_frame_id
'''
return self.transformer.transform_pose_stamped(new_frame_id, pose_stamped, self.get_robot_state())
def transform_constraints(self, new_frame_id, constraints):
'''
Transforms constraints into new_frame_id according to the current state in the diff.
**Args:**
**new_frame_id (string):** new frame id
**constraints (arm_navigation_msgs.msg.Constraints):** constraints to transform
**Returns:**
An arm_navigation_msgs.msg.Constraints defined in new_frame_id
'''
return self.transformer.transform_constraints(new_frame_id, constraints, self.get_robot_state())
def __init__(self, arm_name):
'''
Constructor for ArmPlanner.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') for which to plan
'''
rospy.loginfo("Creating arm planner...")
#public variables
self.arm_name = arm_name
self.hand = HandDescription(arm_name)
self.kinematics_info_service = rospy.ServiceProxy\
('/pr2_'+self.arm_name+ '_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
rospy.loginfo('Waiting for kinematics solver info service')
self.kinematics_info_service.wait_for_service()
self.get_state_validity_service = rospy.ServiceProxy(
'/planning_scene_validity_server/get_state_validity',
GetStateValidity)
rospy.loginfo('Waiting for state validity service.')
self.get_state_validity_service.wait_for_service()
#find the joint names
info = self.kinematics_info_service()
self.joint_names = info.kinematic_solver_info.joint_names
self.joint_limits = info.kinematic_solver_info.limits
for l in range(len(self.joint_limits)):
#for some reason this isn't filled in on return
self.joint_limits[l].joint_name = self.joint_names[l]
#private variables
self._psi = get_planning_scene_interface()
self._transformer = TransformState()
#services (private)
self._move_arm_planner = rospy.ServiceProxy(PLANNER_NAME,
GetMotionPlan)
rospy.loginfo('Waiting for move arm planner')
self._move_arm_planner.wait_for_service()
self._interpolated_ik_planning_service = rospy.ServiceProxy\
('/'+self.arm_name[0]+'_interpolated_ik_motion_plan', GetMotionPlan)
rospy.loginfo('Waiting for interpolated IK planning service')
self._interpolated_ik_planning_service.wait_for_service()
self._interpolated_ik_parameter_service =rospy.ServiceProxy\
('/'+self.arm_name[0]+'_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams)
rospy.loginfo('Waiting for interpolated IK parameter setting service')
self._interpolated_ik_parameter_service.wait_for_service()
self._filter_trajectory_with_constraints_service = rospy.ServiceProxy\
('/trajectory_filter_server/filter_trajectory_with_constraints', FilterJointTrajectoryWithConstraints)
rospy.loginfo('Waiting for trajectory filter with constraints service')
self._filter_trajectory_with_constraints_service.wait_for_service()
self._collision_aware_ik_service = rospy.ServiceProxy\
('/pr2_'+self.arm_name+'_kinematics/get_constraint_aware_ik', GetConstraintAwarePositionIK)
rospy.loginfo('Waiting for collision aware IK service')
self._collision_aware_ik_service.wait_for_service()
self._ik_service = rospy.ServiceProxy(
'/pr2_' + self.arm_name + '_kinematics/get_ik', GetPositionIK)
rospy.loginfo('Waiting for IK service')
self._ik_service.wait_for_service()
self._fk_service = rospy.ServiceProxy(
'/pr2_' + self.arm_name + '_kinematics/get_fk', GetPositionFK)
rospy.loginfo('Waiting for FK service')
self._fk_service.wait_for_service()
database_grasp_planner_name = rospy.get_param(
'/object_manipulator_1/default_database_planner',
DEFAULT_DATABASE_GRASP_PLANNER)
print 'default database grasp planner'
print database_grasp_planner_name
self.database_grasp_planner = rospy.ServiceProxy(
database_grasp_planner_name, GraspPlanning)
rospy.loginfo('Waiting for database grasp planner')
self.database_grasp_planner.wait_for_service()
self._grasps_pub = rospy.Publisher(GRASPS_VISUALIZATION_PUB,
MarkerArray,
latch=True)
rospy.loginfo("Arm planner created")
class ArmPlanner:
'''
This class can be used to plan for the PR2 arms.
This allows you to plan several motions in a row before executing them. This class can be used for collision
free planning, trajectory filtering, and interpolated IK plannig. The function you should mainly use to plan
for the arms is plan_collision_free.
The class also has a number of arm-related functions, such as forward and inverse kinematics,
that are related to planning, although not planning themselves. It also has some utility functions involving
working with joint states and robot states where it is necessary to know joint names (functions for which it
is not necessary to use joint names are in trajectory_tools.py).
This class relies heavily on the planning scene interface. It makes no calls to TF.
**Attributes:**
**arm_name (string):** The name of the arm for which to plan ('right_arm' or 'left_arm')
**hand (hand_description.HandDescription):** Hand description for the arm.
**joint_names ([string]):** Names of the arm joints
**joint_limits ([arm_navigation_msgs.msg.JointLimits]):** Limits for the arm joints
**kinematics_info_service (rospy.ServiceProxy):** Service for retreiving kinematic information about the arm
**get_state_validity_service (rospy.ServiceProxy):** Service for ascertaining state validity.
Be aware that this service has several bugs at the moment.
'''
def __init__(self, arm_name):
'''
Constructor for ArmPlanner.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') for which to plan
'''
rospy.loginfo("Creating arm planner...")
#public variables
self.arm_name = arm_name
self.hand = HandDescription(arm_name)
self.kinematics_info_service = rospy.ServiceProxy\
('/pr2_'+self.arm_name+ '_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
rospy.loginfo('Waiting for kinematics solver info service')
self.kinematics_info_service.wait_for_service()
self.get_state_validity_service = rospy.ServiceProxy(
'/planning_scene_validity_server/get_state_validity',
GetStateValidity)
rospy.loginfo('Waiting for state validity service.')
self.get_state_validity_service.wait_for_service()
#find the joint names
info = self.kinematics_info_service()
self.joint_names = info.kinematic_solver_info.joint_names
self.joint_limits = info.kinematic_solver_info.limits
for l in range(len(self.joint_limits)):
#for some reason this isn't filled in on return
self.joint_limits[l].joint_name = self.joint_names[l]
#private variables
self._psi = get_planning_scene_interface()
self._transformer = TransformState()
#services (private)
self._move_arm_planner = rospy.ServiceProxy(PLANNER_NAME,
GetMotionPlan)
rospy.loginfo('Waiting for move arm planner')
self._move_arm_planner.wait_for_service()
self._interpolated_ik_planning_service = rospy.ServiceProxy\
('/'+self.arm_name[0]+'_interpolated_ik_motion_plan', GetMotionPlan)
rospy.loginfo('Waiting for interpolated IK planning service')
self._interpolated_ik_planning_service.wait_for_service()
self._interpolated_ik_parameter_service =rospy.ServiceProxy\
('/'+self.arm_name[0]+'_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams)
rospy.loginfo('Waiting for interpolated IK parameter setting service')
self._interpolated_ik_parameter_service.wait_for_service()
self._filter_trajectory_with_constraints_service = rospy.ServiceProxy\
('/trajectory_filter_server/filter_trajectory_with_constraints', FilterJointTrajectoryWithConstraints)
rospy.loginfo('Waiting for trajectory filter with constraints service')
self._filter_trajectory_with_constraints_service.wait_for_service()
self._collision_aware_ik_service = rospy.ServiceProxy\
('/pr2_'+self.arm_name+'_kinematics/get_constraint_aware_ik', GetConstraintAwarePositionIK)
rospy.loginfo('Waiting for collision aware IK service')
self._collision_aware_ik_service.wait_for_service()
self._ik_service = rospy.ServiceProxy(
'/pr2_' + self.arm_name + '_kinematics/get_ik', GetPositionIK)
rospy.loginfo('Waiting for IK service')
self._ik_service.wait_for_service()
self._fk_service = rospy.ServiceProxy(
'/pr2_' + self.arm_name + '_kinematics/get_fk', GetPositionFK)
rospy.loginfo('Waiting for FK service')
self._fk_service.wait_for_service()
database_grasp_planner_name = rospy.get_param(
'/object_manipulator_1/default_database_planner',
DEFAULT_DATABASE_GRASP_PLANNER)
print 'default database grasp planner'
print database_grasp_planner_name
self.database_grasp_planner = rospy.ServiceProxy(
database_grasp_planner_name, GraspPlanning)
rospy.loginfo('Waiting for database grasp planner')
self.database_grasp_planner.wait_for_service()
self._grasps_pub = rospy.Publisher(GRASPS_VISUALIZATION_PUB,
MarkerArray,
latch=True)
rospy.loginfo("Arm planner created")
def arm_joint_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm in the current planning scene state or the passed in state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
A sensor_msgs.msg.JointState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
joint_state = JointState()
joint_state.header = robot_state.joint_state.header
for n in self.joint_names:
found = False
for i in range(len(robot_state.joint_state.name)):
if robot_state.joint_state.name[i] == n:
joint_state.name.append(n)
joint_state.position.append(
robot_state.joint_state.position[i])
found = True
break
if not found and fail_if_joint_not_found:
raise ValueError('Joint ' + n + ' is missing from robot state')
return joint_state
def arm_robot_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm (as a robot state) in the current planning scene state or the passed in
state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
An arm_navigation_msgs.msg.RobotState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
joint_state = self.arm_joint_state(
robot_state=robot_state,
fail_if_joint_not_found=fail_if_joint_not_found)
robot_state = RobotState()
robot_state.joint_state = joint_state
return robot_state
def joint_positions_to_joint_state(self, joint_pos):
'''
Converts a list of joint positions to a joint state.
**Args:**
**joint_pos ([double]):** An array of joint positions
**Returns:**
A sensor_msgs.msg.JointState corresponding to these joint positions. Assumes they are in the same order
as joint_names.
'''
joint_state = JointState()
joint_state.name = copy.copy(self.joint_names)
joint_state.position = joint_pos
return joint_state
def set_joint_positions_in_robot_state(self, joint_pos, robot_state):
'''
Sets joint positions in a robot state.
**Args:**
**joint_pos ([double]):** An array of joint positions
**robot_state (arm_navigation_msgs.msg.RobotState):** A robot state in which to set the joint angles
equal to joint_pos
**Returns:**
An arm_navigation_msgs.msg.RobotState in which the joint angles corresponding to this arm are set to
joint_pos (assumes these are in the same order as joint_names) and all other joints match the passed
in robot_state. Also sets these joints in the passed in robot state.
'''
return tt.set_joint_state_in_robot_state(
self.joint_positions_to_joint_state(joint_pos), robot_state)
def get_ik(self,
pose_stamped,
collision_aware=True,
starting_state=None,
seed_state=None,
timeout=5.0,
ordered_collisions=None):
'''
Solves inverse kinematics problems.
**Args:**
**pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions
*collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world
*starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
matching this state. If no state is passed in, it will use the current state in the planning scene
interface.
*seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK. If no state is passed it, it
will use the current state in planning scene interface.
*timeout (double):* Time in seconds that IK is allowed to run
**Returns:**
A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False. The robot state returned has
the arm joints set to the IK solution if found and all other joints set to that of starting_state.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls an IK service which may use TF for transformations! Probably best to only use with pose stampeds
defined in the robot frame (convert them yourself using the planning scene interface).
'''
rospy.logdebug('Solving IK for\n' + str(pose_stamped))
pos_req = PositionIKRequest()
pos_req.ik_link_name = self.hand.hand_frame
pos_req.pose_stamped = pose_stamped
if not starting_state:
starting_state = self._psi.get_robot_state()
if not seed_state:
seed_state = self.arm_robot_state(starting_state)
pos_req.ik_seed_state = seed_state
pos_req.robot_state = starting_state
if collision_aware:
self._psi.add_ordered_collisions(ordered_collisions)
coll_req = GetConstraintAwarePositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._collision_aware_ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(
coll_res.solution.joint_state, copy.deepcopy(starting_state))
self._psi.remove_ordered_collisions(ordered_collisions)
return coll_res
coll_req = GetPositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(
coll_res.solution.joint_state, copy.deepcopy(starting_state))
return coll_res
def get_fk(self, fk_links, robot_state=None, frame_id=None):
'''
Solves forward kinematics.
In general, it is better to use the planning scene or the state transformer get_transform function between
robot links rather than this function.
**Args:**
**fk_links ([string]):** Links for which you want FK solutions
*robot_state (arm_navigation_msgs.mgs.RobotState):* The state of the robot during forward kinematics.
If no state is passed in the state in the planning scene will be used.
*frame_id (string):* The frame ID in which you want the returned poses. Note that the FK service may use
TF so we recommend only using the robot frame. If no frame is specified, the robot frame is used.
**Returns:**
A list of geometry_msgs.msg.PoseStamped corresponding to forward kinematic solutions for fk_links.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls a service which may use TF! Recommended that you only ask for poses in the robot frame.
'''
req = GetPositionFKRequest()
req.header.frame_id = frame_id
if not frame_id:
req.header.frame_id = self._psi.robot_frame
req.header.stamp = rospy.Time(0)
req.robot_state = robot_state
if not robot_state:
req.robot_state = self._psi.get_robot_state()
req.fk_link_names = fk_links
res = self._fk_service(req)
if res.error_code.val != res.error_code.SUCCESS or not res.pose_stamped:
raise ArmNavError('Forward kinematics failed',
error_code=res.error_code)
return res.pose_stamped
def get_hand_frame_pose(self, robot_state=None, frame_id=None):
'''
Returns the pose of the hand in the current or passed in robot state.
Note that this function uses the TransformState get_transform function rather than FK.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state in which you want to find
the pose of the hand frame. If nothing is passed in, returns the pose of the hand frame in the robot
state in the planning scene interface.
*frame_id (string):* The frame in which you want the pose of the hand frame. If nothing is passed in,
returns in the robot frame.
**Returns:**
A geometry_msgs.msg.PoseStamped that is the position of the hand frame.
'''
if frame_id == None:
frame_id = self._psi.robot_frame
if robot_state == None:
robot_state = self._psi.get_robot_state()
trans = self._transformer.get_transform(self.hand.hand_frame, frame_id,
robot_state)
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = conv.transform_to_pose(trans.transform)
return ps
def hand_marker(self,
robot_state=None,
ns='',
r=0.0,
g=0.0,
b=1.0,
a=0.8,
scale=1.0):
'''
Returns a MarkerArray of the hand in the current or passed in state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The state in which to draw the hand. If no state
is specified, the robot state in the planning scene interface is used.
*ns (string):* Marker namespace
*r (double):* Red value (between 0 and 1)
*g (double):* Green value (between 0 and 1)
*b (double):* Blue value (between 0 and 1)
*a (double):* Alpha value (between 0 and 1)
*scale (double):* Scale
**Returns:**
A visualization_msgs.msg.MarkerArray that when published shows the hand in the position specified by
the robot state.
**Raises:**
**rospy.ServiceException:** if there is a problem calling the service for getting the robot markers
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
return vt.robot_marker(robot_state,
link_names=self.hand.hand_links,
ns=ns,
r=r,
g=g,
b=b,
a=a,
scale=scale)
def get_closest_joint_state_in_limits(self, robot_state=None):
'''
Finds the closest joint state to the passed in or current robot state that is within joint limits.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state. If no state is passed in
the current state in the planning scene interface is used.
**Returns:**
A sensor_msgs.msg.JointState that is the closest state in which the arm joints are all within the joint
limits. The return has only the arm joints, but it also sets the joints in the passed in robot state.
'''
return self.arm_joint_state(
self.get_closest_state_in_limits(robot_state=robot_state))
def get_closest_state_in_limits(self, robot_state=None):
'''
Finds the closest robot state to the passed in or current robot state that is within joint limits.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state. If no state is passed in
the current state in the planning scene interface is used.
**Returns:**
An arm_navigation_msgs.msg.RobotState that is the closest state in which the arm joints are all within
the joint limits. Also sets the joints in the passed in robot state.
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
robot_state.joint_state.position = list(
robot_state.joint_state.position)
#is this outside the joint limits?
#if so, modify it slightly so that it is not
for j in range(len(robot_state.joint_state.name)):
for limit in self.joint_limits:
if limit.joint_name == robot_state.joint_state.name[j]:
jp = robot_state.joint_state.position[j]
if limit.has_position_limits:
if jp < limit.min_position:
robot_state.joint_state.position[
j] = limit.min_position + JOINT_LIMIT_PAD
elif jp > limit.max_position:
robot_state.joint_state.position[
j] = limit.max_position - JOINT_LIMIT_PAD
break
return robot_state
def filter_trajectory(self, trajectory, motion_plan_request=None):
'''
Filters a joint trajectory and assigns times using the joint trajectory filter service.
**Args:**
**trajectory (trajectory_msgs.msg.JointTrajectory):** The trajectory to be filtered. All times must be 0.
*motion_plan_request (arm_navigation_msgs.msg.MotionPlanRequest):* If passed in the trajectory
filter will respect the starting state, goal constriants, and path constraints. It will also append the
starting state from the motion plan request to the front of the trajectory.
**Returns:**
A trajectory_msgs.msg.JointTrajectory that has been filtered and had times assigned.
**Raises:**
**exceptions.ArmNavError:** if the trajectory cannot be filtered. This is almost always because the
trajectory did not actually reach the goal state or had a collision. Note that OMPL returns bad plans
with reasonable frequency so if trajectory filtering fails, you want to re-plan.
'''
if len(trajectory.points) < SHORTEST_FILTERABLE_TRAJECTORY:
rospy.logwarn('Not filtering trajectory of length %d. Too small.',
len(trajectory.points))
trajectory = tt.convert_path_to_trajectory(trajectory)
if motion_plan_request:
trajectory = tt.add_state_to_front_of_joint_trajectory\
(self.arm_joint_state(robot_state=motion_plan_request.start_state), trajectory)
return trajectory
req = FilterJointTrajectoryWithConstraintsRequest()
req.trajectory = trajectory
#these have to be in the world frame... go figure
if motion_plan_request:
req.path_constraints =\
self._psi.transform_constraints(self._psi.world_frame, motion_plan_request.path_constraints)
req.goal_constraints =\
self._psi.transform_constraints(self._psi.world_frame, motion_plan_request.goal_constraints)
req.start_state = motion_plan_request.start_state
req.group_name = motion_plan_request.group_name
req.allowed_time = rospy.Duration(1.5)
ec = ArmNavigationErrorCodes()
try:
traj_resp = self._filter_trajectory_with_constraints_service(req)
ec = traj_resp.error_code
except rospy.ServiceException:
#this almost certainly means the trajectory doesn't reach the goal
ec.val = ec.GOAL_CONSTRAINTS_VIOLATED
if ec.val != ec.SUCCESS:
raise ArmNavError(
'Trajectory filter failed probably because OMPL returned a bad plan.',
error_code=ec,
trajectory=trajectory)
traj = traj_resp.trajectory
if motion_plan_request:
traj = tt.add_state_to_front_of_joint_trajectory\
(self.arm_joint_state(robot_state=motion_plan_request.start_state), traj)
return traj
def plan_pose_collision_free(self,
pose_stamped,
starting_state=None,
ordered_collisions=None,
timeout=15.0,
bounds=None,
planner_id='',
ntries=3):
'''
**Deprecated.**
Use plan_collision_free.
'''
starting_state = self.get_closest_state_in_limits(
robot_state=starting_state)
goal = conv.pose_stamped_to_motion_plan_request(pose_stamped,
self.hand.hand_frame,
self.arm_name,
starting_state,
timeout=timeout,
bounds=bounds,
planner_id=planner_id)
return self.plan_collision_free(goal,
ordered_collisions=ordered_collisions,
ntries=ntries)
def plan_joints_collision_free(self,
joint_state,
starting_state=None,
ordered_collisions=None,
timeout=15.0,
bounds=None,
planner_id='',
ntries=3):
'''
**Deprecated.**
Use plan_collision_free.
'''
starting_state = self.get_closest_state_in_limits(
robot_state=starting_state)
goal = conv.joint_state_to_motion_plan_request(joint_state,
self.hand.hand_frame,
self.arm_name,
starting_state,
timeout=timeout,
bounds=bounds,
planner_id=planner_id)
goal_state = tt.set_joint_state_in_robot_state(
joint_state, copy.deepcopy(starting_state))
rospy.loginfo('Position of wrist is\n' + str(
self._transformer.get_transform(
'l_wrist_roll_link', self._psi.robot_frame, goal_state)))
rospy.loginfo('Position of fingertip is\n' + str(
self._transformer.get_transform('l_gripper_r_finger_tip_link', self
._psi.robot_frame, goal_state)))
rospy.loginfo('Collision objects are\n')
cos = self._psi.collision_objects()
for co in cos:
if 'table' not in co.id:
rospy.loginfo(str(co))
rospy.loginfo('Attached collision objects are\n')
aos = self._psi.attached_collision_objects()
for ao in aos:
rospy.loginfo(ao)
return self.plan_collision_free(goal,
ordered_collisions=ordered_collisions,
ntries=ntries)
def plan_collision_free(self,
goal_in,
starting_state=None,
ordered_collisions=None,
timeout=15.0,
bounds=None,
planner_id='',
ntries=3):
'''
Plans collision free paths for the arms.
The trajectory returned from this function is safe to execute. This function may alter the planning scene
during planning, but returns it to its initial state when finished.
**Args:**
**goal_in (geometry_msgs.msg.PoseStamped, sensor_msgs.msg.JointState or
arm_navigation_msgs.msg.MotionPlanRequest):** The goal for the arm. The pose goal should be for
the hand frame defined in hand (see hand_description.py).
*starting_state (arm_navigation_msgs.msg.RobotState):* The state at which planning starts. If
no state is passed in, this will use the current state in the planning scene interface as the starting
state.
*ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):* Any ordered
collisions not already in the planning scene diff that you want applied during planning.
*timeout (double):* The time in seconds allowed for planning.
*bounds ([double] or [[double]]):* The bounds on the goal defining how close the trajectory must get to
the goal. For a pose goal, this should be a list of 6 numbers corresponding to
(x_allowed_error, y_allowed_error, z_allowed_error, roll_allowed_error, pitch_allowed_error,
yaw_allowed_error). For a joint goal, this should be a list of two lists corresponding to
tolerance above or tolerance below. For a motion plan request, this field is ignored. There are
defaults defined in conversions.py if this is not passed in.
*planner_id (string):* The ID of a specific preferred planner. If the empty string is passed in the
default planner (ROS parameter) is used.
*ntries (int):* OMPL returns bad plans sometimes. This is the number of calls to OMPL allowed before
giving up. The function will only retry if OMPL returns success, but trajectory filtering returns
failure. This can lead to this function taking ntries*timeout. If you pass in less than 1, the
function will try once.
**Returns:**
A trajectory_msgs.msg.JointTrajectory from starting_state to the goal_in. This trajectory has already
been filtered and is safe to execute.
**Raises:**
**exceptions.ArmNavError:** if a plan cannot be found.
**rospy.ServiceException:** if there is a problem with the call to the planning service
**TODO:**
Check that this works even if starting state has a different base pose.
'''
if type(goal_in) == PoseStamped:
goal = conv.pose_stamped_to_motion_plan_request\
(goal_in, self.hand.hand_frame, self.arm_name,\
self.get_closest_state_in_limits(robot_state=starting_state), timeout=timeout, bounds=bounds,\
planner_id=planner_id)
elif type(goal_in) == JointState:
goal = conv.joint_state_to_motion_plan_request\
(goal_in, self.hand.hand_frame, self.arm_name,\
self.get_closest_state_in_limits(robot_state=starting_state), timeout=timeout, bounds=bounds,\
planner_id=planner_id)
elif type(goal_in) == MotionPlanRequest:
goal = goal_in
else:
raise ArmNavError('Unsupported goal type for planning: ' +
str(type(goal_in)))
#save the old planning scene robot state (so we can come back to it later)
#current_ps_state = self._psi.get_robot_state()
#plan in the starting state we've just been passed in
#we plan from this anyway so let's not send a planning scene for it
#self._psi.set_robot_state(goal.start_state)
#add in the ordered collisions
self._psi.add_ordered_collisions(ordered_collisions)
rospy.loginfo('Calling plan collision free')
rospy.logdebug('With goal\n' + str(goal))
for i in range(max(1, ntries)):
filter_error = None
plan = self._move_arm_planner(goal)
if plan.error_code.val == plan.error_code.SUCCESS:
trajectory = plan.trajectory.joint_trajectory
try:
trajectory = self.filter_trajectory(
trajectory, motion_plan_request=goal)
except ArmNavError, filter_error:
continue
#self._psi.set_robot_state(current_ps_state)
self._psi.remove_ordered_collisions(ordered_collisions)
return trajectory
#self._psi.set_robot_state(current_ps_state)
self._psi.remove_ordered_collisions(ordered_collisions)
if filter_error:
raise filter_error
raise ArmNavError('Unable to plan collision-free trajectory',
error_code=plan.error_code,
trajectory=plan.trajectory.joint_trajectory)
def __init__(self, arm_name):
'''
Constructor for ArmPlanner.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') for which to plan
'''
rospy.loginfo("Creating arm planner...")
#public variables
self.arm_name = arm_name
self.hand = HandDescription(arm_name)
self.kinematics_info_service = rospy.ServiceProxy\
('/pr2_'+self.arm_name+ '_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
rospy.loginfo('Waiting for kinematics solver info service')
self.kinematics_info_service.wait_for_service()
self.get_state_validity_service = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity',
GetStateValidity)
rospy.loginfo('Waiting for state validity service.')
self.get_state_validity_service.wait_for_service()
#find the joint names
info = self.kinematics_info_service()
self.joint_names = info.kinematic_solver_info.joint_names
self.joint_limits = info.kinematic_solver_info.limits
for l in range(len(self.joint_limits)):
#for some reason this isn't filled in on return
self.joint_limits[l].joint_name = self.joint_names[l]
#private variables
self._psi = get_planning_scene_interface()
self._transformer = TransformState()
#services (private)
self._move_arm_planner = rospy.ServiceProxy(PLANNER_NAME, GetMotionPlan)
rospy.loginfo('Waiting for move arm planner')
self._move_arm_planner.wait_for_service()
self._interpolated_ik_planning_service = rospy.ServiceProxy\
('/'+self.arm_name[0]+'_interpolated_ik_motion_plan', GetMotionPlan)
rospy.loginfo('Waiting for interpolated IK planning service')
self._interpolated_ik_planning_service.wait_for_service()
self._interpolated_ik_parameter_service =rospy.ServiceProxy\
('/'+self.arm_name[0]+'_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams)
rospy.loginfo('Waiting for interpolated IK parameter setting service')
self._interpolated_ik_parameter_service.wait_for_service()
self._filter_trajectory_with_constraints_service = rospy.ServiceProxy\
('/trajectory_filter_server/filter_trajectory_with_constraints', FilterJointTrajectoryWithConstraints)
rospy.loginfo('Waiting for trajectory filter with constraints service')
self._filter_trajectory_with_constraints_service.wait_for_service()
self._collision_aware_ik_service = rospy.ServiceProxy\
('/pr2_'+self.arm_name+'_kinematics/get_constraint_aware_ik', GetConstraintAwarePositionIK)
rospy.loginfo('Waiting for collision aware IK service')
self._collision_aware_ik_service.wait_for_service()
self._ik_service = rospy.ServiceProxy('/pr2_'+self.arm_name+'_kinematics/get_ik', GetPositionIK)
rospy.loginfo('Waiting for IK service')
self._ik_service.wait_for_service()
self._fk_service = rospy.ServiceProxy('/pr2_'+self.arm_name+'_kinematics/get_fk', GetPositionFK)
rospy.loginfo('Waiting for FK service')
self._fk_service.wait_for_service()
database_grasp_planner_name = rospy.get_param('/object_manipulator_1/default_database_planner',
DEFAULT_DATABASE_GRASP_PLANNER)
print 'default database grasp planner'
print database_grasp_planner_name
self.database_grasp_planner = rospy.ServiceProxy(database_grasp_planner_name, GraspPlanning)
rospy.loginfo('Waiting for database grasp planner')
self.database_grasp_planner.wait_for_service()
self._grasps_pub = rospy.Publisher(GRASPS_VISUALIZATION_PUB, MarkerArray, latch=True)
rospy.loginfo("Arm planner created")
class ArmPlanner:
'''
This class can be used to plan for the PR2 arms.
This allows you to plan several motions in a row before executing them. This class can be used for collision
free planning, trajectory filtering, and interpolated IK plannig. The function you should mainly use to plan
for the arms is plan_collision_free.
The class also has a number of arm-related functions, such as forward and inverse kinematics,
that are related to planning, although not planning themselves. It also has some utility functions involving
working with joint states and robot states where it is necessary to know joint names (functions for which it
is not necessary to use joint names are in trajectory_tools.py).
This class relies heavily on the planning scene interface. It makes no calls to TF.
**Attributes:**
**arm_name (string):** The name of the arm for which to plan ('right_arm' or 'left_arm')
**hand (hand_description.HandDescription):** Hand description for the arm.
**joint_names ([string]):** Names of the arm joints
**joint_limits ([arm_navigation_msgs.msg.JointLimits]):** Limits for the arm joints
**kinematics_info_service (rospy.ServiceProxy):** Service for retreiving kinematic information about the arm
**get_state_validity_service (rospy.ServiceProxy):** Service for ascertaining state validity.
Be aware that this service has several bugs at the moment.
'''
def __init__(self, arm_name):
'''
Constructor for ArmPlanner.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') for which to plan
'''
rospy.loginfo("Creating arm planner...")
#public variables
self.arm_name = arm_name
self.hand = HandDescription(arm_name)
self.kinematics_info_service = rospy.ServiceProxy\
('/pr2_'+self.arm_name+ '_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
rospy.loginfo('Waiting for kinematics solver info service')
self.kinematics_info_service.wait_for_service()
self.get_state_validity_service = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity',
GetStateValidity)
rospy.loginfo('Waiting for state validity service.')
self.get_state_validity_service.wait_for_service()
#find the joint names
info = self.kinematics_info_service()
self.joint_names = info.kinematic_solver_info.joint_names
self.joint_limits = info.kinematic_solver_info.limits
for l in range(len(self.joint_limits)):
#for some reason this isn't filled in on return
self.joint_limits[l].joint_name = self.joint_names[l]
#private variables
self._psi = get_planning_scene_interface()
self._transformer = TransformState()
#services (private)
self._move_arm_planner = rospy.ServiceProxy(PLANNER_NAME, GetMotionPlan)
rospy.loginfo('Waiting for move arm planner')
self._move_arm_planner.wait_for_service()
self._interpolated_ik_planning_service = rospy.ServiceProxy\
('/'+self.arm_name[0]+'_interpolated_ik_motion_plan', GetMotionPlan)
rospy.loginfo('Waiting for interpolated IK planning service')
self._interpolated_ik_planning_service.wait_for_service()
self._interpolated_ik_parameter_service =rospy.ServiceProxy\
('/'+self.arm_name[0]+'_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams)
rospy.loginfo('Waiting for interpolated IK parameter setting service')
self._interpolated_ik_parameter_service.wait_for_service()
self._filter_trajectory_with_constraints_service = rospy.ServiceProxy\
('/trajectory_filter_server/filter_trajectory_with_constraints', FilterJointTrajectoryWithConstraints)
rospy.loginfo('Waiting for trajectory filter with constraints service')
self._filter_trajectory_with_constraints_service.wait_for_service()
self._collision_aware_ik_service = rospy.ServiceProxy\
('/pr2_'+self.arm_name+'_kinematics/get_constraint_aware_ik', GetConstraintAwarePositionIK)
rospy.loginfo('Waiting for collision aware IK service')
self._collision_aware_ik_service.wait_for_service()
self._ik_service = rospy.ServiceProxy('/pr2_'+self.arm_name+'_kinematics/get_ik', GetPositionIK)
rospy.loginfo('Waiting for IK service')
self._ik_service.wait_for_service()
self._fk_service = rospy.ServiceProxy('/pr2_'+self.arm_name+'_kinematics/get_fk', GetPositionFK)
rospy.loginfo('Waiting for FK service')
self._fk_service.wait_for_service()
database_grasp_planner_name = rospy.get_param('/object_manipulator_1/default_database_planner',
DEFAULT_DATABASE_GRASP_PLANNER)
print 'default database grasp planner'
print database_grasp_planner_name
self.database_grasp_planner = rospy.ServiceProxy(database_grasp_planner_name, GraspPlanning)
rospy.loginfo('Waiting for database grasp planner')
self.database_grasp_planner.wait_for_service()
self._grasps_pub = rospy.Publisher(GRASPS_VISUALIZATION_PUB, MarkerArray, latch=True)
rospy.loginfo("Arm planner created")
def arm_joint_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm in the current planning scene state or the passed in state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
A sensor_msgs.msg.JointState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
joint_state = JointState()
joint_state.header = robot_state.joint_state.header
for n in self.joint_names:
found = False
for i in range(len(robot_state.joint_state.name)):
if robot_state.joint_state.name[i] == n:
joint_state.name.append(n)
joint_state.position.append(robot_state.joint_state.position[i])
found = True
break
if not found and fail_if_joint_not_found:
raise ValueError('Joint '+n+' is missing from robot state')
return joint_state
def arm_robot_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm (as a robot state) in the current planning scene state or the passed in
state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
An arm_navigation_msgs.msg.RobotState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
joint_state = self.arm_joint_state(robot_state=robot_state, fail_if_joint_not_found=fail_if_joint_not_found)
robot_state = RobotState()
robot_state.joint_state = joint_state
return robot_state
def joint_positions_to_joint_state(self, joint_pos):
'''
Converts a list of joint positions to a joint state.
**Args:**
**joint_pos ([double]):** An array of joint positions
**Returns:**
A sensor_msgs.msg.JointState corresponding to these joint positions. Assumes they are in the same order
as joint_names.
'''
joint_state = JointState()
joint_state.name = copy.copy(self.joint_names)
joint_state.position = joint_pos
return joint_state
def set_joint_positions_in_robot_state(self, joint_pos, robot_state):
'''
Sets joint positions in a robot state.
**Args:**
**joint_pos ([double]):** An array of joint positions
**robot_state (arm_navigation_msgs.msg.RobotState):** A robot state in which to set the joint angles
equal to joint_pos
**Returns:**
An arm_navigation_msgs.msg.RobotState in which the joint angles corresponding to this arm are set to
joint_pos (assumes these are in the same order as joint_names) and all other joints match the passed
in robot_state. Also sets these joints in the passed in robot state.
'''
return tt.set_joint_state_in_robot_state(self.joint_positions_to_joint_state(joint_pos), robot_state)
def joint_trajectory(self, joint_trajectory):
'''
Returns just the part of the trajectory corresponding to the arm.
**Args:**
**joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert
**Returns:**
A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory
**Raises:**
**ValueError:** If some arm joint is not found in joint_trajectory
'''
arm_trajectory = JointTrajectory()
arm_trajectory.header = copy.deepcopy(joint_trajectory.header)
arm_trajectory.joint_names = copy.copy(self.joint_names)
indexes = [-1]*len(self.joint_names)
for (i, an) in enumerate(self.joint_names):
for (j, n) in enumerate(joint_trajectory.joint_names):
if an == n:
indexes[i] = j
break
if indexes[i] < 0:
raise ValueError('Joint '+n+' is missing from joint trajectory')
for joint_point in joint_trajectory.points:
arm_point = JointTrajectoryPoint()
arm_point.time_from_start = joint_point.time_from_start
for i in indexes:
arm_point.positions.append(joint_point.positions[i])
arm_trajectory.points.append(arm_point)
return arm_trajectory
def get_ik(self, pose_stamped, collision_aware=True, starting_state=None,
seed_state=None, timeout=5.0, ordered_collisions=None):
'''
Solves inverse kinematics problems.
**Args:**
**pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions
*collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world
*starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
matching this state. If no state is passed in, it will use the current state in the planning scene
interface.
*seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK. If no state is passed it, it
will use the current state in planning scene interface.
*timeout (double):* Time in seconds that IK is allowed to run
**Returns:**
A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False. The robot state returned has
the arm joints set to the IK solution if found and all other joints set to that of starting_state.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls an IK service which may use TF for transformations! Probably best to only use with pose stampeds
defined in the robot frame (convert them yourself using the planning scene interface).
'''
rospy.logdebug('Solving IK for\n'+str(pose_stamped))
pos_req = PositionIKRequest()
pos_req.ik_link_name = self.hand.hand_frame
pos_req.pose_stamped = pose_stamped
if not starting_state:
starting_state = self._psi.get_robot_state()
if not seed_state:
seed_state = self.arm_robot_state(starting_state)
pos_req.ik_seed_state = seed_state
pos_req.robot_state = starting_state
if collision_aware:
self._psi.add_ordered_collisions(ordered_collisions)
coll_req = GetConstraintAwarePositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._collision_aware_ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(coll_res.solution.joint_state,
copy.deepcopy(starting_state))
self._psi.remove_ordered_collisions(ordered_collisions)
return coll_res
coll_req = GetPositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(coll_res.solution.joint_state,
copy.deepcopy(starting_state))
return coll_res
def get_fk(self, fk_links, robot_state=None, frame_id=None):
'''
Solves forward kinematics.
In general, it is better to use the planning scene or the state transformer get_transform function between
robot links rather than this function.
**Args:**
**fk_links ([string]):** Links for which you want FK solutions
*robot_state (arm_navigation_msgs.mgs.RobotState):* The state of the robot during forward kinematics.
If no state is passed in the state in the planning scene will be used.
*frame_id (string):* The frame ID in which you want the returned poses. Note that the FK service may use
TF so we recommend only using the robot frame. If no frame is specified, the robot frame is used.
**Returns:**
A list of geometry_msgs.msg.PoseStamped corresponding to forward kinematic solutions for fk_links.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls a service which may use TF! Recommended that you only ask for poses in the robot frame.
'''
req = GetPositionFKRequest()
req.header.frame_id = frame_id
if not frame_id:
req.header.frame_id = self._psi.robot_frame
req.header.stamp = rospy.Time(0)
req.robot_state = robot_state
if not robot_state:
req.robot_state = self._psi.get_robot_state()
req.fk_link_names = fk_links
res = self._fk_service(req)
if res.error_code.val != res.error_code.SUCCESS or not res.pose_stamped:
raise ArmNavError('Forward kinematics failed', error_code=res.error_code)
return res.pose_stamped
def get_hand_frame_pose(self, robot_state=None, frame_id=None):
'''
Returns the pose of the hand in the current or passed in robot state.
Note that this function uses the TransformState get_transform function rather than FK.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state in which you want to find
the pose of the hand frame. If nothing is passed in, returns the pose of the hand frame in the robot
state in the planning scene interface.
*frame_id (string):* The frame in which you want the pose of the hand frame. If nothing is passed in,
returns in the robot frame.
**Returns:**
A geometry_msgs.msg.PoseStamped that is the position of the hand frame.
'''
if frame_id == None:
frame_id = self._psi.robot_frame
if robot_state == None:
robot_state = self._psi.get_robot_state()
trans = self._transformer.get_transform(self.hand.hand_frame, frame_id, robot_state)
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = conv.transform_to_pose(trans.transform)
return ps
def hand_marker(self, robot_state=None, ns='', r = 0.0, g = 0.0, b = 1.0, a = 0.8, scale = 1.0):
'''
Returns a MarkerArray of the hand in the current or passed in state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The state in which to draw the hand. If no state
is specified, the robot state in the planning scene interface is used.
*ns (string):* Marker namespace
*r (double):* Red value (between 0 and 1)
*g (double):* Green value (between 0 and 1)
*b (double):* Blue value (between 0 and 1)
*a (double):* Alpha value (between 0 and 1)
*scale (double):* Scale
**Returns:**
A visualization_msgs.msg.MarkerArray that when published shows the hand in the position specified by
the robot state.
**Raises:**
**rospy.ServiceException:** if there is a problem calling the service for getting the robot markers
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
return vt.robot_marker(robot_state, link_names=self.hand.hand_links, ns=ns, r=r, g=g, b=b, a=a, scale=scale)
def get_closest_joint_state_in_limits(self, robot_state=None):
'''
Finds the closest joint state to the passed in or current robot state that is within joint limits.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state. If no state is passed in
the current state in the planning scene interface is used.
**Returns:**
A sensor_msgs.msg.JointState that is the closest state in which the arm joints are all within the joint
limits. The return has only the arm joints, but it also sets the joints in the passed in robot state.
'''
return self.arm_joint_state(self.get_closest_state_in_limits(robot_state=robot_state))
def get_closest_state_in_limits(self, robot_state=None):
'''
Finds the closest robot state to the passed in or current robot state that is within joint limits.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state. If no state is passed in
the current state in the planning scene interface is used.
**Returns:**
An arm_navigation_msgs.msg.RobotState that is the closest state in which the arm joints are all within
the joint limits. Also sets the joints in the passed in robot state.
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
robot_state.joint_state.position = list(robot_state.joint_state.position)
#is this outside the joint limits?
#if so, modify it slightly so that it is not
for j in range(len(robot_state.joint_state.name)):
for limit in self.joint_limits:
if limit.joint_name == robot_state.joint_state.name[j]:
jp = robot_state.joint_state.position[j]
if limit.has_position_limits:
if jp < limit.min_position:
robot_state.joint_state.position[j] = limit.min_position+JOINT_LIMIT_PAD
elif jp > limit.max_position:
robot_state.joint_state.position[j] = limit.max_position-JOINT_LIMIT_PAD
break
return robot_state
def filter_trajectory(self, trajectory, motion_plan_request=None):
'''
Filters a joint trajectory and assigns times using the joint trajectory filter service.
**Args:**
**trajectory (trajectory_msgs.msg.JointTrajectory):** The trajectory to be filtered. All times must be 0.
*motion_plan_request (arm_navigation_msgs.msg.MotionPlanRequest):* If passed in the trajectory
filter will respect the starting state, goal constriants, and path constraints. It will also append the
starting state from the motion plan request to the front of the trajectory.
**Returns:**
A trajectory_msgs.msg.JointTrajectory that has been filtered and had times assigned.
**Raises:**
**exceptions.ArmNavError:** if the trajectory cannot be filtered. This is almost always because the
trajectory did not actually reach the goal state or had a collision. Note that OMPL returns bad plans
with reasonable frequency so if trajectory filtering fails, you want to re-plan.
'''
if len(trajectory.points) < SHORTEST_FILTERABLE_TRAJECTORY:
rospy.logwarn('Not filtering trajectory of length %d. Too small.',
len(trajectory.points))
trajectory = tt.convert_path_to_trajectory(trajectory)
if motion_plan_request:
trajectory = tt.add_state_to_front_of_joint_trajectory\
(self.arm_joint_state(robot_state=motion_plan_request.start_state), trajectory)
return trajectory
req = FilterJointTrajectoryWithConstraintsRequest()
req.trajectory = trajectory
#these have to be in the world frame... go figure
if motion_plan_request:
req.path_constraints =\
self._psi.transform_constraints(self._psi.world_frame, motion_plan_request.path_constraints)
req.goal_constraints =\
self._psi.transform_constraints(self._psi.world_frame, motion_plan_request.goal_constraints)
req.start_state = motion_plan_request.start_state
req.group_name = motion_plan_request.group_name
else:
req.start_state = self._psi.get_robot_state()
req.group_name = self.arm_name
req.allowed_time = rospy.Duration(1.5)
ec = ArmNavigationErrorCodes()
try:
traj_resp = self._filter_trajectory_with_constraints_service(req)
ec = traj_resp.error_code
except rospy.ServiceException:
#this almost certainly means the trajectory doesn't reach the goal
ec.val = ec.GOAL_CONSTRAINTS_VIOLATED
if ec.val != ec.SUCCESS:
raise ArmNavError('Trajectory filter failed probably because OMPL returned a bad plan.',
error_code=ec, trajectory=trajectory)
traj = traj_resp.trajectory
if motion_plan_request:
traj = tt.add_state_to_front_of_joint_trajectory\
(self.arm_joint_state(robot_state=motion_plan_request.start_state), traj)
return traj
def plan_pose_collision_free(self, pose_stamped, starting_state=None, ordered_collisions=None, timeout=15.0,
bounds=None, planner_id='', ntries=3):
'''
**Deprecated.**
Use plan_collision_free.
'''
starting_state = self.get_closest_state_in_limits(robot_state=starting_state)
goal = conv.pose_stamped_to_motion_plan_request(pose_stamped, self.hand.hand_frame, self.arm_name,
starting_state, timeout=timeout,
bounds=bounds, planner_id=planner_id)
return self.plan_collision_free(goal, ordered_collisions=ordered_collisions, ntries=ntries)
def plan_joints_collision_free(self, joint_state, starting_state=None, ordered_collisions=None, timeout=15.0,
bounds=None, planner_id='', ntries=3):
'''
**Deprecated.**
Use plan_collision_free.
'''
starting_state = self.get_closest_state_in_limits(robot_state=starting_state)
goal = conv.joint_state_to_motion_plan_request(joint_state, self.hand.hand_frame, self.arm_name,
starting_state, timeout=timeout,
bounds=bounds, planner_id=planner_id)
goal_state = tt.set_joint_state_in_robot_state(joint_state, copy.deepcopy(starting_state))
rospy.loginfo('Position of wrist is\n'+str(self._transformer.get_transform
('l_wrist_roll_link', self._psi.robot_frame, goal_state)))
rospy.loginfo('Position of fingertip is\n'+str(self._transformer.get_transform('l_gripper_r_finger_tip_link',
self._psi.robot_frame,
goal_state)))
rospy.loginfo('Collision objects are\n')
cos = self._psi.collision_objects()
for co in cos:
if 'table' not in co.id:
rospy.loginfo(str(co))
rospy.loginfo('Attached collision objects are\n')
aos = self._psi.attached_collision_objects()
for ao in aos:
rospy.loginfo(ao)
return self.plan_collision_free(goal, ordered_collisions=ordered_collisions, ntries=ntries)
def plan_collision_free(self, goal_in, starting_state=None, ordered_collisions=None, timeout=15.0, bounds=None,
planner_id='', ntries=3):
'''
Plans collision free paths for the arms.
The trajectory returned from this function is safe to execute. This function may alter the planning scene
during planning, but returns it to its initial state when finished.
**Args:**
**goal_in (geometry_msgs.msg.PoseStamped, sensor_msgs.msg.JointState or
arm_navigation_msgs.msg.MotionPlanRequest):** The goal for the arm. The pose goal should be for
the hand frame defined in hand (see hand_description.py).
*starting_state (arm_navigation_msgs.msg.RobotState):* The state at which planning starts. If
no state is passed in, this will use the current state in the planning scene interface as the starting
state.
*ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):* Any ordered
collisions not already in the planning scene diff that you want applied during planning.
*timeout (double):* The time in seconds allowed for planning.
*bounds ([double] or [[double]]):* The bounds on the goal defining how close the trajectory must get to
the goal. For a pose goal, this should be a list of 6 numbers corresponding to
(x_allowed_error, y_allowed_error, z_allowed_error, roll_allowed_error, pitch_allowed_error,
yaw_allowed_error). For a joint goal, this should be a list of two lists corresponding to
tolerance above or tolerance below. For a motion plan request, this field is ignored. There are
defaults defined in conversions.py if this is not passed in.
*planner_id (string):* The ID of a specific preferred planner. If the empty string is passed in the
default planner (ROS parameter) is used.
*ntries (int):* OMPL returns bad plans sometimes. This is the number of calls to OMPL allowed before
giving up. The function will only retry if OMPL returns success, but trajectory filtering returns
failure. This can lead to this function taking ntries*timeout. If you pass in less than 1, the
function will try once.
**Returns:**
A trajectory_msgs.msg.JointTrajectory from starting_state to the goal_in. This trajectory has already
been filtered and is safe to execute.
**Raises:**
**exceptions.ArmNavError:** if a plan cannot be found.
**rospy.ServiceException:** if there is a problem with the call to the planning service
**TODO:**
Check that this works even if starting state has a different base pose.
'''
if type(goal_in) == PoseStamped:
goal = conv.pose_stamped_to_motion_plan_request\
(goal_in, self.hand.hand_frame, self.arm_name,\
self.get_closest_state_in_limits(robot_state=starting_state), timeout=timeout, bounds=bounds,\
planner_id=planner_id)
elif type(goal_in) == JointState:
goal = conv.joint_state_to_motion_plan_request\
(goal_in, self.hand.hand_frame, self.arm_name,\
self.get_closest_state_in_limits(robot_state=starting_state), timeout=timeout, bounds=bounds,\
planner_id=planner_id)
elif type(goal_in) == MotionPlanRequest:
goal = goal_in
else:
raise ArmNavError('Unsupported goal type for planning: '+str(type(goal_in)))
#save the old planning scene robot state (so we can come back to it later)
#current_ps_state = self._psi.get_robot_state()
#plan in the starting state we've just been passed in
#we plan from this anyway so let's not send a planning scene for it
#self._psi.set_robot_state(goal.start_state)
#add in the ordered collisions
self._psi.add_ordered_collisions(ordered_collisions)
rospy.loginfo('Calling plan collision free')
rospy.logdebug('With goal\n'+str(goal))
for i in range(max(1,ntries)):
filter_error = None
plan = self._move_arm_planner(goal)
if plan.error_code.val == plan.error_code.SUCCESS:
trajectory = plan.trajectory.joint_trajectory
try:
trajectory = self.filter_trajectory(trajectory, motion_plan_request=goal)
except ArmNavError, filter_error:
continue
#self._psi.set_robot_state(current_ps_state)
self._psi.remove_ordered_collisions(ordered_collisions)
return trajectory
#self._psi.set_robot_state(current_ps_state)
self._psi.remove_ordered_collisions(ordered_collisions)
if filter_error:
raise filter_error
raise ArmNavError('Unable to plan collision-free trajectory', error_code=plan.error_code,
trajectory=plan.trajectory.joint_trajectory)