PyDrake中MonteCarloSimulation下自由体位姿被重置问题排查

我在调试PyDrake的MonteCarloSimulation时遇到如下异常：

• 预先将自由立方体的默认位姿设为平移(2, 1, 0)
• 在传入MonteCarloSimulation的make_simulator函数中，自由体的位姿正确
• 但在输出评估回调calc_loss函数中，自由体的平移位姿被重置为(0, 0, 0)
• 若为立方体添加平面关节则无此问题

复现代码 #

import numpy as np
np.set_printoptions(suppress=True)
np.set_printoptions(formatter={'float': lambda x: "{0:0.3f}".format(x)})

from pydrake.all import (
    AddMultibodyPlantSceneGraph, Box, DiagramBuilder, MeshcatVisualizer, RigidTransform, RotationMatrix, 
    Simulator, SpatialInertia, Sphere, UnitInertia, 
    StartMeshcat, CoulombFriction, MonteCarloSimulation, RandomGenerator
)

# Start the visualizer.
meshcat = StartMeshcat()

def AddBox(plant, shape, name, mass=1, mu=1, color=[.5, .5, .9, 1.0]):
    instance = plant.AddModelInstance(name)
    inertia = UnitInertia.SolidBox(shape.width(), shape.depth(),
                                       shape.height())
     
    body = plant.AddRigidBody(
        name, instance,
        SpatialInertia(mass=mass,
                       p_PScm_E=np.array([0., 0., 0.]),
                       G_SP_E=inertia))
    if plant.geometry_source_is_registered():
        new_box_dims = np.array([x-(2 * 1e-7) for x in shape.size()])
        smaller_box = Box(*new_box_dims)

        sphere = Sphere(1e-7)
        sphere_transforms = [
            RigidTransform(RotationMatrix(),new_box_dims * 0.5),
            RigidTransform(RotationMatrix(),new_box_dims * -0.5),
            RigidTransform(RotationMatrix(),new_box_dims * 0.5 * [-1, 1, 1]),
            RigidTransform(RotationMatrix(),new_box_dims * 0.5 * [1, -1, 1]),
            RigidTransform(RotationMatrix(),new_box_dims * 0.5 * [1, 1, -1]),
            RigidTransform(RotationMatrix(),new_box_dims * 0.5 * [1, -1, -1]),
            RigidTransform(RotationMatrix(),new_box_dims * 0.5 * [-1, -1, 1]),
            RigidTransform(RotationMatrix(),new_box_dims * 0.5 * [-1, 1, -1]),
            ]
        for i, t in enumerate(sphere_transforms):
            sphere_name = f"point_{i}"
            plant.RegisterVisualGeometry(body, t, sphere, sphere_name, color)
            plant.RegisterCollisionGeometry(body, t, sphere, sphere_name,
                                        CoulombFriction(mu, mu))       
        
        plant.RegisterCollisionGeometry(body, RigidTransform(), smaller_box, name,
                                        CoulombFriction(mu, mu))        
        plant.RegisterVisualGeometry(body, RigidTransform(), shape, name, color)
    return body

def run_demo():
    mass = 1
    mu = 0.1
    mu_g = 0.1
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01)
    box_geometry = Box(10,10,0.1) 
    X_WBox = RigidTransform(RotationMatrix(), [0, 0, -0.6])
    plant.RegisterCollisionGeometry(plant.world_body(), X_WBox, box_geometry, "ground",
                                    CoulombFriction(mu_g, mu_g))
    plant.RegisterVisualGeometry(plant.world_body(), X_WBox, box_geometry, "ground",
                                    [.9, .9, .9, 1.0])
    
    box_geometry = Box(1,1,1) 
    
    X_WBox_Default = RigidTransform(RotationMatrix(), [2, 1, 0])
    
    box = AddBox(plant, box_geometry, "box", 
                        mass, mu, color=[0.8, 0, 0, 1.0])
    
    plant.SetDefaultFreeBodyPose(box, X_WBox_Default)

    plant.Finalize()

    MeshcatVisualizer.AddToBuilder(
        builder, scene_graph, meshcat)

             
    diagram = builder.Build()
    
    # Set up a simulator to run this diagram
    simulator = Simulator(diagram)
    
    context = simulator.get_mutable_context()
    plant_context = plant.GetMyMutableContextFromRoot(context)
    simulator.set_target_realtime_rate(1.0)
    simulator.Initialize()
    simulator.AdvanceTo(3) # Wait 3 seconds to verify initial positions is correct
    print("Initial position: ", plant.GetPositions(plant_context))

    # Perform the Monte Carlo simulation.
    def make_simulator(generator):
        ''' Create a simulator for the system
            using the given generator. '''
        simulator = Simulator(diagram)
        sim_context = simulator.get_mutable_context()
        sim_plant_context = plant.GetMyMutableContextFromRoot(sim_context)
        simulator.set_target_realtime_rate(1.0)
        simulator.Initialize()
        simulator.AdvanceTo(2.0)
        box_pos = plant.GetPositions(sim_plant_context)
        print(f"\nmake_simulator box pos: {box_pos}")

        return simulator

    
    def calc_loss(system, calc_loss_context):
        ''' Given a context from the end of the simulation,
            calculate a loss - distance between goal state
            and current state '''
 
        calc_loss_plant_context = plant.GetMyContextFromRoot(calc_loss_context)
        box_pos = plant.GetPositions(calc_loss_plant_context)
        
        print(f"calc_loss box pos: {box_pos}")
        return 1
    
    results = MonteCarloSimulation(
                make_simulator=make_simulator, output=calc_loss,
                final_time=5.0, num_samples=10, generator=RandomGenerator())
    

    meshcat.DeleteAddedControls()
    meshcat.Delete()
    return

run_demo()

运行输出 #

INFO:drake:Meshcat listening for connections at http://localhost:7002
Initial position: [1.000 -0.000 0.000 0.000 2.000 1.000 -0.050]

make_simulator box pos: [1.000 -0.000 0.000 0.000 2.000 1.000 -0.050]
calc_loss box pos: [1.000 -0.000 0.000 -0.000 0.000 0.000 -0.050]

make_simulator box pos: [1.000 -0.000 0.000 0.000 2.000 1.000 -0.050]
calc_loss box pos: [1.000 -0.000 0.000 -0.000 0.000 0.000 -0.050]
...

补充说明 #

若为立方体添加平面关节，calc_loss调用时位姿不会被重置。

内容的提问来源于stack exchange，提问作者Shao Yuan Chew Chia