def test2_1(self):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "muh"
co.header.frame_id = "/odom_combined"
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions.append(0.3)
cylinder.dimensions.append(0.03)
co.primitives = [cylinder]
co.primitive_poses = [Pose()]
co.primitive_poses[0].position = Point(1.2185, 0, 0)
co.primitive_poses[0].orientation = Quaternion(0, 0, 0, 1)
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions.append(0.1)
box.dimensions.append(0.1)
box.dimensions.append(0.1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[1].position = Point(1.1185, 0, 0)
co.primitive_poses[1].orientation = Quaternion(0, 0, 0, 1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[2].position = Point(0, 0, 0)
co.primitive_poses[2].orientation = Quaternion(0, 0, 0, 1)
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1, 0, 0)
p.pose.orientation = euler_to_quaternion(0, 0, 0)
self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def test1_1(self):
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1, 0, 0)
p.pose.orientation = euler_to_quaternion(0, pi, 0)
p2 = Point(0.7815, 0, 0)
p1 = get_fingertip(p)
self.assertTrue(abs(p1.point.x - p2.x) < 0.0001)
self.assertTrue(abs(p1.point.y - p2.y) < 0.0001)
self.assertTrue(abs(p1.point.z - p2.z) < 0.0001)
def test1_1(self):
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,pi,0)
p2 = Point(0.7815,0,0)
p1 = get_fingertip(p)
self.assertTrue(abs(p1.point.x - p2.x) < 0.0001)
self.assertTrue(abs(p1.point.y - p2.y) < 0.0001)
self.assertTrue(abs(p1.point.z - p2.z) < 0.0001)
def test2_1(self):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "muh"
co.header.frame_id = "/odom_combined"
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions.append(0.3)
cylinder.dimensions.append(0.03)
co.primitives = [cylinder]
co.primitive_poses = [Pose()]
co.primitive_poses[0].position = Point(1.2185, 0,0)
co.primitive_poses[0].orientation = Quaternion(0,0,0,1)
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions.append(0.1)
box.dimensions.append(0.1)
box.dimensions.append(0.1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[1].position = Point(1.1185, 0,0)
co.primitive_poses[1].orientation = Quaternion(0,0,0,1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[2].position = Point(0, 0,0)
co.primitive_poses[2].orientation = Quaternion(0,0,0,1)
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,0,0)
self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def execute(self, userdata):
rospy.loginfo('Executing state GraspObject')
collision_object_name = userdata.object_to_move.mpe_object.id
rospy.loginfo('Trying to grasp %s' % collision_object_name)
if userdata.enable_movement:
move_to_func = utils.manipulation.move_arm_and_base_to
plan_to_func = utils.manipulation.plan_arm_and_base_to
else:
move_to_func = utils.manipulation.move_to
plan_to_func = utils.manipulation.plan_arm_to
#get the collisionobject out of the planningscene
collision_object = utils.manipulation.get_planning_scene().get_collision_object(collision_object_name)
if collision_object is None:
rospy.logwarn("Collision Object " + collision_object_name + " is not in planningscene.")
return 'objectNotInPlanningscene'
rospy.logdebug("Grasping: " + str(collision_object))
grasp_positions = calculate_grasp_position(collision_object, utils.manipulation.transform_to)
#filter out some invalid grasps
grasp_positions = utils.manipulation.filter_low_poses(grasp_positions)
if not userdata.enable_movement:
grasp_positions = utils.manipulation.filter_close_poses(grasp_positions)
if len(grasp_positions) == 0:
rospy.logwarn("No grasppositions found for " + collision_object_name)
userdata.failed_object = userdata.object_to_move
return 'noGraspPosition'
#sort to try the best grasps first
grasp_positions.sort(cmp=lambda x, y: utils.manipulation.cmp_pose_stamped(collision_object, x, y))
visualize_poses(grasp_positions)
utils.manipulation.open_gripper()
grasp_positions = [utils.manipulation.transform_to(grasp) for grasp in grasp_positions]
utils.manipulation.blow_up_objects(do_not_blow_up_list=(collision_object_name))
for grasp in grasp_positions:
plan_pre_grasp = plan_to_func(get_pre_grasp(grasp))
if plan_pre_grasp is None:
continue
rospy.logdebug("Plan to pregraspposition found")
utils.manipulation.blow_up_objects(do_not_blow_up_list=("map", collision_object_name))
plan_to_grasp = plan_to_func(grasp, start_state=utils.manipulation.get_end_state(plan_pre_grasp))
if plan_to_grasp is None or not utils.manipulation.move_with_plan_to(plan_pre_grasp):
rospy.logdebug("Failed to move to Graspposition")
continue
rospy.sleep(0.5)
if not utils.manipulation.move_with_plan_to(plan_to_grasp):
rospy.logdebug("Failed to move to Graspposition")
continue
rospy.logdebug("Graspposition taken")
time.sleep(0.5)
rospy.sleep(1)
if not utils.manipulation.close_gripper(collision_object, get_fingertip(utils.manipulation.transform_to(grasp))):
# userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
time.sleep(0.5)
rospy.sleep(1.5)
#calculate the center of mass and weight of the object and call the load object service
com = utils.manipulation.get_center_of_mass(collision_object)
com = utils.manipulation.transform_to(com, "/tcp")
if com is None:
rospy.logwarn("TF failed")
# userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
density = 1
for obj in userdata.yaml.objects:
if obj.name == collision_object_name:
density = obj.primitive_densities[0]
if isinf(density):
rospy.logerr("Infinite density! WTF. setting density to 7850")
density = 7850
utils.manipulation.load_object(utils.manipulation.calc_object_weight(collision_object, density),
Vector3(com.point.x, com.point.y, com.point.z))
rospy.loginfo("grasped " + collision_object_name)
#save grasp data for placing
userdata.grasp = grasp
fingertip = get_fingertip(grasp)
fingertip_to_tcp = subtract_point(grasp.pose.position, fingertip.point)
userdata.dist_to_obj = magnitude(fingertip_to_tcp)
rospy.logdebug("lift object")
the_pre_grasp = get_pre_grasp(grasp)
the_move_to_func = move_to_func(the_pre_grasp, do_not_blow_up_list=collision_object_name)
if not the_move_to_func:
rospy.logwarn("couldnt lift object. continue anyway")
userdata.failed_object = None
return 'success'
rospy.logwarn("Grapsing failed.")
userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state GraspObject')
collision_object_name = userdata.object_to_move.mpe_object.id
rospy.loginfo('Trying to grasp %s' % collision_object_name)
if userdata.enable_movement:
move_to_func = utils.manipulation.move_arm_and_base_to
plan_to_func = utils.manipulation.plan_arm_and_base_to
else:
move_to_func = utils.manipulation.move_to
plan_to_func = utils.manipulation.plan_arm_to
#get the collisionobject out of the planningscene
collision_object = utils.manipulation.get_planning_scene(
).get_collision_object(collision_object_name)
if collision_object is None:
rospy.logwarn("Collision Object " + collision_object_name +
" is not in planningscene.")
return 'objectNotInPlanningscene'
rospy.logdebug("Grasping: " + str(collision_object))
grasp_positions = calculate_grasp_position(
collision_object, utils.manipulation.transform_to)
#filter out some invalid grasps
grasp_positions = utils.manipulation.filter_low_poses(grasp_positions)
if not userdata.enable_movement:
grasp_positions = utils.manipulation.filter_close_poses(
grasp_positions)
if len(grasp_positions) == 0:
rospy.logwarn("No grasppositions found for " +
collision_object_name)
userdata.failed_object = userdata.object_to_move
return 'noGraspPosition'
#sort to try the best grasps first
grasp_positions.sort(cmp=lambda x, y: utils.manipulation.
cmp_pose_stamped(collision_object, x, y))
visualize_poses(grasp_positions)
utils.manipulation.open_gripper()
grasp_positions = [
utils.manipulation.transform_to(grasp) for grasp in grasp_positions
]
utils.manipulation.blow_up_objects(
do_not_blow_up_list=(collision_object_name))
for grasp in grasp_positions:
plan_pre_grasp = plan_to_func(get_pre_grasp(grasp))
if plan_pre_grasp is None:
continue
rospy.logdebug("Plan to pregraspposition found")
utils.manipulation.blow_up_objects(
do_not_blow_up_list=("map", collision_object_name))
plan_to_grasp = plan_to_func(
grasp,
start_state=utils.manipulation.get_end_state(plan_pre_grasp))
if plan_to_grasp is None or not utils.manipulation.move_with_plan_to(
plan_pre_grasp):
rospy.logdebug("Failed to move to Graspposition")
continue
rospy.sleep(0.5)
if not utils.manipulation.move_with_plan_to(plan_to_grasp):
rospy.logdebug("Failed to move to Graspposition")
continue
rospy.logdebug("Graspposition taken")
time.sleep(0.5)
rospy.sleep(1)
if not utils.manipulation.close_gripper(
collision_object,
get_fingertip(utils.manipulation.transform_to(grasp))):
# userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
time.sleep(0.5)
rospy.sleep(1.5)
#calculate the center of mass and weight of the object and call the load object service
com = utils.manipulation.get_center_of_mass(collision_object)
com = utils.manipulation.transform_to(com, "/tcp")
if com is None:
rospy.logwarn("TF failed")
# userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'
density = 1
for obj in userdata.yaml.objects:
if obj.name == collision_object_name:
density = obj.primitive_densities[0]
if isinf(density):
rospy.logerr(
"Infinite density! WTF. setting density to 7850")
density = 7850
utils.manipulation.load_object(
utils.manipulation.calc_object_weight(collision_object,
density),
Vector3(com.point.x, com.point.y, com.point.z))
rospy.loginfo("grasped " + collision_object_name)
#save grasp data for placing
userdata.grasp = grasp
fingertip = get_fingertip(grasp)
fingertip_to_tcp = subtract_point(grasp.pose.position,
fingertip.point)
userdata.dist_to_obj = magnitude(fingertip_to_tcp)
rospy.logdebug("lift object")
the_pre_grasp = get_pre_grasp(grasp)
the_move_to_func = move_to_func(
the_pre_grasp, do_not_blow_up_list=collision_object_name)
if not the_move_to_func:
rospy.logwarn("couldnt lift object. continue anyway")
userdata.failed_object = None
return 'success'
rospy.logwarn("Grapsing failed.")
userdata.failed_object = userdata.object_to_move
utils.manipulation.blow_down_objects()
return 'fail'