Селезень: проблема с подключением выхода установки к входу визуализатора

Question

На заднем плане я управляю четырехугольным крылом с состояниями x=[X,Y,Z,ϕ,θ,ψ,X˙,Y˙,Z˙,ϕ˙,θ˙,ψ˙] и управляю введите u=[ω1^2,ω2^2,ω3^2,ω4^2,θ1,θ2,θ3,θ4], здесь ωi - скорость гребного винта, а θi - угол наклона крыльев. По сути, я предполагаю, что могу мгновенно контролировать углы наклона, и меня волнует только положение и положение тела самолета. Поэтому в моем коде num_states равно 12, а num_inputs - 8.

Для визуализации я создал файл urdf, в котором я устанавливаю наклонные крылья, используя вращающиеся соединения. И в Drake, когда я загружаю urdf с использованием плавающей базы, жесткое тело генерирует 20 состояний. Это неудивительно, так как я предполагаю, что 12 из состояний: x=[X,Y,Z,ϕ,θ,ψ,X˙,Y˙,Z˙,ϕ˙,θ˙,ψ˙], и дополнительные 8 состояний происходят из состояний вращающихся соединений [θ1,θ2,θ3,θ4,θ1˙,θ2˙,θ3˙,θ4˙].

Просто сегодня, когда я строил код на С ++, возникает проблема, если я использую:builder.Connect(quad_tilt_wing->get_output_port(0), visualizer->get_input_port(0));

потому что 12 не равно 20.

Вот мне и интересно, как мне это решить? Я думаю о двух способах:

(1) Во-первых, я изменяю свою динамику так, чтобы мои состояния были x=[X,Y,Z,ϕ,θ,ψ,X˙,Y˙,Z˙,ϕ˙,θ˙,ψ˙,θ1,θ2,θ3,θ4,θ1˙,θ2˙,θ3˙,θ4˙] и мои элементы управления: u=[ω21,ω22,ω23,ω24,θ1,θ2,θ3,θ4]. Но в этом смысле я не могу придумать способ записать динамику в виде x˙ = f (x, u).

(2) Другой способ, мне интересно, могу ли я разделить входной порт визуализатора на 2 порта, один из которых 12, а другой 8, а затем я подаю 12 с моей динамикой х, 8 с прямой контролировать с помощью Это выполнимо?

Или у вас есть лучший способ решения этой проблемы?

Answer 1

Взгляните на http://drake.mit.edu/doxygen_cxx/group__primitive__systems.html

Вы должны иметь возможность демультиплексировать ваш вход, чтобы получить сигнал, который имеет только тета-значения (которые вы хотите подключить к своему входу визуализации). Соблазнительно сказать, что вы можете объединить два сигнала в сигнал, который вы посылаете на вход визуализатора, но я думаю, что вы хотите вставить входы в середину вектора. Поэтому я подозреваю, что вы захотите сделать что-то вроде Multiplex, а затем получить (матричное) усиление, которое переставляет ваши векторные элементы в правильном порядке.

Или, так как вы пишете завод самостоятельно, вы можете просто сделать второй выходной порт, который содержит векторную информацию, необходимую для визуализации. Такой подход используется, например, в https://github.com/RobotLocomotion/drake/blob/master/examples/rimless_wheel/rimless_wheel.h#L31