python - Setting non-zero time step in Multibody Plant still results in a Continuous system - Stack Overflow

admin管理员组

文章数量:1122846

For a piece of open-source drake software, I have to ensure that the multibody plant I create for the robot needs to be a discrete system. It is my understanding that if you call a AddMultibodyPlant with a time_step=0.01 (any non-zero timestep) ensures that the resultant plant is discrete. However, when I use my initialized plant, drake seems to keep initializing it with a continuous differential equation.

EDIT: I had mistakenly said that drake initializes with a difference equation, it should have been a continouous differential equation.

def CreateSystemModel(plant, scene_graph, config):

    # create system model for iLQR implementation
    iiwa_dmd = './assembly.dmd.yaml'
    parser = Parser(plant)

    # add iiwa robot, tables, and hole
    model_indices = parser.AddModels(iiwa_dmd)
    
    # add some transforms to denote the hole
    hole_index = plant.GetModelInstanceByName("hole")
    hole_frame = plant.AddFrame(
        FixedOffsetFrame(
            "hole_frame",
            plant.GetFrameByName(
                "base_link",
                hole_index),
            RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2))
        )
    )

    # adding peg to the scene
    G_PPcm_P = UnitInertia.SolidCylinder(
        config["peg_width"]*0.5,
        config["peg_length"],
        [0, 0, 1]
    )
    M_PPcm_P = SpatialInertia(
        config["peg_mass"],
        [0, 0, 0],
        G_PPcm_P
    )
    peg_geom = Cylinder(
        config["peg_width"]*0.5,
        config["peg_length"]
    )

    peg = plant.AddRigidBody(
        'peg',
        plant.GetModelInstanceByName(
            'iiwa'),
        M_PPcm_P
    )
    plant.RegisterVisualGeometry(
        peg,
        RigidTransform(),
        peg_geom,
        'peg',
        [0.5, 0.5, 0.5, 1.0]
    )
    plant.RegisterCollisionGeometry(
        peg,
        RigidTransform(),
        peg_geom,
        'peg',
        config['peg_props']
    )
    plant.WeldFrames(
        plant.GetFrameByName("iiwa_link_7"),
        peg.body_frame(),
        RigidTransform([0, 0, 0.06])
    )
    stiffness_frame = plant.AddFrame(
        FixedOffsetFrame(
            "stiffness_frame",
            peg.body_frame(),
            RigidTransform(
                RotationMatrix.MakeXRotation(-np.pi),
                [0, 0, config["peg_length"]/2]))
    )
    plant.RegisterVisualGeometry(
        peg,
        RigidTransform(
            [0, 0, config['peg_length']/2]),
        Sphere(0.002),
        "peg_tip",
        [1.0, 0.0, 0.0, 1.0]
    )
    
    # load up uncertain hole just for visualisation
    # load up the noisy hole to plan towards
    hole_uncertain = "/root/workspace/model/peg_uncertain.urdf"
    hole_plan = "/root/workspace/model/peg_plan.urdf"
    sigma_parser = Parser(plant, "sigma")
    plan_parser = Parser(plant, "plan_hole")
    sigma_parser.SetAutoRenaming(True)
    plan_parser.SetAutoRenaming(True)
    for i in range(10):
        x = np.random.normal(0.5, 0.005)
        y = np.random.normal(0.0, 0.005)
        z = np.random.normal(0.08, 0.0025)
        rot = np.random.normal(-np.pi/2, 0.005)
        hole_index = sigma_parser.AddModels(hole_uncertain)[0]
        plant.WeldFrames(
            plant.world_frame(),
            plant.GetFrameByName("base_link", hole_index),
            RigidTransform(
            RotationMatrix.MakeXRotation(np.pi/2),
            [x, y, z])
        )
        hole_frame = plant.AddFrame(
            FixedOffsetFrame(
                "hole_frame_{}".format(i),
                plant.GetFrameByName(
                    "base_link",
                    hole_index),
                RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2))
            )
        )

    x = np.random.normal(0.5, 0.0025)
    y = np.random.normal(0.0, 0.0025)
    # z = np.random.normal(0.08, 0.0025)
    z = 0.08
    x_rot = np.random.normal(0.0, 2.5)*0
    y_rot = np.random.normal(0.0, 2.5)*0
    z_rot = np.random.normal(0.0, 2.5)*0
    drake_rotation = RotationMatrix.MakeXRotation(np.pi/2 + np.deg2rad(x_rot)).MakeYRotation(np.deg2rad(y_rot)).MakeZRotation(np.deg2rad(z_rot))
    plan_hole_index = plan_parser.AddModels(hole_plan)[0]
    plant.WeldFrames(
        plant.world_frame(),
        plant.GetFrameByName("base_link", plan_hole_index),
        RigidTransform(
        RotationMatrix.MakeXRotation(np.pi/2 + np.deg2rad(x_rot)),
        [x, y, z])
    )
    hole_frame = plant.AddFrame(
        FixedOffsetFrame(
            "hole_frame",
            plant.GetFrameByName(
                "base_link",
                plan_hole_index),
            RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2))
        )
    )
    plant.Finalize()
    logger.info(f" time step: {plant.time_step()}")
    assert plant.IsDifferenceEquationSystem() "Has to be discrete time"
    return plant, scene_graph
  
