child class of leafsystem generating sinusoidal signal

Question

I am trying to make a child class of LeafSystem whose output is sinusoidal and its derivative. I wrote the code and try to plot it but signal logger doesn't log correctly.

#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;
}

The output of the code is 2 0 : 0 0 0 : 0 0 END Whatever number I used in StepTo function, signal logger only cate 2 data whose sampled times are both 0.

Answer 1

The code looks good. Note that TrajectorySource does this almost exactly (and used SingleOutputVectorSource as a base class, which you might consider, too). The only problem is that you do not have anything telling the simulator that there is a reason to evaluate the output port. The logger block will pull on that for every publish event, but you haven't told the simulator to publish.

The solution is to call

simulator.set_publish_every_timestep(true)

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

If you want to further control the timestep of the integrator, you could set the parameters of the integrator (e.g. simalator.get_integerator), then calls like set_fixed_step_mode.