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 close_gripper(self, object=None, grasp_point=None): """ Closes the gripper completely or far enough to hold the object, when one is given :param object: Object that will be grasped. :type: CollisionObject :return: success of the movement :type: bool """ rospy.logdebug("Closing Gripper") if type(object) is CollisionObject: self.__gripper_group.attach_object(object.id, "gp", ["gp", "finger1", "finger2"]) rospy.sleep(1.0) id = get_grasped_part(object, grasp_point)[1] # id = min(range(len(object.primitives)), key=lambda i: min(object.primitives[i].dimensions)) # TODO: only works for cubes and cylinders and only "sometimes" for object compositions if object.primitives[id].type == shape_msgs.msg.SolidPrimitive.BOX: length = min(object.primitives[id].dimensions) self.__gripper_group.set_joint_value_target( [-(length / 2), length / 2]) elif object.primitives[ id].type == shape_msgs.msg.SolidPrimitive.CYLINDER: radius = object.primitives[id].dimensions[ shape_msgs.msg.SolidPrimitive.CYLINDER_RADIUS] if radius >= gripper_max_pose: rospy.logdebug("Object is too big!") return False self.__gripper_group.set_joint_value_target( [-radius + 0.005, radius - 0.005]) else: self.__gripper_group.set_joint_value_target([0.0, 0.0]) path = self.__gripper_group.plan() return self.__manService.move(path)
def close_gripper(self, object=None, grasp_point=None): """ Closes the gripper completely or far enough to hold the object, when one is given :param object: Object that will be grasped. :type: CollisionObject :return: success of the movement :type: bool """ rospy.logdebug("Closing Gripper") if type(object) is CollisionObject: self.__gripper_group.attach_object(object.id, "gp", ["gp", "finger1", "finger2"]) rospy.sleep(1.0) id = get_grasped_part(object, grasp_point)[1] # id = min(range(len(object.primitives)), key=lambda i: min(object.primitives[i].dimensions)) # TODO: only works for cubes and cylinders and only "sometimes" for object compositions if object.primitives[id].type == shape_msgs.msg.SolidPrimitive.BOX: length = min(object.primitives[id].dimensions) self.__gripper_group.set_joint_value_target([-(length / 2), length / 2]) elif object.primitives[id].type == shape_msgs.msg.SolidPrimitive.CYLINDER: radius = object.primitives[id].dimensions[shape_msgs.msg.SolidPrimitive.CYLINDER_RADIUS] if radius >= gripper_max_pose: rospy.logdebug("Object is too big!") return False self.__gripper_group.set_joint_value_target([-radius + 0.005, radius - 0.005]) else: self.__gripper_group.set_joint_value_target([0.0, 0.0]) path = self.__gripper_group.plan() return self.__manService.move(path)
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)