How to make pendulum swing due to gravity in Drake - simulation

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

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()

Answer 2

Before finalizing the plant, could you try calling
plant.mutable_gravity_field().set_gravity_vector([0, 0, -9.8])
?

Answer 3

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.