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 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)
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 _initialize(self):
'''Connect up all services and action clients.
'''
self._world_interface = WorldInterface()
self._controller_manager = ControllerManagerClient()
if self._arm_name in ['right_arm', 'left_arm']:
self._group_name = self._arm_name
self._planner = ArmPlanner(self._arm_name)
self._hand_description = HandDescription(self._arm_name)
arm_abbr = self._arm_name[0]
self._joint_controller = '%s_arm_controller' % arm_abbr
self._cartesian_controller = '%s_cart' % arm_abbr
self._move_arm_client = actionlib.SimpleActionClient(
'move_%s' % self._arm_name,
arm_navigation_msgs.msg.MoveArmAction)
self._wait_for_action_server(self._move_arm_client)
#jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr
#self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction)
#self._wait_for_action_server(self._joint_trajectory_client)
jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr
self._joint_trajectory_client = actionlib.SimpleActionClient(
jt_action_name, FollowJointTrajectoryAction)
self._wait_for_action_server(self._joint_trajectory_client)
self._cart_interface = CartesianControllerInterface(self._arm_name)
elif self._arm_name == 'both':
self._joint_controller = 'two_arm_controller'
#jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action'
#self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction)
jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory'
self._joint_trajectory_client = actionlib.SimpleActionClient(
jt_two_arm_action_name, FollowJointTrajectoryAction)
self._wait_for_action_server(self._joint_trajectory_client)
else:
raise ValueError('Invalid arm name for worker: %s' %
self._arm_name)
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 execute(self):
WorldInterface().detach_and_add_back_attached_object(
self.arm, self.object_id)
return MotionHandleDummy()
def execute(self):
WorldInterface().attach_object_to_gripper(self.arm, self.object_id)
return MotionHandleDummy()
def execute(self, target=PoseStamped(), blocking=True):
ah = ActionHandle("pick_up", "dummy", "", blocking)
rospy.loginfo("Picking up object...")
#ah = self.actions.grasp_object(target, blocking)
wi = WorldInterface()
wi.reset_attached_objects()
wi.reset_collision_objects()
# add table
table_extent = (2.0, 2.0, 1.0)
table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], 'base_link')
wi.add_collision_box(table_pose, table_extent, "table")
mp = MotionPlan()
mp += CallFunction(sss.move, 'torso','front')
#mp += MoveArm('arm',['pregrasp'])
mp += CallFunction(sss.move, 'sdh','cylopen', False)
# OpenIssues:
# - where to place the sdh_grasp_link to grasp the object located at target?
# - there is no orientation in the target? -> hardcoded taken from pregrasp
grasp_pose = PoseStamped()
grasp_pose = deepcopy(target)
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.orientation.x = 0.220
grasp_pose.pose.orientation.y = 0.670
grasp_pose.pose.orientation.z = -0.663
grasp_pose.pose.orientation.w = -0.253
mp += MoveArm('arm',[grasp_pose,['sdh_grasp_link']], seed = 'pregrasp')
mp += CallFunction(sss.move, 'sdh','cylclosed', True)
#ah = self.actions.lift_object(target, blocking)
lift_pose = PoseStamped()
lift_pose = deepcopy(target)
lift_pose.header.stamp = rospy.Time.now()
lift_pose.pose.position.z += 0.08
lift_pose.pose.orientation.x = 0.220
lift_pose.pose.orientation.y = 0.670
lift_pose.pose.orientation.z = -0.663
lift_pose.pose.orientation.w = -0.253
mp += MoveArm('arm',[lift_pose,['sdh_grasp_link']])
#ah = self.actions.retrieve_object(target, blocking)
#mp += MoveArm('arm',['pregrasp'])
mp += MoveArm('arm',['wavein'])
planning_res = mp.plan(2)
print planning_res
if planning_res.success:
for e in mp.execute():
exec_res = e.wait()
print exec_res
if not exec_res.success:
#rospy.logerr("Execution of MotionExecutable %s failed", e.name)
ah.set_failed(4)
break
ah.set_succeeded()
else:
rospy.logerr("Planning failed")
ah.set_failed(4)
return ah
def main():
obj = sys.argv[1]
my_world = WorldInterface()
my_world.attach_object_to_gripper('left_arm', obj)
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!')
#!/usr/bin/env python
import roslib
roslib.load_manifest('cob_arm_navigation_python')
import rospy
from cob_arm_navigation_python.MotionPlan import *
from cob_arm_navigation_python.MoveArm import *
from cob_arm_navigation_python.MoveHand import *
from tf.transformations import *
from copy import deepcopy
rospy.init_node("test")
from pr2_python.world_interface import WorldInterface
wi = WorldInterface()
grasp_pose = PoseStamped()
#grasp_pose.header.frame_id = "base_link"
grasp_pose.header.frame_id = "odom_combined"
grasp_pose.header.stamp = rospy.Time()
grasp_pose.pose = wi.collision_object('milk').poses[0]
p = grasp_pose.pose.position
#p.x, p.y, p.z = [-0.8,-0.2,0.9]
p.x += 0.02
p.y += 0.02
p.z += 0.02
o = grasp_pose.pose.orientation
o.x, o.y, o.z, o.w = quaternion_from_euler(-1.581, -0.019, 2.379)
lift_pose = deepcopy(grasp_pose)
p = lift_pose.pose.position
p.x += 0.0
