Могу ли я визуализировать позу Multibody без явного расчета полного преобразования каждого тела? - PullRequest
Новая
       6

Могу ли я визуализировать позу Multibody без явного расчета полного преобразования каждого тела?

0 голосов
/ 13 июня 2019

В примере examples/quadrotor/ указан пользовательский QuadrotorPlant, и его выходные данные передаются в QuadrotorGeometry, где состояние QuadrotorPlant упаковано в FramePoseVector для SceneGraph для визуализации.

Соответствующий сегмент кода в QuadrotorGeometry, который делает это: 

...
  builder->Connect(
      quadrotor_geometry->get_output_port(0),
      scene_graph->get_source_pose_port(quadrotor_geometry->source_id_));
...

void QuadrotorGeometry::OutputGeometryPose(
    const systems::Context<double>& context,
    geometry::FramePoseVector<double>* poses) const {
  DRAKE_DEMAND(frame_id_.is_valid());

  const auto& state = get_input_port(0).Eval(context);
  math::RigidTransformd pose(
      math::RollPitchYawd(state.segment<3>(3)),
      state.head<3>());

  *poses = {{frame_id_, pose.GetAsIsometry3()}};
}

В моем случае, у меня есть многотельная система с плавающей запятой (например, квадратор с прикрепленным маятником), из которой я создалзавод по индивидуальному заказу (LeafSystem).Минимальные координаты для такой системы были бы 4 (кватернион) + 3 (x, y, z) + 1 (угол соединения) = 7. Если бы я следовал примеру QuadrotorGeometry, я считаю, что мне нужно было бы указать полный RigidTransformd для квадротора и полный RigidTransformd маятника.

Вопрос

Можно ли настроить визуализацию / указать позу так, чтобы мне нужно было только указать 7 (поза квадротора + угол сочленения) задайте минимальные координаты и попросите внутренний MultibodyPlant обработать вычисление каждого отдельного тела (квадротора и маятника), заполненного RigidTransform, который затем может быть передан в SceneGraph для визуализации?

Я полагаю, что это было возможно с «чердаком» (что я понимаю как «быть устаревшим») RigidBodyTree, что было достигнуто в examples/compass_gait

  lcm::DrakeLcm lcm;
  auto publisher = builder.AddSystem<systems::DrakeVisualizer>(*tree, &lcm);
  publisher->set_name("publisher");

  builder.Connect(compass_gait->get_floating_base_state_output_port(),
                  publisher->get_input_port(0));

Где get_floating_base_state_output_port()выводил состояние CompassGait только с 7 состояниями (3 rpy + 3 xyz + 1 угол бедра).

Что такое MultibodyPlant, SceneGraph эквивалент этого?

Обновление(Используя MultibodyPositionToGeometryPose из удаленного ответа Русса

Я создал следующую функцию, которая пытается создать MultibodyPlant из заданного model_file и соединяет данное растение pose_output_port через MultibodyPositionToGeometryPose.

Используемое pose_output_port - это минимальное состояние 4 (кватернион) + 3 (xyz) + 1 (угол соединения). 

void add_plant_visuals(
        systems::DiagramBuilder<double>* builder,
        geometry::SceneGraph<double>* scene_graph,
        const std::string model_file,
        const systems::OutputPort<double>& pose_output_port)
{
    multibody::MultibodyPlant<double> mbp;
    multibody::Parser parser(&mbp, scene_graph);
    auto model_id = parser.AddModelFromFile(model_file);
    mbp.Finalize();

    auto source_id = *mbp.get_source_id();

    auto multibody_position_to_geometry_pose = builder->AddSystem<systems::rendering::MultibodyPositionToGeometryPose<double>>(mbp);
    builder->Connect(pose_output_port,
            multibody_position_to_geometry_pose->get_input_port());
    builder->Connect(
            multibody_position_to_geometry_pose->get_output_port(),
            scene_graph->get_source_pose_port(source_id));

    geometry::ConnectDrakeVisualizer(builder, *scene_graph);
}

Вышеприведенное не выполнено со следующим исключением 

abort: Failure at multibody/plant/multibody_plant.cc:2015 in get_geometry_poses_output_port(): condition 'geometry_source_is_registered()' failed.

Ответы [ 2 ]

1 голос
/ 19 июня 2019

Этот ответ относится, в частности, к вашему редактированию, когда вы пытаетесь использовать подход MultibodyPositionToGeometryPose.Это не решает большие проблемы дизайна.

Ваша проблема в том, что система MultibodyPositiontToGeometryPose берет ссылку на MBP и сохраняет ссылку на ту же самую MBP.Это означает, что MBP должен быть жив и здоров, по крайней мере, до тех пор, пока MPTGP.Однако в вашем фрагменте кода ваш MBP является локальным по отношению к функции add_plant_visuals(), поэтому он уничтожается, как только функция завершается.

Вам нужно будет создать что-то, что сохраняется и принадлежиткто-то еще.

(Это тесно связано с моим вариантом 2 - теперь отредактировано для большей ясности.)

1 голос
/ 14 июня 2019

Итак, здесь много всего. У меня есть подозрение, что есть простой ответ, но нам, возможно, придется сойтись на нем.

Сначала мои предположения:

  1. У вас есть «внутренний» MultibodyPlant (MBP). Предположительно, у вас также есть контекст для этого, позволяющий выполнять значимые вычисления в зависимости от состояния.
  2. Кроме того, я предполагаю, что MBP отвечал за регистрацию геометрии (вероятно, это произошло, когда вы ее проанализировали).
  3. Ваш LeafSystem будет напрямую подключаться к SceneGraph для предоставления поз.
  4. Учитывая ваше состояние, вы обычно устанавливаете состояние в контексте MBP для выполнения этой оценки.

Вариант 1 (отредактировано):

В своем пользовательском LeafSystem создайте выходной порт FramePoseVector, создайте для него обратный вызов calc и внутри этого обратного вызова просто вызовите Eval() выходного порта позы внутреннего MBP, который ваш LeafSystem own (предварительно установив состояние в Context в вашем локальном владении для MBP и передав указатель на FramePoseVector, который был предоставлен вашему обратному вызову LeafSystem).

По существу (очень грубо): 

MySystem::MySystem() {
  this->DeclareAbstractOutputPort("geometry_pose",
      &MySystem::OutputGeometryPose);
}

void MySystem::OutputGeometryPose(
    const Context& context, FramePoseVector* poses) const {
  mbp_context_.get_mutable_continuous_state()
    .SetFromVector(my_state_vector);
  mbp_.get_geometry_poses_output_port().Eval(mpb_context_, poses);
}

Вариант 2:

Вместо реализации LeafSystem с внутренней установкой вы можете иметь Diagram, который содержит MBP и экспортирует вывод FramePoseVector MBP непосредственно через диаграмму для подключения.

...