def main(): obj_type = sys.argv[1] my_world = WorldInterface() # print "quat="+str(gt.euler_to_quaternion(math.pi/2,math.pi/2,0)) if obj_type == "plate" or obj_type == "paper_plate" or obj_type == "plastic_plate": obj = create_plate(my_world) elif obj_type == "box": obj = create_box(my_world) my_world.attach_object_to_gripper('left_arm', obj)
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)
def main(): obj = sys.argv[1] my_world = WorldInterface() my_world.attach_object_to_gripper('left_arm', obj)