Pendulum with gear ratio and motor inertia behaves differently than symbolic vector system from reflected inertia notebook excersise

[marekjg]

Hi, I'm trying to use Pendulum.urdf from Drake and modify it so it has 1kg mass and length 1m with massless link. Here's my code:

import numpy as np
from pydrake.all import (
    DiagramBuilder,
    Parser,
    AddMultibodyPlantSceneGraph,
    MeshcatVisualizer,
    Simulator,
    StartMeshcat,
    PidController,
    ConstantVectorSource,
    Adder,
    VectorLogSink,
    Gain
)

from manipulation.utils import RenderDiagram

def animation_demo(q_d=np.pi):
    builder = DiagramBuilder()
    logger = builder.AddSystem(VectorLogSink(2))
    
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
    urdf_path = "file:///home/mgrzeg/Downloads/Pendulum.urdf"
    pendulum = Parser(plant).AddModelsFromUrl(urdf_path)[0]
    plant.Finalize()
    
    kp, ki, kd = [5, 2, 1]
    pid_controller = builder.AddSystem(PidController([kp], [ki], [kd]))
    desired_state = builder.AddSystem(ConstantVectorSource([q_d, 0.0]))
    gain = builder.AddSystem(Gain(1, 1)) #Tried to scale by 160 but didn't work
    
    builder.Connect(desired_state.get_output_port(0), pid_controller.get_input_port(1))
    builder.Connect(plant.get_state_output_port(pendulum), pid_controller.get_input_port(0))
    builder.Connect(pid_controller.get_output_port(0), gain.get_input_port(0))
    builder.Connect(gain.get_output_port(0), plant.get_actuation_input_port(pendulum))
    builder.Connect(plant.get_state_output_port(pendulum), logger.get_input_port(0))
    
    # Visualizer
    MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
    
    diagram = builder.Build()
    context = diagram.CreateDefaultContext()
    
    plant_context = plant.GetMyMutableContextFromRoot(context)

    joint = plant.GetJointByName("theta")
    joint.set_angle(plant_context, 0.0)
    joint.set_angular_rate(plant_context, 0.0)
    
    simulator = Simulator(diagram, context)
    simulator.set_target_realtime_rate(1.0)
    
    # Animate in Meshcat
    meshcat.StartRecording()
    simulator.AdvanceTo(20.0)
    meshcat.StopRecording()
    meshcat.PublishRecording()
    return simulator, logger.FindLog(context)

simulator, log = animation_demo()
from matplotlib import pyplot as plt
plt.plot(log.data()[0].flatten())

I've tried to replicate reflected inertia notebook. Dynamics without gear reduction and motor inertia looks the same when I plot it. However, when I modify actuator:

  <transmission name="elbow_trans" type="SimpleTransmission">
    <joint name="theta"/>
    <actuator name="tau">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
      <drake:gear_ratio value="160.0"/>
      <drake:rotor_inertia value="0.000346"/>
    </actuator>
  </transmission>

the system is unstable:

What I think works is to scale gains by 160:

kp, ki, kd = [5 * 160, 2 * 160, 1 * 160]

and the behaviour looks similar but I wonder if that's the right approach:

[RussTedrake]

fyi @bernhardpg -- for visibility.