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.
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.