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 calc_object_volume(object):
"""
Calculates the Volume of an object
:param object: object
:type: CollisionObject / Object
:return: volume
:type: float
"""
volume = 0
for i in xrange(len(object.primitives)):
if object.primitives[i].type == SolidPrimitive().BOX:
x = object.primitives[i].dimensions[SolidPrimitive.BOX_X]
y = object.primitives[i].dimensions[SolidPrimitive.BOX_Y]
z = object.primitives[i].dimensions[SolidPrimitive.BOX_Z]
volume += x * y * z
elif object.primitives[i].type == SolidPrimitive().CYLINDER:
r = object.primitives[i].dimensions[SolidPrimitive.CYLINDER_RADIUS]
h = object.primitives[i].dimensions[SolidPrimitive.CYLINDER_HEIGHT]
volume += pi * r * r * h
return volume
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
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)
