How to make pendulum swing due to gravity in Drake

Question

I have been trying to simulate a simple pendulum that can swing due to gravity after setting the initial angle. I am now able to create the urdf, and create a slider that will set the angle of the pendulum arm. But I am still facing trouble when trying to get the pendulum to move. I have been basing my code on the code present in the "Running a simple simulation" section in https://deepnote.com/project/Authoring-a-Multibody-Simulation-jnoKyVLkS5CYUgHG3ASsBA/%2Fauthor_multibody_sim.ipynb.

This is the code I have so far:

import os
from tempfile import mkdtemp

from pydrake.all import (
    AddMultibodyPlantSceneGraph,
    DiagramBuilder,
    Meshcat,
    MeshcatVisualizerCpp,
    Parser,
    Simulator,
)

from manipulation.meshcat_cpp_utils import (
  StartMeshcat, MeshcatJointSlidersThatPublish)

meshcat = StartMeshcat("https://b8cf4725-e938-4907-9172-f08ccc4873ab.deepnoteproject.com")

tmp_dir = mkdtemp()
model_file = os.path.join(tmp_dir, "test_model.urdf")

model_text = f"""\
<?xml version="1.0"?>
<robot name="materials">

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>

  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>

  <joint name="world_2_base" type="fixed">
    <parent link="world"/>
    <child link="base"/>
     <origin xyz="0 0 0.5"/>
  </joint>

 <link name="base">
    <visual>
      <geometry>
        <box size="0.5 0.5 0.1"/>
      </geometry>
    </visual>
    <inertial>
      <mass value="1"/>
    </inertial>
  </link>

  <joint name="base" type="revolute">
    <parent link="base"/>
    <child link="arm"/>
    <axis xyz="0 1 0"/>
    <origin xyz="0 0 -0.05"/>
  </joint>

 <link name="arm">
    <visual>
      <geometry>
        <cylinder length="0.5" radius="0.01"/>
      </geometry>
      <material name="blue"/>
      <origin xyz="0 0 -0.25"/>
    </visual>
    <inertial>
      <mass value="1"/>
    </inertial>
  </link>

  <joint name="base2" type="fixed">
    <parent link="arm"/>
    <child link="ball"/>
    <origin xyz="0 0 -0.5"/>
  </joint>

 <link name="ball">
    <visual>
      <geometry>
        <sphere radius="0.1"/>
      </geometry>
      <material name="green"/>
      <origin xyz="0 0 -0.01"/>
    </visual>
    <inertial>
      <mass value="10"/>
    </inertial>
  </link>

  

</robot>
"""

# Write temporary file.
with open(model_file, "w") as f:
    f.write(model_text)

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0001)

# Add pendulum model.
model = Parser(plant, scene_graph).AddModelFromFile(model_file)
# - Weld base at origin.
base_link = plant.GetBodyByName("base", model)
#plant.WeldFrames(plant.world_frame(), base_link.body_frame())

plant.Finalize()
#visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)

visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph,
                                                   meshcat)
diagram = builder.Build()
context = diagram.CreateDefaultContext()



sliders = MeshcatJointSlidersThatPublish(meshcat, plant, visualizer, context) 


pend = plant.GetBodyByName(f"base", model)
def pose_callback(context):
        pose = plant.EvalBodyPoseInWorld(context, pend)

sliders.Run(pose_callback)

simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)

#visualizer.StartRecording()
simulator.AdvanceTo(5.0)
#visualizer.PublishRecording()

simulator.AdvanceTo(5.0)

Please let me know what I am doing wrong. Thank you very much

Answer 1

I think you need to fix the pendulum input port to zero, you can put

diagram.get_input_port(0).FixValue(context, [0.])

after creating "context". This says "apply zero torque at the pendulum joint actuator". So the pendulum will swing passively.

Answer 2

Before finalizing the plant, could you try calling

plant.mutable_gravity_field().set_gravity_vector([0, 0, -9.8])

?

Answer 3

You'd need to specify a non-zero initial angle in the joint between the base and the arm to observe anything interesting. One way of doing this is

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01)

# Add pendulum model.
model = Parser(plant, scene_graph).AddModelFromFile(model_file)
plant.Finalize()
joint = plant.GetJointByName('base')
joint.set_default_angle(0.2)

visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat,
        MeshcatVisualizerParams(role=Role.kIllustration, prefix="visual"))
diagram = builder.Build()

simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)

visualizer.StartRecording()
simulator.AdvanceTo(5.0)
visualizer.PublishRecording()

To set the initial condition with the slider, try

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01)

# Add pendulum model.
model = Parser(plant, scene_graph).AddModelFromFile(model_file)
plant.Finalize()
visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat,
        MeshcatVisualizerParams(role=Role.kIllustration, prefix="visual"))
diagram = builder.Build()
simulator = Simulator(diagram)
# Get context of the diagram being simulated.
context = simulator.get_mutable_context()
# Set initial condition of the joint.
joint_slider = MeshcatJointSliders(meshcat, plant, context)
joint_slider.Run(visualizer, context)
# Run the simulation and record.
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
visualizer.StartRecording()
simulator.AdvanceTo(5.0)
visualizer.PublishRecording()