def __init__(self,
header=None,
orientation=None,
link_name=None,
absolute_x_axis_tolerance=0.0,
absolute_y_axis_tolerance=0.0,
absolute_z_axis_tolerance=0.0,
weight=1):
"""
Notes
-----
The naming of the absolute_x/y/z_axis_tolerances might be misleading:
If you specify the absolute_x/y/z_axis_tolerances with [0.01, 0.01, 6.3],
it means that the frame's x-axis and y-axis are allowed to rotate about
the z-axis by an angle of 6.3 radians, whereas the z-axis can only change
by 0.01.
"""
self.header = header or Header()
self.orientation = orientation or Quaternion(
) # geometry_msgs/Quaternion
self.link_name = link_name or ""
self.absolute_x_axis_tolerance = float(absolute_x_axis_tolerance)
self.absolute_y_axis_tolerance = float(absolute_y_axis_tolerance)
self.absolute_z_axis_tolerance = float(absolute_z_axis_tolerance)
self.weight = float(weight)
def from_sphere(cls, sphere):
"""Creates a `BoundingVolume` from a :class:`compas.geometry.Sphere`.
Parameters
----------
sphere: `compas.geometry.Sphere`
"""
primitive = SolidPrimitive.from_sphere(sphere)
pose = Pose(Point(*sphere.point), Quaternion(0, 0, 0, 1))
return cls(primitives=[primitive], primitive_poses=[pose])
def from_orientation_constraint(cls, header, orientation_constraint):
"""Creates a ``OrientationConstraint`` from a :class:`compas_fab.robots.OrientationConstraint`.
"""
qw, qx, qy, qz = orientation_constraint.quaternion
ax, ay, az = orientation_constraint.tolerances
kwargs = {}
kwargs['header'] = header
kwargs['orientation'] = Quaternion(qx, qy, qz, qw)
kwargs['link_name'] = orientation_constraint.link_name
kwargs['absolute_x_axis_tolerance'] = ax
kwargs['absolute_y_axis_tolerance'] = ay
kwargs['absolute_z_axis_tolerance'] = az
kwargs['weight'] = orientation_constraint.weight
return cls(**kwargs)
def to_collision_meshes(self):
"""Creates a list of collision meshes from a :class:`compas_fab.backends.CollisionObject`
"""
collision_meshes = []
for mesh, pose in zip(self.meshes, self.mesh_poses):
pose = pose if isinstance(pose, Pose) else Pose(**pose)
pose.position = pose.position if isinstance(
pose.position, Point) else Point(**pose.position)
pose.position.x = float(pose.position.x)
pose.position.y = float(pose.position.y)
pose.position.z = float(pose.position.z)
pose.orientation = pose.orientation if isinstance(
pose.orientation, Quaternion) else Quaternion(
**pose.orientation)
pose.orientation.x = float(pose.orientation.x)
pose.orientation.y = float(pose.orientation.y)
pose.orientation.z = float(pose.orientation.z)
pose.orientation.w = float(pose.orientation.w)
mesh = mesh if isinstance(mesh, Mesh) else Mesh(**mesh)
mesh.triangles = [
t if isinstance(t, MeshTriangle) else MeshTriangle(**t)
for t in mesh.triangles
]
for triangle in mesh.triangles:
triangle.vertex_indices = [
int(x) for x in triangle.vertex_indices
]
mesh.vertices = [
v if isinstance(v, Point) else Point(**v)
for v in mesh.vertices
]
for vertex in mesh.vertices:
vertex.x = float(vertex.x)
vertex.y = float(vertex.y)
vertex.z = float(vertex.z)
root_name = getattr(self.header, 'frame_id',
None) or self.header['frame_id']
cm = CollisionMesh(mesh.mesh, self.id, pose.frame, root_name)
collision_meshes.append(cm)
return collision_meshes