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 __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 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)