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 to_collision_object(self): """ :return: the map as a collision object :type: CollisionObject """ co = CollisionObject() co.header.frame_id = "/odom_combined" co.id = "map" primitive = SolidPrimitive() primitive.type = SolidPrimitive.BOX primitive.dimensions.append(self.cell_size) primitive.dimensions.append(self.cell_size) primitive.dimensions.append(2) for x in range(0, len(self.field)): for y in range(0, len(self.field[x])): if self.field[x][y].is_free() or self.field[x][y].is_object(): continue if self.field[x][y].is_obstacle(): primitive.dimensions[primitive.BOX_Z] = self.get_cell_by_index(x, y).highest_z * 2 else: primitive.dimensions[primitive.BOX_Z] = self.get_average_z_of_surrounded_obstacles(x, y) * 2 primitive.dimensions[primitive.BOX_Z] += 0.02 co.primitives.append(deepcopy(primitive)) pose = Pose() (pose.position.x, pose.position.y) = self.index_to_coordinates(x, y) pose.orientation.w = 1 co.primitive_poses.append(pose) return co
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 __make_position_goal(self, eef_link, goal): """ Helper method to create a pose goal out of a posestamped. :param eef_link: name of the eef link :type: str :param goal: eef goal position :type: PoseStamped :return: :type: PositionConstraint """ position_tolerance = 0.00001 orientation_tolerance = 0.001 position_goal = PositionConstraint() position_goal.header = goal.header position_goal.link_name = eef_link position_goal.target_point_offset = Vector3() primitive = SolidPrimitive() primitive.type = SolidPrimitive.SPHERE primitive.dimensions.append(position_tolerance) position_goal.constraint_region.primitives.append(primitive) p = Pose() p.position.x = goal.pose.position.x p.position.y = goal.pose.position.y p.position.z = goal.pose.position.z p.orientation.w = 1 position_goal.constraint_region.primitive_poses.append(p) position_goal.weight = 1.0 orientation_goal = OrientationConstraint() orientation_goal.header = goal.header orientation_goal.link_name = eef_link orientation_goal.absolute_x_axis_tolerance = orientation_tolerance orientation_goal.absolute_y_axis_tolerance = orientation_tolerance orientation_goal.absolute_z_axis_tolerance = orientation_tolerance orientation_goal.orientation = goal.pose.orientation orientation_goal.weight = 1.0 return (position_goal, orientation_goal)
def __make_position_goal(self, eef_link, goal): """ Helper method to create a pose goal out of a posestamped. :param eef_link: name of the eef link :type: str :param goal: eef goal position :type: PoseStamped :return: :type: PositionConstraint """ position_tolerance = 0.00001 orientation_tolerance = 0.001 position_goal = PositionConstraint() position_goal.header = goal.header position_goal.link_name = eef_link position_goal.target_point_offset = Vector3() primitive = SolidPrimitive() primitive.type = SolidPrimitive.SPHERE primitive.dimensions.append(position_tolerance) position_goal.constraint_region.primitives.append(primitive) p = Pose() p.position.x = goal.pose.position.x p.position.y = goal.pose.position.y p.position.z = goal.pose.position.z p.orientation.w = 1 position_goal.constraint_region.primitive_poses.append(p) position_goal.weight = 1.0 orientation_goal = OrientationConstraint() orientation_goal.header = goal.header orientation_goal.link_name = eef_link orientation_goal.absolute_x_axis_tolerance = orientation_tolerance orientation_goal.absolute_y_axis_tolerance = orientation_tolerance orientation_goal.absolute_z_axis_tolerance = orientation_tolerance orientation_goal.orientation = goal.pose.orientation orientation_goal.weight = 1.0 return (position_goal, orientation_goal)
def make_box(name, pose, size): """ Creates a box collision object. :param name: name of the box :type: str :param pose: position of the box :type: PoseStamped :param size: box size :type: [float(x), float(y), float(z)] :return: box collision object :type: CollisionObject """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives = [box] co.primitive_poses = [pose.pose] return co
def make_cylinder(name, pose, size): """ Creates a cylinder collision object. :param name: name of the cylinder :type: str :param pose: position of the cylinder :type: PoseStamped :param size: cylinder size :type: [float(height), float(radius)] :return: cylinder collisionobject :type: CollisionObject """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = list(size) co.primitives = [cylinder] co.primitive_poses = [pose.pose] return co
def make_cylinder(name, pose, size): """ Creates a cylinder collision object. :param name: name of the cylinder :type: str :param pose: position of the cylinder :type: PoseStamped :param size: cylinder size :type: [float(height), float(radius)] :return: cylinder collisionobject :type: CollisionObject """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = list(size) co.primitives = [cylinder] co.primitive_poses = [pose.pose] return co
def make_box(name, pose, size): """ Creates a box collision object. :param name: name of the box :type: str :param pose: position of the box :type: PoseStamped :param size: box size :type: [float(x), float(y), float(z)] :return: box collision object :type: CollisionObject """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives = [box] co.primitive_poses = [pose.pose] return co
def to_collision_object(self): """ :return: the map as a collision object :type: CollisionObject """ co = CollisionObject() co.header.frame_id = "/odom_combined" co.id = "map" primitive = SolidPrimitive() primitive.type = SolidPrimitive.BOX primitive.dimensions.append(self.cell_size) primitive.dimensions.append(self.cell_size) primitive.dimensions.append(2) for x in range(0, len(self.field)): for y in range(0, len(self.field[x])): if self.field[x][y].is_free() or self.field[x][y].is_object(): continue if self.field[x][y].is_obstacle(): primitive.dimensions[ primitive.BOX_Z] = self.get_cell_by_index( x, y).highest_z * 2 else: primitive.dimensions[ primitive. BOX_Z] = self.get_average_z_of_surrounded_obstacles( x, y) * 2 primitive.dimensions[primitive.BOX_Z] += 0.02 co.primitives.append(deepcopy(primitive)) pose = Pose() (pose.position.x, pose.position.y) = self.index_to_coordinates(x, y) pose.orientation.w = 1 co.primitive_poses.append(pose) return co