Приложение внешней силы к объекту в пидраке - PullRequest
Новая
       7

Приложение внешней силы к объекту в пидраке

3 голосов
/ 17 апреля 2020

Этот вопрос тесно связан с добавлением сил к телу после финализации

Я хотел бы иметь возможность применить внешнюю силу к простым примитивам геометрии c в пидраке. Это необходимо для оценки взаимодействия между телами.

Моя текущая реализация: 


builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.001))
parser = Parser(plant)
cube_instance = parser.AddModelFromFile('cube.urdf', model_name='cube')

plant.Finalize()

force = builder.AddSystem(ConstantVectorSource(np.zeros(6)))
builder.Connect(force.get_output_port(0), plant.get_applied_spatial_force_input_port())

diagram = builder.Build()

Однако, когда я ее запускаю, я получаю следующую ошибку: 

builder.Connect(force.get_output_port(0), plant.get_applied_spatial_force_input_port())
RuntimeError: DiagramBuilder::Connect: Cannot mix vector-valued and abstract-valued ports while connecting output port y0 of System drake/systems/ConstantVectorSource@0000000002db5aa0 to input port applied_spatial_force of System drake/multibody/MultibodyPlant@0000000003118680

У меня есть склонность к реализации LeafSystem, которая реализует порт абстрактного значения на заводе.

Обновление на основе предложения

Использование: AbstractValue и ConstantValueSource 

value = AbstractValue.Make([np.zeros(6)])
force = builder.AddSystem(ConstantValueSource(value))

ref_vector_externally_applied_spatial_force = plant.get_applied_spatial_force_input_port()
builder.Connect(force.get_output_port(0), ref_vector_externally_applied_spatial_force)

Я получил следующую ошибку: 

RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting
output port y0 of System drake/systems/ConstantValueSource@0000000002533a30 (type pybind11::object) to
input port applied_spatial_force of System drake/multibody/MultibodyPlant@0000000002667760 (type std::vector<drake::multibody::ExternallyAppliedSpatialForce<double>,std::allocator<drake::multibody::ExternallyAppliedSpatialForce<double>>>)

Что имеет смысл типы из портов ввода-вывода должны совпадать. Предполагается, что ожидаемый тип является вектором ExternallyAppliedSpatialForce.

Затем я изменил тип Abstract следующим образом: 

value = AbstractValue.Make(ExternallyAppliedSpatialForce())

RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting
output port y0 of System drake/systems/ConstantValueSource@0000000002623980 (type drake::multibody::ExternallyAppliedSpatialForce<double>)
to input port applied_spatial_force of System drake/multibody/MultibodyPlant@00000000027576b0 (type std::vector<drake::multibody::ExternallyAppliedSpatialForce<double>,std::allocator<drake::multibody::ExternallyAppliedSpatialForce<double>>>)

Я приближаюсь. Однако я не смог отправить вектор ExternallyAppliedSpatialForce. Если я пытаюсь отправить его в виде списка, я получаю жалобу на невозможность засолки объекта. В примерах AbstractValue я не видел, как создать такой вектор объектов.

Любая дополнительная помощь будет принята с благодарностью.

Решением было использование типа VectorExternallyAppliedSpatialForced

полного решения, которое будет опубликовано позже.

Ответы [ 2 ]

4 голосов
/ 17 апреля 2020

Нет необходимости реализовывать LeafSystem - у нас есть ConstantValueSource, аналогичный ConstantVectorSource, но для абстрактных типов. https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_constant_value_source.html

И вы правы, что вам нужен абстрактный тип для этого порта: https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#ab2ad1faa7547d440f008cdddd32d85e8

Некоторые примеры работы с абстрактные значения от python можно найти здесь: https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/systems/test/value_test.py#L84

2 голосов
/ 20 апреля 2020

Вот рабочий пример применения внешней жесткости c к жесткому объекту: 

sim_time_step = 0.001
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, sim_time_step)
object_instance = Parser(plant).AddModelFromFile('box.urdf')
scene_graph.AddRenderer("renderer", MakeRenderEngineVtk(RenderEngineVtkParams()))
ConnectDrakeVisualizer(builder, scene_graph)

plant.Finalize()

force_object = ExternallyAppliedSpatialForce()
force_object.body_index = plant.GetBodyIndices(object_instance).pop()
force_object.F_Bq_W = SpatialForce(tau=np.zeros(3), f=np.array([0., 0., 10.]))

forces = VectorExternallyAppliedSpatialForced()
forces.append(force_object)

value = AbstractValue.Make(forces)
force_system = builder.AddSystem(ConstantValueSource(value))

builder.Connect(force_system.get_output_port(0), plant.get_applied_spatial_force_input_port())

diagram = builder.Build()
simulator = Simulator(diagram)

context = simulator.get_mutable_context()

plant.SetPositions(context, object_instance, [0, 0, 0, 1, 0, 0, 0])

time_ = 0
while True:
    time_ += sim_time_step
    simulator.AdvanceTo(time_)
    time.sleep(sim_time_step)

Однако я не смог изменить внешне приложенную силу в симуляции l oop впоследствии ,

Для достижения этой цели мне пришлось создать систему LeafSystem.

Можно найти реализацию, которая позволяет изменять силу, прикладываемую к жесткому объекту с течением времени, здесь

Примеры: c и динамические c * можно найти здесь .

...