Search code examples
pythondrake

Pydrake: how to get a single sine wave out of sine.get_output_port(0).Eval(sine_context)? (currently generates 3 waves)

| drake 0.38.0 | python 3.7 | OS: ubuntu 20.04 | pycharm |

I want to use a sine wave (amplitude: 0.01, f: 2) as the joint torque input for the base of a kuka iiwa simulated robot. (i.e. I just want to see the base swivel back and forth to test some other code)

I used the Sine system builder to add a sine system to my diagram.

sine = builder.AddSystem(Sine(np.array([0.01]),np.array([2]),np.array([0])))
context = diagram.CreateDefaultContext()
sine_context = sine.GetMyMutableContextFromRoot(context)
## sine_context = sine.CreateDefaultContext() # exact same behavior occurs

...
qt = np.zeros(n_dof)
qt[0] = sine.get_output_port(0).Eval(sine_context)
plant.get_actuation_input_port().FixValue(plant_context, qt)

when I call the below (at some time t) and attempt to graph qt[0], I see three waves instead of one: sine, cosine, and something else out of phase. My understanding is that Sine()'s output port 0 = sine, 1 = cosine, 2 = -sine and that I must use qt[0] = sine.get_output_port(0).Eval(sine_context) to get the floating point value of sine at a given time t.

simulator = Simulator(diagram, context)
simulator.set_target_realtime_rate(1.0)
# Advance the simulation slightly to initialize the GUI
simulator.AdvanceTo(0.01)
...
ml = momentum_log(5000, plant)
simulator.set_monitor(partial(momentum_log.monitor, ml))
while meshcat.GetButtonClicks(stop_sim_button) < 1:
      t = simulator.get_context().get_time()
      sine_context.SetTime(t)
      qt[0] = sine.get_output_port(0).Eval(sine_context)
      plant.get_actuation_input_port().FixValue(plant_context, qt)
      plotp(t,qt[0],0.05)
      simulator.AdvanceTo(t + 1.0)

ml.save()
plotp(t,qt[0],0.05)
plt.show()

What am I missing?

Edit: Please see below graph for clarification

Graph with mutlicolored dots shows the progression of a sine wave, cosine wave, and other, out of-phase wave from 0 - 50 seconds. Amplitude of the waves is 0.01 per the settings used.

Solution

  • The plot looks correct. When you sample sin(x) every 2 radians, that's what it should look like:

    import matplotlib.pyplot as plt
from math import sin

amplitude = 0.01
frequency = 2.0
t = list(range(0, 50))
y = [amplitude * sin(frequency * x) for x in t]
plt.plot(t, y, 'ro')
plt.show()

    output plot