plant, scene_graph = AddMultibodyPlant(
        config=plant_config,
        builder=builder
    )
plant, scene_graph = CreateSystemModel(plant, scene_graph, config)

Is there something else I need to be looking at? Any tips would be helpful

Thank you for your instruction.

For a piece of open-source drake software, I have to ensure that the multibody plant I create for the robot needs to be a discrete system. It is my understanding that if you call a AddMultibodyPlant with a time_step=0.01 (any non-zero timestep) ensures that the resultant plant is discrete. However, when I use my initialized plant, drake seems to keep initializing it with a continuous differential equation.

EDIT: I had mistakenly said that drake initializes with a difference equation, it should have been a continouous differential equation.

def CreateSystemModel(plant, scene_graph, config):

    # create system model for iLQR implementation
    iiwa_dmd = './assembly.dmd.yaml'
    parser = Parser(plant)

    # add iiwa robot, tables, and hole
    model_indices = parser.AddModels(iiwa_dmd)
    
    # add some transforms to denote the hole
    hole_index = plant.GetModelInstanceByName("hole")
    hole_frame = plant.AddFrame(
        FixedOffsetFrame(
            "hole_frame",
            plant.GetFrameByName(
                "base_link",
                hole_index),
            RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2))
        )
    )

    # adding peg to the scene
    G_PPcm_P = UnitInertia.SolidCylinder(
        config["peg_width"]*0.5,
        config["peg_length"],
        [0, 0, 1]
    )
    M_PPcm_P = SpatialInertia(
        config["peg_mass"],
        [0, 0, 0],
        G_PPcm_P
    )
    peg_geom = Cylinder(
        config["peg_width"]*0.5,
        config["peg_length"]
    )

    peg = plant.AddRigidBody(
        'peg',
        plant.GetModelInstanceByName(
            'iiwa'),
        M_PPcm_P
    )
    plant.RegisterVisualGeometry(
        peg,
        RigidTransform(),
        peg_geom,
        'peg',
        [0.5, 0.5, 0.5, 1.0]
    )
    plant.RegisterCollisionGeometry(
        peg,
        RigidTransform(),
        peg_geom,
        'peg',
        config['peg_props']
    )
    plant.WeldFrames(
        plant.GetFrameByName("iiwa_link_7"),
        peg.body_frame(),
        RigidTransform([0, 0, 0.06])
    )
    stiffness_frame = plant.AddFrame(
        FixedOffsetFrame(
            "stiffness_frame",
            peg.body_frame(),
            RigidTransform(
                RotationMatrix.MakeXRotation(-np.pi),
                [0, 0, config["peg_length"]/2]))
    )
    plant.RegisterVisualGeometry(
        peg,
        RigidTransform(
            [0, 0, config['peg_length']/2]),
        Sphere(0.002),
        "peg_tip",
        [1.0, 0.0, 0.0, 1.0]
    )
    
    # load up uncertain hole just for visualisation
    # load up the noisy hole to plan towards
    hole_uncertain = "/root/workspace/model/peg_uncertain.urdf"
    hole_plan = "/root/workspace/model/peg_plan.urdf"
    sigma_parser = Parser(plant, "sigma")
    plan_parser = Parser(plant, "plan_hole")
    sigma_parser.SetAutoRenaming(True)
    plan_parser.SetAutoRenaming(True)
    for i in range(10):
        x = np.random.normal(0.5, 0.005)
        y = np.random.normal(0.0, 0.005)
        z = np.random.normal(0.08, 0.0025)
        rot = np.random.normal(-np.pi/2, 0.005)
        hole_index = sigma_parser.AddModels(hole_uncertain)[0]
        plant.WeldFrames(
            plant.world_frame(),
            plant.GetFrameByName("base_link", hole_index),
            RigidTransform(
            RotationMatrix.MakeXRotation(np.pi/2),
            [x, y, z])
        )
        hole_frame = plant.AddFrame(
            FixedOffsetFrame(
                "hole_frame_{}".format(i),
                plant.GetFrameByName(
                    "base_link",
                    hole_index),
                RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2))
            )
        )

    x = np.random.normal(0.5, 0.0025)
    y = np.random.normal(0.0, 0.0025)
    # z = np.random.normal(0.08, 0.0025)
    z = 0.08
    x_rot = np.random.normal(0.0, 2.5)*0
    y_rot = np.random.normal(0.0, 2.5)*0
    z_rot = np.random.normal(0.0, 2.5)*0
    drake_rotation = RotationMatrix.MakeXRotation(np.pi/2 + np.deg2rad(x_rot)).MakeYRotation(np.deg2rad(y_rot)).MakeZRotation(np.deg2rad(z_rot))
    plan_hole_index = plan_parser.AddModels(hole_plan)[0]
    plant.WeldFrames(
        plant.world_frame(),
        plant.GetFrameByName("base_link", plan_hole_index),
        RigidTransform(
        RotationMatrix.MakeXRotation(np.pi/2 + np.deg2rad(x_rot)),
        [x, y, z])
    )
    hole_frame = plant.AddFrame(
        FixedOffsetFrame(
            "hole_frame",
            plant.GetFrameByName(
                "base_link",
                plan_hole_index),
            RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2))
        )
    )
    plant.Finalize()
    logger.info(f" time step: {plant.time_step()}")
    assert plant.IsDifferenceEquationSystem() "Has to be discrete time"
    return plant, scene_graph
  
