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()
