def time_plate_main():
rospy.loginfo('Reading file')
[result, goal, collision_objects, robot_state] = pickle.load(open(sys.argv[1], 'r'))
rospy.loginfo('Original starting state was\n'+str(robot_state))
markers = vt.robot_marker(robot_state, ns='robot_starting_state', a = 0.5)
pub = rospy.Publisher('/darrt_planning/robot_state_markers', MarkerArray)
for i in range(10):
rospy.loginfo('Publishing starting state!')
pub.publish(markers)
rospy.sleep(0.1)
wi = WorldInterface()
wi.reset(repopulate=False)
psi = get_planning_scene_interface()
psi.reset()
plate = None
for co in collision_objects:
wi.add_object(co)
if (co.id == goal.pickup_goal.collision_object_name):
plate = co
psi.reset()
ops = PoseStamped()
ops.header = copy.deepcopy(plate.header)
ops.pose = copy.deepcopy(plate.poses[0])
#the old pickup goal may have used base link to generate grasps
#i don't know what we do with things for which we need a point cloud
goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id),
object_pose_stamped=ops, collision_object_name=goal.pickup_goal.collision_object_name,
collision_support_surface_name=goal.pickup_goal.collision_support_surface_name,
desired_grasps=plate_grasps(ops, ops.header.frame_id))
rospy.loginfo('Teleop to initial state. Ready to go?')
raw_input()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal.execute = False
goal.planning_time = 200
goal.tries = 10000
goal.debug_level = 1
goal.do_pause = False
goal.goal_bias = 0.2
# goal.place_goal.place_locations[0].pose.position.x = 1
# goal.place_goal.place_locations[0].pose.position.y = 1
# goal.place_goal.place_locations[0].pose.position.z = 0.86
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('File: '+sys.argv[1]+', restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
def pickplace_main():
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
wi = WorldInterface()
psi = get_planning_scene_interface()
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal = DARRTGoal()
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.lift_distance = 0.3
goal.pickup_goal.arm_name = 'right_arm'
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -1.3
place_pose_stamped.pose.position.y = 0.7
if 'graspable' in goal.pickup_goal.collision_object_name:
place_pose_stamped.pose.position.z = 0.8
else:
place_pose_stamped.pose.position.z = 0.75
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'serving_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.object_sampling_fraction = 0.3
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 30
goal.tries = 10
goal.goal_bias = 0.2
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('Restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
def __init__(self):
self.arm_planner = ArmPlanner('right_arm')
self.arm_mover = ArmMover()
self.base_plan_service = rospy.ServiceProxy('/move_base/make_plan',
GetPlan)
self.psi = get_planning_scene_interface()
self.viz_pub = rospy.Publisher('lower_bound', MarkerArray)
self.pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped)
def plan(self, update_ps = True):
psi = get_planning_scene_interface()
self.joint_goal, err = get_joint_goal(self.name, self.target, psi.get_robot_state(), self.seed)
#print joint_goal
if err is not None and err.val != err.SUCCESS:
self.joint_goal = None
return ErrorCode("IK error: " + arm_nav_error_dict[err.val])
if update_ps:
set_planning_scene_joint_state(self.joint_goal)
return ErrorCode()
def plan(self, update_ps = True):
psi = get_planning_scene_interface()
joint_goal, err = get_joint_goal(self.name, self.target, psi.get_robot_state())
#print joint_goal
if err is not None and err.val != err.SUCCESS:
self.goal = None
return ErrorCode("IK error: " + arm_nav_error_dict[err.val])
req = GetMotionPlanRequest()
req.motion_plan_request.group_name = self.name
req.motion_plan_request.num_planning_attempts = 1
req.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
req.motion_plan_request.planner_id= ""
#req.motion_plan_request.start_state = planning_scene.get_current_scene().robot_state
for n,p in zip(joint_goal.name, joint_goal.position):
new_constraint = JointConstraint()
new_constraint.joint_name = n
new_constraint.position = p
new_constraint.tolerance_below = 0.05
new_constraint.tolerance_above = 0.05
req.motion_plan_request.goal_constraints.joint_constraints.append(new_constraint)
if self.constraint_aware:
res = self.planner(req)
print "Planner Result: ", arm_nav_error_dict[res.error_code.val]
if not self.constraint_aware or res.error_code.val == res.error_code.SUCCESS:
self.goal = MoveArmGoal()
self.goal.planner_service_name = self.planner_service_name
self.goal.motion_plan_request = req.motion_plan_request
if self.constraint_aware:
self.goal.operations = deepcopy(psi.current_diff.operations)
else:
op = CollisionOperation()
op.object1 = op.COLLISION_SET_OBJECTS
op.object2 = op.COLLISION_SET_ATTACHED_OBJECTS
op.operation = op.DISABLE
self.goal.operations.collision_operations.append(op)
if update_ps:
if self.constraint_aware:
set_planning_scene_joint_state(last_state_on_joint_trajectory(res.trajectory.joint_trajectory))
else:
set_planning_scene_joint_state(joint_goal)
#print "points:", res.trajectory.joint_trajectory.points
return ErrorCode()
else:
self.goal = None
return ErrorCode(arm_nav_error_dict[res.error_code.val])
def plan(self, update_ps=True):
psi = get_planning_scene_interface()
ec = MoveArmUnplanned.plan(self, False)
if not ec.success:
return ec
req = GetMotionPlanRequest()
req.motion_plan_request.group_name = self.name
req.motion_plan_request.num_planning_attempts = 1
req.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
req.motion_plan_request.planner_id = ""
#req.motion_plan_request.start_state = planning_scene.get_current_scene().robot_state
for n, p in zip(self.joint_goal.name, self.joint_goal.position):
new_constraint = JointConstraint()
new_constraint.joint_name = n
new_constraint.position = p
new_constraint.tolerance_below = 0.05
new_constraint.tolerance_above = 0.05
req.motion_plan_request.goal_constraints.joint_constraints.append(
new_constraint)
if self.constraint_aware:
res = self.planner(req)
print "Planner Result: ", arm_nav_error_dict[res.error_code.val]
if not self.constraint_aware or res.error_code.val == res.error_code.SUCCESS:
self.goal = MoveArmGoal()
self.goal.planner_service_name = self.planner_service_name
self.goal.motion_plan_request = req.motion_plan_request
if self.constraint_aware:
self.goal.operations = deepcopy(psi.current_diff.operations)
else:
op = CollisionOperation()
op.object1 = op.COLLISION_SET_OBJECTS
op.object2 = op.COLLISION_SET_ATTACHED_OBJECTS
op.operation = op.DISABLE
self.goal.operations.collision_operations.append(op)
if update_ps:
set_planning_scene_joint_state(self.joint_goal)
#print "points:", res.trajectory.joint_trajectory.points
return ErrorCode()
else:
self.goal = None
return ErrorCode(arm_nav_error_dict[res.error_code.val])
def main():
rospy.init_node("touring", anonymous=True)
sm = smach.StateMachine(outcomes=["success",
"failure"])
base_mover = base.Base()
detector = TablewareDetection()
pickplace = PickPlace()
pickplace.min_open_gripper_pos = 0.0014
world = two_arms_states.WorldState()
interface = WorldInterface()
tasks = Tasks()
placer_r = SensingPlacer('right_arm')
placer_l = SensingPlacer('left_arm')
interface.reset()
psi = get_planning_scene_interface()
psi.reset()
tasks.move_arms_to_side()
rospy.loginfo("Creating State Machine")
with sm:
detect = create_detect_sm(world, base_mover, detector)
smach.StateMachine.add("detect", detect,
transitions = {"success":"clean_table",
"failure":"failure"}
)
clean_table = clean_table_sm(world, tasks, base_mover, placer_l, placer_r, interface)
smach.StateMachine.add("clean_table", clean_table,
transitions = {"success":"setup_table",
"failure":"failure"
}
)
setup_table = setup_table_sm(world, tasks, base_mover, placer_l, placer_r, interface)
smach.StateMachine.add("setup_table", setup_table,
transitions = {"success":"success",
"failure":"failure"
}
)
outcome = sm.execute()
rospy.loginfo("Outcome: %s", outcome)
def __init__(self):
arms = ['right_arm', 'left_arm']
self.grippers = {}
self.arm_planners = {}
for arm in arms:
self.grippers[arm] = Gripper(arm)
self.arm_planners[arm] = ArmPlanner(arm)
self.arm_mover = ArmMover()
self.arm_tasks = ArmTasks()
self.base = Base()
self.wi = WorldInterface()
self.psi = get_planning_scene_interface()
self.base_action = SimpleActionClient('base_trajectory_action',
BaseTrajectoryAction)
rospy.loginfo('Waiting for base trajectory action')
self.base_action.wait_for_server()
rospy.loginfo('Found base trajectory action')
rospy.loginfo('DARRT trajectory executor successfully initialized!')
def main():
[result, goal, collision_objects,
robot_state] = pickle.load(open(sys.argv[1], 'r'))
wi = WorldInterface()
#set the planning scene up correctly
wi.reset(repopulate=False)
for co in collision_objects:
#for running easy version
if ('table' in co.id or co.id == 'plate'):
wi.add_object(co)
psi = get_planning_scene_interface()
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
average_time = 0
goal.planning_time = 30
goal.tries = 10000
goal.debug_level = 0
goal.do_pause = False
goal.goal_bias = 0.2
#i think that this does not work
#goal.robot_state = robot_state
goal.execute = False
rospy.loginfo('First place goal is\n' +
str(goal.place_goal.place_locations[0]))
rospy.loginfo('File: ' + sys.argv[1] + ', restart after ' +
str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error ' +
str(result.error_code) + '. We had ' + str(i) +
' successful trials with an average time of ' +
str(average_time / float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL ' + str(i) + ': ' + str(result.planning_time))
rospy.loginfo(
str(NTRIALS) + ' averaged ' + str(average_time / float(NTRIALS)) +
' seconds to solve')
def __init__(self,
world,
screenshot,
fake_walls=False,
box_plate=False,
real=False):
self.world = world
self.screenshot = screenshot
self.wi = WorldInterface()
self.psi = get_planning_scene_interface()
self.pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped)
self.arms = ArmTasks()
self.torso = Torso()
self.gripper = {
'right_arm': Gripper('right_arm'),
'left_arm': Gripper('left_arm')
}
self.box_plate = box_plate
self.real = real
self.fake_walls = fake_walls
def execute(self):
get_planning_scene_interface().reset()
for ex in self.executables:
print "\nStart executing executable " + ex.type
yield ex.execute()
def set_planning_scene_joint_state(js):
psi = get_planning_scene_interface()
rs = psi.get_robot_state()
set_joint_state_in_robot_state(js,rs)
psi.set_robot_state(rs)
psi.send_diff()
def plate_main():
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
wi = WorldInterface()
psi = get_planning_scene_interface()
detector = TablewareDetection()
plate = wi.collision_object('plate')
good_detection = False
goal = DARRTGoal()
if plate:
rospy.loginfo('Use last detection?')
good_detection = (raw_input() == 'y')
ops = PoseStamped()
ops.header = copy.deepcopy(plate.header)
ops.pose = copy.deepcopy(plate.poses[0])
goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id),
object_pose_stamped=ops, collision_object_name='plate',
collision_support_surface_name='serving_table')
while not good_detection:
det = detector.detect_objects(add_table_to_map=False,
add_objects_as_mesh=False, table_name='serving_table')
goal.pickup_goal = get_plate(det.pickup_goals, det.table.pose, wi)
psi.reset()
if not goal.pickup_goal:
rospy.loginfo('Nothing to pick up!')
rospy.loginfo('Good detection?')
good_detection = (raw_input() == 'y')
if not goal.pickup_goal:
rospy.loginfo('Nothing to pick up!')
return
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal.pickup_goal.arm_name = 'right_arm'
goal.pickup_goal.desired_grasps = plate_grasps(goal.pickup_goal.object_pose_stamped,
goal.pickup_goal.target.reference_frame_id)
goal.pickup_goal.lift.desired_distance = 0.3
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
# place_pose_stamped.pose.position.x = 1.3
# place_pose_stamped.pose.position.y = -0.3
# place_pose_stamped.pose.position.z = 1.01
place_pose_stamped.pose.position.x = 0.0
place_pose_stamped.pose.position.y = 0.0
place_pose_stamped.pose.position.z = 0.76
place_pose_stamped.pose.orientation.w = 1.0
# place_pose_stamped.pose.orientation.x = 0.1
# place_pose_stamped.pose.orientation.y = 0.1
# place_pose_stamped.pose.orientation.w = np.sqrt(0.98)
#place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
#place_pose_stamped.pose.position.x -= 0.2
#place_pose_stamped.pose.position.y -= 0.2
#place_pose_stamped.pose.position.z += 0.2
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'dirty_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.place_goal.approach.direction.header.frame_id = wi.world_frame
goal.place_goal.approach.direction.vector.x = np.sqrt(0.18)
goal.place_goal.approach.direction.vector.y = -np.sqrt(0.18)
goal.place_goal.approach.direction.vector.z = -0.8
goal.place_goal.approach.desired_distance = 0.2
goal.primitives = [goal.PICK, goal.PLACE, goal.PUSH, goal.BASE_TRANSIT]
goal.min_grasp_distance_from_surface = 0.19
goal.object_sampling_fraction = 0.7
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 6000
goal.tries = 1
goal.goal_bias = 0.2
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
marray = MarkerArray()
if result.error_code == result.SUCCESS:
#pickle everything!! great excitement
filename = 'trajectory_and_objects.pck'
rospy.loginfo('Pickling to '+filename)
#the last bit allows us to recreate the planning
pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
rospy.loginfo('Successful write')
for t in result.primitive_trajectories:
marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
for (i, m) in enumerate(marray.markers):
m.id = i
(m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
while not rospy.is_shutdown():
pub.publish(marray)
rospy.sleep(0.1)
def main():
#Find an object to pick up
psi = get_planning_scene_interface()
psi.reset()
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
psi.reset()
#DARRT action client
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
rospy.loginfo('Waiting for DARRT action')
client.wait_for_server()
rospy.loginfo('Found DARRT action')
#DARRTGoal
goal = DARRTGoal()
#pickup goal (from the tabletop detection)
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
#world interface to tell us whether we're using the
#map or odom_combined frame
wi = WorldInterface()
#place goal (move far away)
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -2
place_pose_stamped.pose.position.y = 0
place_pose_stamped.pose.position.z = 0.85
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(
goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name=det.table_name,
collision_object_name=goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.planning_time = 30
goal.tries = 3
goal.debug_level = 1
goal.object_sampling_fraction = 0.5
#Send the goal to the action
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
#Check the result
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Planning failed. Error code: ' + str(result.error_code))
return
rospy.loginfo('Planning succeeded!')
#at this point the planning is done.
#now we execute and visualize the plan
#visualize trajectory
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
marray = get_trajectory_markers(result.primitive_trajectories)
for i in range(10):
pub.publish(marray)
rospy.sleep(0.05)
#Executor is a Python class for executing DARRT plans
executor = Executor()
#execute trajectory
rospy.loginfo('Press enter to execute')
raw_input()
executor.execute_trajectories(result)
rospy.loginfo('Successfully executed!')
def plan(self):
get_planning_scene_interface().detach_and_add_back_attached_object(
self.arm, self.object_id)
return ErrorCode()
def execute(self, userdata):
self.interface.reset()
psi = get_planning_scene_interface()
psi.reset()
return "success"
def execute(self):
get_planning_scene_interface().reset()
for ex in self.executables:
print "\nStart executing executable " + ex.type
yield ex.execute()
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 main():
wi = WorldInterface()
wi.reset()
psi = get_planning_scene_interface()
psi.reset()
rospy.sleep(2.0)
def set_planning_scene_joint_state(js):
psi = get_planning_scene_interface()
rs = psi.get_robot_state()
set_joint_state_in_robot_state(js, rs)
psi.set_robot_state(rs)
psi.send_diff()
def plan(self):
get_planning_scene_interface().add_ordered_collisions(OrderedCollisionOperations([self]))
return ErrorCode()
def plan(self):
get_planning_scene_interface().set_collisions(None)
return ErrorCode()
def plan(self):
get_planning_scene_interface().add_ordered_collisions(
OrderedCollisionOperations([self]))
return ErrorCode()
def plan(self):
get_planning_scene_interface().attach_object_to_gripper(self.arm, self.object_id)
return ErrorCode()
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 plan(self):
get_planning_scene_interface().detach_and_add_back_attached_object(self.arm, self.object_id)
return ErrorCode()
def plan(self):
get_planning_scene_interface().attach_object_to_gripper(
self.arm, self.object_id)
return ErrorCode()
def plan(self):
get_planning_scene_interface().set_collisions(None)
return ErrorCode()
def main():
rospy.init_node("touring", anonymous=True)
sm = smach.StateMachine(outcomes=["success",
"failure"])
base_mover = base.Base()
detector = TablewareDetection()
pickplace = PickPlace()
pickplace.min_open_gripper_pos = 0.0014
poses = basic_states.poses
world = basic_states.WorldState()
interface = WorldInterface()
tasks = Tasks()
placer = SensingPlacer('right_arm')
interface.reset()
psi = get_planning_scene_interface()
psi.reset()
tasks.move_arms_to_side()
rospy.loginfo("Creating State Machine")
nav_states = {}
for name, pos in poses.iteritems():
nav_states[name] = basic_states.NavigateTo(
world, base_mover, pos[0], pos[1], pos[2])
detect_state = basic_states.Detect(world, detector)
top_pos_place_down = (-2.15, .5, .98)
place_down_top = basic_states.PlaceDown(world, placer,
top_pos_place_down)
T = {"failure":"failure"}
with sm:
T["success"] = "detect"
T["failure"] = "failure"
smach.StateMachine.add("move_shelf", nav_states["shelf"],
transitions=T.copy())
T["success"] = "choose_bowl"
T["failure"] = "failure"
smach.StateMachine.add("detect", detect_state,
transitions=T.copy()
)
T["success"] = "move_to_pickable"
T["failure"] = "failure"
smach.StateMachine.add("choose_bowl",
basic_states.ChooseItem(world, "bowl"),
transitions=T.copy()
)
T["success"] = "pickup"
T["failure"] = "pickup"
smach.StateMachine.add("move_to_pickable",
basic_states.MoveToPickable(world, base_mover ),
transitions=T.copy()
)
T["success"] = "move_arms_to_side"
T["failure"] = "move_arms_to_side_after_pickup"
smach.StateMachine.add("pickup",
basic_states.PickUp(world, pickplace, interface),
transitions=T.copy()
)
T["success"] = "detect"
T["failure"] = "failure"
smach.StateMachine.add("move_arms_to_side_after_pickup",
basic_states.MoveArmsToSide(world, tasks),
transitions=T.copy()
)
T["success"] = "move_top_table"
T["failure"] = "failure"
smach.StateMachine.add("move_arms_to_side",
basic_states.MoveArmsToSide(world, tasks),
transitions=T.copy()
)
T["success"] = "approach_top_table"
T["failure"] = "failure"
smach.StateMachine.add("move_top_table",
nav_states["table_top_edge"],
transitions=T.copy())
T["success"] = "place_down_top"
T["failure"] = "failure"
smach.StateMachine.add("approach_top_table",
basic_states.MoveToReachable(world, base_mover, top_pos_place_down),
transitions=T.copy())
T["success"] = "success"
T["failure"] = "failure"
smach.StateMachine.add("place_down_top",
place_down_top,
transitions=T.copy())
outcome = sm.execute()
rospy.loginfo("Outcome: %s", outcome)