plant, scene_graph = AddMultibodyPlant(
        config=plant_config,
        builder=builder
    )
plant, scene_graph = CreateSystemModel(plant, scene_graph, config)

Is there something else I need to be looking at? Any tips would be helpful

Thank you for your instruction.

• python
• drake

Share Improve this question Follow edited Nov 21, 2024 at 18:23 kamiradi asked Nov 21, 2024 at 14:40 kamiradikamiradi 2577 bronze badges 5

• This all comes down to the value in plant_config but that doesn't seem to be included in your code snippet. – Sean Curtis Commented Nov 21, 2024 at 15:32
• Also, where does the IsDifferentEquationSystem() method come from? I don't believe it's part of Drake. – Sean Curtis Commented Nov 21, 2024 at 15:34
• Perhaps it's a misspelling of IsDifferenceEquationSystem. See also the documentation around SetUsedSampledOutputPorts. – Russ Tedrake Commented Nov 21, 2024 at 15:37
• plant_config is as follows: ` plant_config = MultibodyPlantConfig() plant_config.time_step = 1e-2 plant_config.contact_model = "hydroelastic_with_fallback" plant_config.contact_surface_representation = "polygon" plant_config.penetration_allowance = 1e-5 ` and yes it is plant.IsDifferenceEquation(), corrected now. – kamiradi Commented Nov 21, 2024 at 18:21
• My first thought is that github.com/RobotLocomotion/drake/pull/21623 introduced non-discrete state into MultibodyPlant so that it’s no longer a difference equation system. In others word, the plant is still discrete (plant.is_discrete() == true), but your particular assertion is no longer true. – Xuchen Han Commented Nov 22, 2024 at 7:11

Add a comment  | 

1 Answer 1

Sorted by: Reset to default 2

If you want plant.IsDifferenceEquationSystem() to be true (i.e., you need the plant to be purely a discrete system), then you must change plant_config.use_sampled_output_ports = False. Refer to the MbP#output_port_sampling doc for details. The plant is using discrete dynamics (plant.is_discrete() is True), but when output port sampling is enabled, it has additional state that is not expressible as a simple difference equation.

本文标签： pythonSetting nonzero time step in Multibody Plant still results in a Continuous systemStack Overflow