Установка ненулевого шага по времени в Multibody Plant по-прежнему приводит к непрерывной системе. ⇐ Python

[Anonymous]

Что касается программного обеспечения Drake с открытым исходным кодом, я должен убедиться, что объект из множества тел, который я создаю для робота, должен быть дискретной системой. Насколько я понимаю, если вы вызываете AddMultibodyPlant с time_step=0.01 (любой ненулевой временной шаг), гарантируется, что результирующий объект будет дискретным. Однако, когда я использую свой инициализированный объект, Дрейк, кажется, продолжает инициализировать его с помощью непрерывного дифференциального уравнения.
РЕДАКТИРОВАТЬ: я ошибочно сказал, что Дрейк инициализирует с помощью разностное уравнение, оно должно было быть непрерывным дифференциальным уравнением.

Код: Выделить всё

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)

Есть ли еще что-то, на что мне следует обратить внимание? Любые советы будут полезны.
Спасибо за ваши инструкции.

Подробнее здесь: https://stackoverflow.com/questions/792 ... nuous-syst