def main(): pub = rospy.Publisher('/fail_location', Marker) pickplace = PickPlace(search_place=[]) place_pose = PoseStamped() place_pose.header.frame_id = '/torso_lift_link' place_pose.pose.position.x = 0.7 place_pose.pose.position.y = -0.2 place_pose.pose.position.z = -0.28 place_pose.pose.orientation.x = 0.707 place_pose.pose.orientation.w = 0.707 obj = sys.argv[1] try: place_goal = PlaceGoal('right_arm', [place_pose], \ collision_support_surface_name='table', \ collision_object_name=obj) pickplace.place(place_goal) except ManipulationError: pass # my_world = WorldInterface() # fail_loc = my_world.collision_object(obj) # rospy.loginfo("FAIL LOC="+str(fail_loc)) marker = Marker() marker.header.frame_id = '/torso_lift_link' marker.ns = '' marker.id = 0 marker.type = Marker.SPHERE marker.action = marker.ADD marker.header.frame_id = place_pose.header.frame_id marker.pose = place_pose.pose marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.color.a = 0.8 rospy.loginfo("marker msg=" + str(marker)) pub.publish(marker) rospy.sleep(15.0)
def __init__(self, goal, mass, prims): ''' @type goal: PoseStameped @param goal: goal pose of the object @type mass: float @param mass: mass of the object @type prims: list @param prims: list of possible motion primitives for the given object ''' self.goal = goal self.mass = mass self.prims = prims self.my_world = world_interface.WorldInterface() self.right_arm = arm_planner.ArmPlanner('right_arm') self.left_arm = arm_planner.ArmPlanner('left_arm') self.pickplace = PickPlace(search_place=[], place_on_table=False)
def main(goal, mass): rospy.init_node("test_placing") arm = arm_planner.ArmPlanner('right_arm') pickplace = PickPlace(search_place=[]) obj_name = sys.argv[1] # shape = Shape() # shape.type = 1 # BOX # shape.dimensions = [5,5,10] # orien = Orientation(0.0, 0.0, 0.0) # obj = Object(shape, mass, orien) my_world = world_interface.WorldInterface() objs = my_world.attached_collision_objects() # objs = [toShape.get_obj()] rospy.loginfo("collision objects="+str(len(objs))) for obj in objs: rospy.loginfo("id="+str(obj.object.id)) rospy.loginfo("name="+obj_name) print "id="+str(obj.object.id)+" name"+str(obj_name) if obj.object.id == obj_name: for i in range(len(obj.object.shapes)): orien = quat_to_orien(obj.object.poses[i].orientation) my_obj = Object(obj.object.shapes[i], mass, orien) #marker(my_obj.mesh_centroid()) placements = copy.deepcopy(my_obj.placements) placements.reverse() (height, orientation) = placements.pop() print "height="+str(height)+" orien="+str(orientation) pose = make_pose(goal, height, orientation) # while not good(pose): while not acceptable(arm, pose): # while not acceptable(pickplace, obj_name, pose): (height, orientation) = placements.pop() print "height="+str(height)+" orien="+str(orientation) pose = make_pose(goal, height, orientation) print "pose= "+str(pose) place(pickplace, obj_name, pose)
def __init__(self, goal, modes): ''' @type goal: PoseStameped @param goal: goal pose of the object @type modes: list @param modes: list of priors for each of the modes [tipping, sliding] ''' self.goal = goal self.modes = modes self.my_world = world_interface.WorldInterface() self.right_arm = arm_planner.ArmPlanner('right_arm') self.left_arm = arm_planner.ArmPlanner('left_arm') self.my_arm_mover = arm_mover.ArmMover() self.pickplace = PickPlace(search_place=[], place_on_table=False) self._get_model_mesh_srv = rospy.ServiceProxy( '/objects_database_node/get_model_mesh', GetModelMesh) rospy.loginfo('Waiting for model mesh service') self._get_model_mesh_srv.wait_for_service()
def __init__(self, goal, prims): ''' @type goal: PoseStameped @param goal: goal pose of the object @type prims: list @param prims: list of possible motion primitives for the given object ''' self.goal = goal self.prims = prims self.my_world = world_interface.WorldInterface() self.right_arm = arm_planner.ArmPlanner('right_arm') self.left_arm = arm_planner.ArmPlanner('left_arm') self.my_arm_mover = arm_mover.ArmMover() self.pickplace = PickPlace(search_place=[], place_on_table=False) self._get_model_mesh_srv = rospy.ServiceProxy( '/objects_database_node/get_model_mesh', GetModelMesh) rospy.loginfo('Waiting for model mesh service') self._get_model_mesh_srv.wait_for_service()