class Executor: 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 execute_trajectories(self, result): rospy.loginfo('There are ' + str(len(result.primitive_names)) + ' segments') for (i, t) in enumerate(result.primitive_types): rospy.loginfo('Executing trajectory ' + str(i) + ' of type ' + t + ' and length ' + str( len(result.primitive_trajectories[i]. joint_trajectory.points))) #print 'Trajectory is\n', result.primitive_trajectories[i] splits = t.split('-') if splits[0] == 'BaseTransit': self.execute_base_trajectory(result.primitive_trajectories[i]) continue if splits[0] == 'BaseWarp': self.arm_tasks.move_arm_to_side('right_arm') self.arm_tasks.move_arm_to_side('left_arm') self.execute_warp_trajectory(result.primitive_trajectories[i]) continue if splits[0] == 'ArmTransit': self.execute_arm_trajectory(splits[1], result.primitive_trajectories[i]) continue #if splits[0] == 'Approach': #gripper.open() if splits[0] == 'Pickup': try: self.grippers[splits[1]].close() except ActionFailedError: pass self.wi.attach_object_to_gripper(splits[1], splits[2]) #so the trajectory filter doesn't complain ops = OrderedCollisionOperations() op = CollisionOperation() op.operation = op.DISABLE op.object2 = splits[2] for t in self.wi.hands[splits[1]].touch_links: op.object1 = t ops.collision_operations.append(copy.deepcopy(op)) self.psi.add_ordered_collisions(ops) self.execute_straight_line_arm_trajectory( splits[1], result.primitive_trajectories[i]) if splits[0] == 'Place': self.grippers[splits[1]].open() self.wi.detach_and_add_back_attached_object( splits[1], splits[2]) self.psi.reset() def execute_warp_trajectory(self, trajectory): #figure out the last point on the trajectory tp = trajectory.multi_dof_joint_trajectory.points[-1] (phi, theta, psi) = gt.quaternion_to_euler(tp.poses[0].orientation) self.base.move_to(tp.poses[0].position.x, tp.poses[0].position.y, psi) def execute_base_trajectory(self, trajectory): goal = BaseTrajectoryGoal() #be a little slower and more precise goal.linear_velocity = 0.2 goal.angular_velocity = np.pi / 8.0 goal.linear_error = 0.01 goal.angular_error = 0.01 joint = -1 for (i, n) in enumerate( trajectory.multi_dof_joint_trajectory.joint_names): if n == 'world_joint' or n == '/world_joint': goal.world_frame = trajectory.multi_dof_joint_trajectory.frame_ids[ i] goal.robot_frame = trajectory.multi_dof_joint_trajectory.child_frame_ids[ i] joint = i break if joint < 0: raise ActionFailedError('No world joint found in base trajectory') for p in trajectory.multi_dof_joint_trajectory.points: (phi, theta, psi) = gt.quaternion_to_euler(p.poses[joint].orientation) goal.trajectory.append( Pose2D(x=p.poses[joint].position.x, y=p.poses[joint].position.y, theta=psi)) self.base_action.send_goal_and_wait(goal) def execute_arm_trajectory(self, arm_name, trajectory): arm_trajectory = self.arm_planners[arm_name].joint_trajectory( trajectory.joint_trajectory) arm_trajectory.header.stamp = rospy.Time.now() #try: # arm_trajectory = self.arm_planners[arm_name].filter_trajectory(arm_trajectory) #except ArmNavError, e: #rospy.logwarn('Trajectory filter failed with error '+str(e)+'. Executing anyway') arm_trajectory = tt.convert_path_to_trajectory(arm_trajectory, time_per_pt=0.35) self.arm_mover.execute_joint_trajectory(arm_name, arm_trajectory) def execute_straight_line_arm_trajectory(self, arm_name, trajectory): arm_trajectory = self.arm_planners[arm_name].joint_trajectory( trajectory.joint_trajectory) arm_trajectory.header.stamp = rospy.Time.now() arm_trajectory = tt.convert_path_to_trajectory(arm_trajectory, time_per_pt=0.4) self.arm_mover.execute_joint_trajectory(arm_name, arm_trajectory)
class Planner: 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) #self.base = Base() def publish(self, markers): for i in range(50): self.viz_pub.publish(markers) rospy.sleep(0.01) def plan_base(self, goal): #find the current base pose from the planning scene robot_state = self.psi.get_robot_state() starting_state = PoseStamped() starting_state.header.frame_id = robot_state.multi_dof_joint_state.frame_ids[ 0] starting_state.pose = robot_state.multi_dof_joint_state.poses[0] goal_robot_state = copy.deepcopy(robot_state) goal_robot_state.multi_dof_joint_state.poses[0] = goal.pose start_markers = vt.robot_marker(robot_state, ns='base_starting_state') goal_markers = vt.robot_marker(goal_robot_state, ns='base_goal_state', g=1, b=0) self.publish(start_markers) self.publish(goal_markers) #plan good_plan = False while not good_plan: try: start_time = time.time() self.base_plan_service(start=starting_state, goal=goal) end_time = time.time() good_plan = True except: rospy.loginfo( 'Having trouble probably with starting state. Sleeping and trying again' ) rospy.sleep(0.2) #it's apparently better just to do the trajectory #than fool with the planning scene goal_cov = PoseWithCovarianceStamped() goal_cov.header = goal.header goal_cov.pose.pose = goal.pose self.pose_pub.publish(goal_cov) self.psi.reset() # self.base.move_to(goal.pose.position.x, goal.pose.position.y, # 2.0*np.arcsin(goal.pose.orientation.z)) return (end_time - start_time) def plan_arm(self, goal, ordered_colls=None, interpolated=False): time = 0 robot_state = self.psi.get_robot_state() ending_robot_state = copy.deepcopy(robot_state) start_markers = vt.robot_marker(robot_state, ns='arm_starting_state') self.publish(start_markers) marray = MarkerArray() marker = vt.marker_at(goal, ns="arm_goal_pose") marray.markers.append(marker) self.publish(marray) if not interpolated: traj, time = self.arm_planner.plan_collision_free( goal, ordered_collisions=ordered_colls, runtime=True) else: goal_bl = self.psi.transform_pose_stamped('base_link', goal) marray = MarkerArray() marker = vt.marker_at(goal_bl, ns="arm_goal_pose_base_link") marray.markers.append(marker) self.publish(marray) traj, time = self.arm_planner.plan_interpolated_ik( goal_bl, ordered_collisions=ordered_colls, consistent_angle=2.0 * np.pi, runtime=True) joint_state = tt.joint_trajectory_point_to_joint_state( traj.points[-1], traj.joint_names) ending_robot_state = tt.set_joint_state_in_robot_state( joint_state, ending_robot_state) goal_markers = vt.robot_marker(ending_robot_state, ns='arm_goal_state', g=1, b=0) self.publish(goal_markers) #do the trajectory # traj.points = [traj.points[0], traj.points[1], traj.points[-1]] # traj.header.stamp = rospy.Time.now() # traj.points[-1].time_from_start = 3 #rospy.loginfo('Trajectory =\n'+str(traj)) self.arm_mover.execute_joint_trajectory('right_arm', traj) #self.psi.set_robot_state(ending_robot_state) return time def plan_arm_interpolated(self, goal, ordered_colls=None): return self.plan_arm(goal, ordered_colls=ordered_colls, interpolated=True)