дочерний класс листовой системы, генерирующий синусоидальный сигнал - PullRequest
Новая
       8

дочерний класс листовой системы, генерирующий синусоидальный сигнал

0 голосов
/ 10 июня 2018

Я пытаюсь сделать дочерний класс LeafSystem, чей вывод является синусоидальным и его производным.Я написал код и попытался построить его, но регистратор сигналов не регистрируется правильно. 

#include "drake/systems/framework/leaf_system.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/signal_logger.h"
#include "drake/common/proto/call_python.h"

class Sinusoid : public drake::systems::LeafSystem<double>
{
public:
    Sinusoid (double tstart, double freq, double amp, double offset) :
        m_freq(freq), m_amp(amp), m_offset(offset), m_tstart(tstart) {
    this->DeclareVectorOutputPort(
            drake::systems::BasicVector<double>(2), &Sinusoid::output);
        }

private:
    void output(const drake::systems::Context<double>& c,             drake::systems::BasicVector<double>* output) const {
      double t(c.get_time());
      double tknot(t - m_tstart);
      if (t > m_tstart) {
          output->SetAtIndex(0, std::sin(tknot*m_freq +     m_offset)*m_amp);
          output->SetAtIndex(1, std::cos(tknot*m_freq +     m_offset)*m_amp*m_freq);
      } else {
          output->SetAtIndex(0, 0.0);
          output->SetAtIndex(1, 0.0);
      }
    }
    double m_freq{0.0}, m_amp{0.0}, m_offset{0.0}, m_tstart{0.0};
};

int main(int argc, char *argv[])
{
    // Add System and Connect
    drake::systems::DiagramBuilder<double> builder;
    auto system  = builder.AddSystem<Sinusoid>(1.0, 2.*M_PI*1., 3., 0.);
    auto logger = LogOutput(system->get_output_port(0), &builder);
    auto diagram = builder.Build();

    // Construct Simulator
    drake::systems::Simulator<double> simulator(*diagram);

    // Run simulation
    simulator.StepTo(100);

    // Plot with Python
    auto sample_time = logger->sample_times();
    auto sample_data = logger->data();
    std::cout << sample_time.size() << std::endl;
    for (int i = 0; i < sample_time.size(); ++i) {
        std::cout << sample_time(i) << " : " << sample_data(i, 0) << "      " << sample_data(i, 1) << std::endl;
        }

    std::cout << "END" << std::endl;
    return 0;
}

Вывод кода: 2 0 : 0 0 0 : 0 0 END Независимо от того, какое число я использовал в функции StepTo, регистратор сигналов только cate2 данных, время выборки которых равно 0.

1 Ответ

0 голосов
/ 11 июня 2018

Код выглядит хорошо.Обратите внимание, что TrajectorySource делает это почти точно (и использовал SingleOutputVectorSource в качестве базового класса, который вы также можете рассмотреть).Единственная проблема в том, что у вас нет ничего, говорящего симулятору, что есть причина для оценки выходного порта.Блок логгера включит это для каждого события публикации, но вы не сказали симулятору опубликовать.

Решение состоит в том, чтобы вызвать 

simulator.set_publish_every_timestep(true)

http://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_simulator.html#aef1dc6aeb821503379ab1dd8c6044562

Если вы хотите дополнительно контролировать временной шаг интегратора, вы можете установить параметры интегратора (например, simalator.get_integerator), затем вызывает как set_fixed_step_mode .

...