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)
