При прямом сопоставлении: InputPort :: Eval (): требуется InputPort [0] (geometry_query) - PullRequest
Новая
       18

При прямом сопоставлении: InputPort :: Eval (): требуется InputPort [0] (geometry_query)

0 голосов
/ 06 мая 2020

Я получаю сообщение об ошибке при выполнении DirectCollocation с геометрией столкновения: 

RuntimeError: InputPort::Eval(): required InputPort[0] (geometry_query) of System ::drake/multibody/MultibodyPlant@0000000003550740 (MultibodyPlant<drake::AutoDiffXd>) is not connected

На основании предыдущего вопроса , я добавил оба соединения как следует, но все еще не исправить ошибку. 

builder.Connect(plant.get_geometry_poses_output_port(),
                scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
                plant.get_geometry_query_input_port())

Это мой скелет кода: 

builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Make and add the model
tmp = make_acrobot_plant(params, finalize, scene_graph)
plant = builder.AddSystem(tmp)
plant.Finalize()

# https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html
builder.Connect(plant.get_geometry_poses_output_port(),
                scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
                plant.get_geometry_query_input_port())

num_time_samples = 40
dircol = DirectCollocation(
    plant,
    context,
    num_time_samples=num_time_samples,
    minimum_timestep=0.01,
    maximum_timestep=0.05,
    input_port_index=plant.get_actuation_input_port().get_index())
dircol.AddEqualTimeIntervalsConstraints()
u = dircol.input()
dircol.AddRunningCost(Ru * u.dot(u))
dircol.SetInitialTrajectory(traj_init_u, traj_init_x)
result = Solve(dircol)
assert result.is_success()

# Connect
# Simulate u
controls = builder.AddSystem(TrajectorySource(u_trajectory))
builder.Connect(
    controls.get_output_port(0), plant.get_actuation_input_port())
'''
# Simulate x
source = builder.AddSystem(TrajectorySource(x_trajectory))
pos_to_pose = builder.AddSystem(
    MultibodyPositionToGeometryPose(plant, input_multibody_state=True))
builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port())
builder.Connect(pos_to_pose.get_output_port(),
                scene_graph.get_source_pose_port(plant.get_source_id()))
'''
# ...
# Visualization:
builder.Connect(scene_graph.get_pose_bundle_output_port(),
                planar_scene_graph_vis.get_input_port(0))
diagram = builder.Build()

1 Ответ

0 голосов
/ 06 мая 2020

Вы близки, но вы только передали растение в DirectCollocation. Вам нужно передать диаграмму с растением + графом сцены в DirectCollocation. (И вам также потребуется assume_non_continuous_states_are_fixed=true, чтобы игнорировать абстрактное состояние SceneGraph).

...