Function simulateSytem

sandbox/simulation_utils.py:120–554

(
    model: pin.Model,
    geom_model: pin.GeometryModel,
    visual_model: pin.GeometryModel,
    q0: np.ndarray,
    v0: np.ndarray,
    policy: Policy,
    args: Dict,
)

def simulateSytem(
    model: pin.Model,
    geom_model: pin.GeometryModel,
    visual_model: pin.GeometryModel,
    q0: np.ndarray,
    v0: np.ndarray,
    policy: Policy,
    args: Dict,
):
    print(f"Number of bodies in model = {model.nbodies}")
    print("Num geom obj = ", len(geom_model.geometryObjects))
    if args.debug:
        for i, inertia in enumerate(model.inertias):
            print("------------------------")
            print(f"Inertia {i} =\n {inertia}")

        for gobj in geom_model.geometryObjects:
            print(
                f"name = {gobj.name}, {gobj.geometry.getNodeType()}, parent joint = {gobj.parentJoint}"
            )

    if args.display:
        from pinocchio.visualize import MeshcatVisualizer
        import meshcat

        if args.debug:
            if args.display_collision_model:
                rendered_model = geom_model
            else:
                rendered_model = visual_model
            for gobj in rendered_model.geometryObjects:
                color = gobj.meshColor
                if gobj.name == "floor":
                    color[3] = 0.2
                else:
                    color[3] = args.debug_transparency
                gobj.meshColor = color

        # visualize the trajectory
        viewer = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000")
        viewer.delete()
        viewer["/Background"].set_property("top_color", BEIGE[:3].tolist())
        viewer["/Background"].set_property("bottom_color", BEIGE[:3].tolist())
        viewer["/Lights/SpotLight/<object>"].set_property("position", [-10, -10, -10])
        viewer["/Lights/PointLightPositiveX/<object>"].set_property(
            "position", [10, 10, 10]
        )
        vizer: MeshcatVisualizer = MeshcatVisualizer(model, geom_model, visual_model)
        vizer.initViewer(viewer=viewer, open=False, loadModel=True)
        vizer.display(q0)
        if args.display_collision_model:
            vizer.displayCollisions(True)
            vizer.displayVisuals(False)
        else:
            vizer.displayCollisions(False)
            vizer.displayVisuals(True)

    data = model.createData()