(geom: hppfcl.ShapeBase)
| 171 | |
| 172 | |
| 173 | def load_primitive(geom: hppfcl.ShapeBase): |
| 174 | import meshcat.geometry as mg |
| 175 | |
| 176 | # Cylinders need to be rotated |
| 177 | basic_three_js_transform = np.array( |
| 178 | [ |
| 179 | [1.0, 0.0, 0.0, 0.0], |
| 180 | [0.0, 0.0, -1.0, 0.0], |
| 181 | [0.0, 1.0, 0.0, 0.0], |
| 182 | [0.0, 0.0, 0.0, 1.0], |
| 183 | ] |
| 184 | ) |
| 185 | RotatedCylinder = type( |
| 186 | "RotatedCylinder", |
| 187 | (mg.Cylinder,), |
| 188 | {"intrinsic_transform": lambda self: basic_three_js_transform}, |
| 189 | ) |
| 190 | |
| 191 | # Cones need to be rotated |
| 192 | |
| 193 | if isinstance(geom, hppfcl.Capsule): |
| 194 | if hasattr(mg, "TriangularMeshGeometry"): |
| 195 | obj = createCapsule(2.0 * geom.halfLength, geom.radius) |
| 196 | else: |
| 197 | obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius) |
| 198 | elif isinstance(geom, hppfcl.Cylinder): |
| 199 | obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius) |
| 200 | elif isinstance(geom, hppfcl.Cone): |
| 201 | obj = RotatedCylinder(2.0 * geom.halfLength, 0, geom.radius, 0) |
| 202 | elif isinstance(geom, hppfcl.Box): |
| 203 | obj = mg.Box(npToTuple(2.0 * geom.halfSide)) |
| 204 | elif isinstance(geom, hppfcl.Sphere): |
| 205 | obj = mg.Sphere(geom.radius) |
| 206 | elif isinstance(geom, hppfcl.TriangleP): |
| 207 | faces = np.empty((1, 3), dtype=int) |
| 208 | vertices = np.empty((3, 3)) |
| 209 | vertices[0] = geom.a |
| 210 | vertices[1] = geom.b |
| 211 | vertices[2] = geom.c |
| 212 | faces[0] = [0, 1, 2] |
| 213 | obj = mg.TriangularMeshGeometry(vertices, faces) |
| 214 | elif isinstance(geom, hppfcl.Ellipsoid): |
| 215 | obj = mg.Ellipsoid(geom.radii) |
| 216 | elif isinstance(geom, (hppfcl.Plane, hppfcl.Halfspace)): |
| 217 | plane_transform: pin.SE3 = pin.SE3.Identity() |
| 218 | # plane_transform.translation[:] = geom.d # Does not work |
| 219 | plane_transform.rotation = pin.Quaternion.FromTwoVectors( |
| 220 | geom.n, pin.ZAxis |
| 221 | ).toRotationMatrix() |
| 222 | TransformedPlane = type( |
| 223 | "TransformedPlane", |
| 224 | (Plane,), |
| 225 | {"intrinsic_transform": lambda self: plane_transform.homogeneous}, |
| 226 | ) |
| 227 | obj = TransformedPlane(10, 10) |
| 228 | elif isinstance(geom, hppfcl.ConvexBase): |
| 229 | obj = loadMesh(geom) |
| 230 | else: |
no test coverage detected