(
viz: MeshcatVisualizer,
pt1: np.ndarray,
pt2: np.ndarray,
line_name: str,
linewidth: float = 0.01,
color: np.ndarray = BLACK,
)
| 62 | |
| 63 | |
| 64 | def register_line( |
| 65 | viz: MeshcatVisualizer, |
| 66 | pt1: np.ndarray, |
| 67 | pt2: np.ndarray, |
| 68 | line_name: str, |
| 69 | linewidth: float = 0.01, |
| 70 | color: np.ndarray = BLACK, |
| 71 | ) -> int: |
| 72 | height = np.linalg.norm(pt2 - pt1) |
| 73 | if height > 1e-6: |
| 74 | axis_ref = np.array([0.0, 0.0, 1.0]) |
| 75 | axis = (pt2 - pt1) / height # - np.array([0., 0., 1.]) |
| 76 | num = np.outer(axis_ref + axis, axis_ref + axis) |
| 77 | den = np.dot(axis_ref + axis, axis_ref + axis) |
| 78 | if den > 1e-6: |
| 79 | R = 2 * num / den - np.eye(3) |
| 80 | else: |
| 81 | R = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) |
| 82 | t = pt1 |
| 83 | translation = pin.SE3(np.eye(3), np.array([0.0, 0.0, height / 2])) |
| 84 | M = pin.SE3(R, t) |
| 85 | Mtot = M * translation |
| 86 | cylinder = hppfcl.Cylinder(linewidth, height) |
| 87 | meshcat_shape = load_primitive(cylinder) |
| 88 | viz.viewer[line_name].set_object(meshcat_shape, meshcat_material(*color)) |
| 89 | viz.viewer[line_name].set_transform(Mtot.homogeneous) |
| 90 | |
| 91 | |
| 92 | def register_arrowed_line( |
no test coverage detected