I would like to simulate system like "vibrating table" with linear displacement.
I studied several examples from tutorials:
table_sdf = """<?xml version="1.0"?>
<sdf version="1.7">
<model name="table_top">
<link name="table_link">
<visual name="visual">
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.55 1.1 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.9 0.8 0.7 1.0</diffuse>
</material>
</visual>
<collision name="collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.55 1.1 0.05</size>
</box>
</geometry>
</collision>
</link>
<frame name="table_center">
<pose relative_to="table_link">0 0 0 0 0 0</pose>
</frame>
</model>
</sdf>
"""
def create_scene(sim_time_step):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(
builder, time_step=sim_time_step)
parser = Parser(plant)
# Loading models
parser.SetAutoRenaming(True)
parser.AddModelsFromString(table_sdf, "sdf")
# Adding some objects in scene
num_of_parts = 3
objects_instance = []
for i in range(num_of_parts):
objects_instance.append(parser.AddModels(url="package://drake/manipulation/models/ycb/sdf/005_tomato_soup_can.sdf")[0])
table_frame = plant.GetFrameByName("table_center")
plant.WeldFrames(plant.world_frame(), table_frame)
# Finalize the plant after loading the scene.
plant.Finalize()
# We use the default context to calculate the transformation of the table
# in world frame but this is NOT the context the Diagram consumes.
plant_context = plant.CreateDefaultContext()
# Define the orientaion of added objects
objects = []
X_TablePart = []
X_WorldPart = []
for i in range(num_of_parts):
objects.append(plant.GetBodyByName("base_link_soup", model_instance=objects_instance[i]))
X_WorldTable = table_frame.CalcPoseInWorld(plant_context)
X_TablePart.append(RigidTransform(
RollPitchYaw(np.asarray([random.uniform(0,180), random.uniform(0,180), random.uniform(0,180)]) * np.pi / 180),
p=[random.uniform(-0.2, 0.2), random.uniform(-0.2, 0.2), 0.3+random.uniform(-0.05, 0.05)]))
X_WorldPart.append(X_WorldTable.multiply(X_TablePart[i]))
plant.SetDefaultFreeBodyPose(objects[i], X_WorldPart[i])
# Add visualization to see the geometries.
AddDefaultVisualization(builder=builder, meshcat=meshcat)
diagram = builder.Build()
return diagram
def initialize_simulation(diagram):
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.)
return simulator
def run_simulation(sim_time_step, sim_time):
diagram = create_scene(sim_time_step)
simulator = initialize_simulation(diagram)
meshcat.StartRecording()
simulator.AdvanceTo(sim_time)
meshcat.PublishRecording()
meshcat = StartMeshcat()
# Run the simulation with a small time step. Try gradually increasing it!
run_simulation(sim_time_step=0.0001, sim_time=2)
def MakeGripperFrames(X_WG):
Initial_position = RigidTransform([0., 0., 0.])
Final_position = RigidTransform([0.5, 0., 0.])
X_WG["one"] = Initial_position
X_WG["two"] = Final_position
X_WG["three"] = Initial_position
X_WG["four"] = Final_position
X_WG["five"] = Initial_position
# Now let's set the timing
times = {"zero": 0}
times["one"] = times["zero"] + 2 * 2
times["two"] = times["zero"] + 2 * 3
times["three"] = times["zero"] + 2 * 4
times["four"] = times["zero"] + 2 * 5
times["five"] = times["zero"] + 2 * 6
return X_WG, times
def MakeGripperPoseTrajectory(X_G, times):
"""
Constructs a gripper position trajectory from the plan "sketch".
"""
sample_times = []
poses = []
for name in [
"zero",
"one",
"two",
"three",
"four",
"five",
]:
sample_times.append(times[name]) # вывести их через принт
poses.append(X_G[name])
return PiecewisePose.MakeLinear(sample_times, poses)
class GripperTrajectoriesToPosition(LeafSystem):
def __init__(self, plant, traj_p_G, traj_R_G):
LeafSystem.__init__(self)
self.plant = plant
self.plant_context = plant.CreateDefaultContext()
self.table_body = plant.GetBodyByName("table_link")
self.traj_p_G = traj_p_G
self.traj_R_G = traj_R_G
self.W = plant.world_frame()
self.DeclareVectorOutputPort(
"position", plant.num_positions(), self.CalcPositionOutput
)
def CalcPositionOutput(self, context, output):
t = context.get_time()
X_G = RigidTransform(
Quaternion(self.traj_R_G.value(t)), self.traj_p_G.value(t)
)
self.plant.SetFreeBodyPose(self.plant_context, self.table_body, X_G)
output.SetFromVector(self.plant.GetPositions(self.plant_context))
def visualize_pick_and_place_trajectory(traj_p_G, traj_R_G):
builder = DiagramBuilder()
# Note: Don't use AddMultibodyPlantSceneGraph because we are only using
# MultibodyPlant for parsing, then wiring directly to SceneGraph.
scene_graph = builder.AddSystem(SceneGraph())
plant = MultibodyPlant(time_step=0.0001)
plant.RegisterAsSourceForSceneGraph(scene_graph)
parser = Parser(plant, scene_graph)
object_1 = parser.AddModelsFromString(table_sdf, "sdf")[0]
plant.Finalize()
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()),
)
traj_to_position = builder.AddSystem(
GripperTrajectoriesToPosition(
plant, traj_p_G, traj_R_G
)
)
builder.Connect(
traj_to_position.get_output_port(), to_pose.get_input_port()
)
MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(2.)
meshcat.StartRecording(set_visualizations_while_recording=False)
simulator.AdvanceTo(traj_p_G.end_time())
meshcat.PublishRecording()
# Start the visualizer.
meshcat = StartMeshcat()
X_G = {
"zero": RigidTransform([0, 0, 0.])
}
X_G, times = MakeGripperFrames(X_G)
traj_X_G = MakeGripperPoseTrajectory(X_G, times)
traj_p_G = traj_X_G.get_position_trajectory()
traj_R_G = traj_X_G.get_orientation_trajectory()
visualize_pick_and_place_trajectory(traj_p_G, traj_R_G)
And I don't understand how to combine these two solutions together.
If it's possible, could you get some advices about consctruct this system in Drake? I tried to add some block of codes in first solution and vice versa - it didn't work.
My full code here in two notebooks (DeepNote):DeepNote-project
@Russ Tedrake, thank you for your answers and I solved my task with your help and added some ideas else. I would like to write here these ideas and I hope this helps someone else in the future. Perhaps I used some tools differently than how it was originally intended in Drake, I will be glad to know how to do it correctly!
First, my goal was to get a model containing:
As a mechanism with several degrees of freedom, I took a double pendulum (DP), and as a free object, a can of soup from YCB-library.
DP:
dp_sdf = """<?xml version='1.0' ?>
<sdf version='1.6'>
<model name='double_pendulum_with_base'>
<link name='base'>
<inertial>
<mass>100</mass>
</inertial>
<visual name='vis_plate_on_ground'>
<pose>0 0 0.01 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.8</radius>
<length>0.02</length>
</cylinder>
</geometry>
</visual>
<visual name='vis_pole'>
<pose>-0.275 0 1.1 0 0 0</pose>
<geometry>
<box>
<size>0.2 0.2 2.2</size>
</box>
</geometry>
</visual>
<collision name='col_plate_on_ground'>
<pose>0 0 0.01 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.8</radius>
<length>0.02</length>
</cylinder>
</geometry>
</collision>
<collision name='col_pole'>
<pose>-0.275 0 1.1 0 0 0</pose>
<geometry>
<box>
<size>0.2 0.2 2.2</size>
</box>
</geometry>
</collision>
</link>
<!-- upper link, length 1, angle -90 degrees -->
<link name='upper_link'>
<pose>0 0 2.1 0 0 0</pose>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
<mass>30.0</mass>
</inertial>
<visual name='vis_upper_joint'>
<pose>-0.05 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
</visual>
<visual name='vis_lower_joint'>
<pose>0 0 1.0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</geometry>
</visual>
<visual name='vis_cylinder'>
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
</visual>
<collision name='col_upper_joint'>
<pose>-0.05 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
</collision>
<collision name='col_lower_joint'>
<pose>0 0 1.0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
<collision name='col_cylinder'>
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
</collision>
</link>
<!-- lower link, length 1, angle ~-120 degrees more -->
<link name='lower_link'>
<pose>0.25 0.0 3.1 0 0 0</pose>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
<mass>30.0</mass>
</inertial>
<visual name='vis_lower_joint'>
<pose>0 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.3</length>
</cylinder>
</geometry>
</visual>
<visual name='vis_cylinder'>
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
</visual>
<collision name='col_lower_joint'>
<pose>0 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.3</length>
</cylinder>
</geometry>
</collision>
<collision name='col_cylinder'>
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
</collision>
</link>
<!-- pin joint for upper link, at origin of upper link -->
<joint name='upper_joint' type='revolute'>
<parent>base</parent>
<child>upper_link</child>
<axis>
<xyz>1.0 0 0</xyz>
<use_parent_model_frame>true</use_parent_model_frame>
<limit>
<effort>0.0</effort>
</limit>
</axis>
</joint>
<!-- pin joint for lower link, at origin of child link -->
<joint name='lower_joint' type='revolute'>
<parent>upper_link</parent>
<child>lower_link</child>
<axis>
<xyz>1.0 0 0</xyz>
<use_parent_model_frame>true</use_parent_model_frame>
<limit>
<effort>0.0</effort>
</limit>
</axis>
</joint>
<frame name="dp_base">
<pose relative_to="base">0 0 0 0 0 0</pose>
</frame>
</model>
</sdf>
"""
So,
sim_step = 0.0001 # Simulation step
sim_time = 10 # Simulation time
num_of_actuated_joints = 2 # Number of actuated joints
num_of_objects = 2 # Number of free objects
def MakeTrajectory(joint_name):
# There are only two desired positions for every joints becauce I'd like to get periodic motion
if joint_name == "lower_joint":
first_pos = np.array([0]) # This corresponds to 0 deg
second_pos = np.array([0.7]) # This corresponds to ~40 deg (given in radians)
elif joint_name == "upper_joint":
first_pos = np.array([0])
second_pos = np.array([1.4])
else:
first_pos = np.array([0])
second_pos = np.array([0])
times = np.array([0, 2, 4, 6, 8, 10]) # Timestamps for positions
# Set the desired positions/trajectory through special class by defining two first points of trajectory
traj_command = PiecewisePolynomial.FirstOrderHold(
[times[0], times[1]],
np.hstack([[first_pos], [first_pos]]),
)
# Add additional positions
traj_command.AppendFirstOrderSegment(times[2], second_pos)
traj_command.AppendFirstOrderSegment(times[3], first_pos)
traj_command.AppendFirstOrderSegment(times[4], second_pos)
traj_command.AppendFirstOrderSegment(times[5], first_pos)
return traj_command
# Getting desired trajectories for all of two joints of DP
traj_JT1_command = MakeTrajectory(joint_name="lower_joint")
traj_JT2_command = MakeTrajectory(joint_name="upper_joint")
class TrajectoriesToPosition(LeafSystem):
def __init__(self, traj_JT1, traj_JT2):
LeafSystem.__init__(self)
self.traj_JT1_command = traj_JT1
self.traj_JT2_command = traj_JT2
# Becauce this system is source, we need to define output port, whose output positions
# will be desired positions for joints.
# The size of port are joints*2 because I use PID controller (it has position and velocity for each input)
# But I set here only positions
self.DeclareVectorOutputPort(
"joint_angles", num_of_actuated_joints*2, self.CalcPositionOutput
)
def CalcPositionOutput(self, context, output):
t = context.get_time()
JT1 = self.traj_JT1_command.value(t)[0, 0]
JT2 = self.traj_JT2_command.value(t)[0, 0]
output.SetFromVector(np.array([JT1, JT2, 0, 0]))
meshcat = StartMeshcat() # Creating visualiser
# Define the coefficients of PID controller
Kp = np.ones(num_of_actuated_joints) * 4000.0
Ki = np.ones(num_of_actuated_joints) * 1
Kd = np.ones(num_of_actuated_joints) * 400
builder = DiagramBuilder()
# Scene graph determines the position of bodies relative to each other
scene_graph = builder.AddSystem(SceneGraph())
scene_graph.set_name("scene_graph")
# As I understand, Multibody plant is a physics engine of drake-simulator
dp = builder.AddSystem(MultibodyPlant(sim_step))
dp.set_name("dp_plant")
dp.RegisterAsSourceForSceneGraph(scene_graph)
parser = Parser(dp)
# Add double pendulum in scene
dp_model_instance_index = parser.AddModelsFromString(dp_sdf, "sdf")[0]
# Adding some objects in scene
parser.SetAutoRenaming(True) # This need because I add some identical objects in scene
objects_instance = []
objects_name = []
objects_pose = []
for i in range(num_of_objects):
objects_instance.append(
parser.AddModels(
url="package://drake/manipulation/models/ycb/sdf/005_tomato_soup_can.sdf"
)[0]
)
objects_name.append(
dp.GetBodyByName("base_link_soup", objects_instance[i]
)
)
# Define the positions of objects above. I decided to use random for this goal
objects_pose.append(
RigidTransform(
RollPitchYaw(
np.asarray(
[random.uniform(0,180), random.uniform(0,180), random.uniform(0,180)]) * np.pi / 180),
p=[random.uniform(-0.2, 0.2), random.uniform(-0.2, 0.2), 0.3+random.uniform(-0.05, 0.05)])
)
# This frame is double-pendulum static base and this frame is defined in sdf-decription of DP above
root_frame = dp.GetFrameByName("base")
# Determining the position of the fixed base of the DP relative to the world coordinate system
X_0 = RigidTransform(
RollPitchYaw(
np.array([0, 0, 0]) * np.pi / 180),
p=[[0], [0], [0]]
)
# Add a frame that will be associated with the world coordinate system, but will be of type FixedOffsetFrame
# I tried to weld DP base and dp.world_frame() directly, but it didn't work
world_frame_joint = dp.AddFrame(
FixedOffsetFrame(
"world_joint_frame", dp.world_frame(),
X_0
)
)
# Weld the DP base to the world coordinate system.
dp.AddJoint(
WeldJoint(
name="weld_base",
frame_on_parent_F=world_frame_joint,
frame_on_child_M=root_frame,
X_FM=X_0
)
)
# Add actuators to joints
# Interesting points: if firstly define actuator for lower_joint in didn't work. Why??
dp.AddJointActuator("JT2", dp.GetJointByName("upper_joint"))
dp.AddJointActuator("JT1", dp.GetJointByName("lower_joint"))
# Because all of objects and joints are added, we can finalize phycics engine.
dp.Finalize()
# Add PID controller for 2 joints
# 11 port (double_pendulum_with_base_state - port) - because if I connect dp.get_state_output_port it didn't work:
# controller input port size is 4 (q1_pos, q2_pos, q1_vel, q2_vel),
# but size of dp.get_state_output_port is 4 + 7 * number of free object (7 = 3 (XYZ) + 4 (quaternion))
# Question: Is it possible to pass the port incompletely to the PID without states of free objects?
pid = builder.AddSystem(PidController(Kp, Ki, Kd))
builder.Connect(
dp.get_output_port(11),
pid.get_input_port_estimated_state()
)
builder.Connect(
pid.get_output_port_control(),
dp.get_actuation_input_port()
)
# Add system as source of desired position
desired_base_source = builder.AddSystem(
TrajectoriesToPosition(
traj_JT1=traj_JT1_command, traj_JT2=traj_JT2_command
)
)
# Everything seems clear here
builder.Connect(
desired_base_source.get_output_port(),
pid.get_input_port_desired_state()
)
# If interesting why we connect these ports below I reccomend
# to read https://drake.guzhaoyuan.com/ about the interatcion of scene graph and multibody plant
builder.Connect(
dp.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(dp.get_source_id())
)
builder.Connect(
scene_graph.get_query_output_port(), dp.get_geometry_query_input_port()
)
# Set initial positions of free objects added above
for i in range(num_of_objects):
dp.SetDefaultFreeBodyPose(objects_name[i], objects_pose[i])
MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
# Creating and getting context to set the initial positions of actuated joints
diagram_context = diagram.CreateDefaultContext()
dp_context = dp.GetMyContextFromRoot(simulator.get_mutable_context())
positions = np.ndarray(num_of_actuated_joints)
positions[0] = 0 * np.pi/180
positions[1] = 0 * np.pi/180
dp.SetPositions(dp_context, dp_model_instance_index, positions)
simulator.Initialize()
simulator.set_target_realtime_rate(1.)
meshcat.StartRecording(set_visualizations_while_recording=False)
simulator.AdvanceTo(sim_time)
meshcat.PublishRecording()
SVG(
pydot.graph_from_dot_data(diagram.GetGraphvizString(max_depth=1))[0].create_svg()
)
PS: This code is not entirely clean and of course there is something to improve and optimize. I will be glad to receive any comments and suggestions for improvement!