def __init__(self,
builder,
dt=5e-4,
N=150,
params=None,
trj_decay=0.7,
x_w_cov=1e-5,
door_angle_ref=1.0,
visualize=False):
self.plant_derivs = MultibodyPlant(time_step=dt)
parser = Parser(self.plant_derivs)
self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs,
parser,
params=params)
self.plant_derivs.Finalize()
self.plant_derivs_context = self.plant_derivs.CreateDefaultContext()
self.plant_derivs.get_actuation_input_port().FixValue(
self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.])
null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.plant_derivs.GetInputPort("applied_generalized_force").FixValue(
self.plant_derivs_context, null_force)
self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=dt)
parser = Parser(self.plant, scene_graph)
self.iiwa, self.hinge, self.bushing = self.add_models(self.plant,
parser,
params=params)
self.plant.Finalize(
) # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor.
self.meshcat = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url=zmq_url)
self.sym_derivs = False # If system should use symbolic derivatives; if false, autodiff
self.custom_sim = False # If rollouts should be gathered with sys.dyn() calls
nq = self.plant.num_positions()
nv = self.plant.num_velocities()
self.n_x = nq + nv
self.n_u = self.plant.num_actuators()
self.n_y = self.plant.get_state_output_port(self.iiwa).size()
self.N = N
self.dt = dt
self.decay = trj_decay
self.V = 1e-2 * np.ones(self.n_y)
self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv)))
self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv)))
self.x_w_cov = x_w_cov
self.door_angle_ref = door_angle_ref
self.q0 = np.array(
[-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
#self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
self.x0 = np.concatenate((self.q0, np.zeros(nv)))
self.door_index = None
self.phi = {}
def test_thigh_torque_return_type(self):
"""Verify the signature of ChooseThighTorque"""
from hopper_2d import Hopper2dController
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.0005))
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.Finalize()
controller = Hopper2dController(plant, desired_lateral_velocity=0.0)
x0 = np.zeros(10)
x0[1] = 4. # in air
x0[4] = 0.5 # feasible leg length
x0[5] = 0.1 # initial speed
torquedes = controller.ChooseThighTorque(x0)
self.assertIsInstance(torquedes, float,
"ChooseThighTorque returned a type other than "\
"float for X0 = %s, desired_lateral_velocity = %f" %
(np.array_str(x0), controller.desired_lateral_velocity))
# Try from another desired velocity
controller.desired_lateral_velocity = -1.0
torquedes = controller.ChooseThighTorque(x0)
self.assertIsInstance(torquedes, float,
"ChooseThighTorque returned a type other than "\
"float for X0 = %s, desired_lateral_velocity = %f" %
(np.array_str(x0), controller.desired_lateral_velocity))
def compile_scene_tree_clearance_geometry_to_mbp_and_sg(
scene_tree, timestep=0.001, alpha=0.25):
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=timestep))
parser = Parser(mbp)
parser.package_map().PopulateFromEnvironment("ROS_PACKAGE_PATH")
world_body = mbp.world_body()
free_body_poses = []
# For generating colors.
node_class_to_color_dict = {}
cmap = plt.cm.get_cmap('jet')
cmap_counter = 0.
for node in scene_tree.nodes:
if node.tf is not None and node.physics_geometry_info is not None:
# Don't have to do anything if this does not introduce geometry.
sanity_check_node_tf_and_physics_geom_info(node)
phys_geom_info = node.physics_geometry_info
has_clearance_geometry = len(phys_geom_info.clearance_geometry) > 0
if not has_clearance_geometry:
continue
# Add a body for this node and register the clearance geometry.
# TODO(gizatt) This tree body index is built in to disambiguate names.
# But I forsee a name-to-stuff resolution crisis when inference time comes...
# this might get resolved by the solution to that.
body = mbp.AddRigidBody(name=node.name,
M_BBo_B=phys_geom_info.spatial_inertia)
tf = torch_tf_to_drake_tf(node.tf)
mbp.SetDefaultFreeBodyPose(body, tf)
# Pick out a color for this class.
node_type_string = node.__class__.__name__
if node_type_string in node_class_to_color_dict.keys():
color = node_class_to_color_dict[node_type_string]
else:
color = list(cmap(cmap_counter))
color[3] = alpha
node_class_to_color_dict[node_type_string] = color
cmap_counter = np.fmod(cmap_counter + np.pi * 2., 1.)
# Handle adding primitive geometry by adding it all to one
# mbp.
if len(phys_geom_info.clearance_geometry) > 0:
for k, (tf, geometry) in enumerate(
phys_geom_info.clearance_geometry):
mbp.RegisterCollisionGeometry(
body=body,
X_BG=torch_tf_to_drake_tf(tf),
shape=geometry,
name=node.name + "_col_%03d" % k,
coulomb_friction=default_friction)
mbp.RegisterVisualGeometry(body=body,
X_BG=torch_tf_to_drake_tf(tf),
shape=geometry,
name=node.name +
"_vis_%03d" % k,
diffuse_color=color)
return builder, mbp, scene_graph
def runPendulumExample(args):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant)
parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
plant.Finalize()
pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
Tview=Tview,
xlim=[-1.2, 1.2],
ylim=[-1.2, 1.2]))
builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
# Fix the input port to zero.
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.FixInputPort(plant.get_actuation_input_port().get_index(),
np.zeros(plant.num_actuators()))
plant_context.SetContinuousState([0.5, 0.1])
simulator.StepTo(args.duration)
def CreateIiwaControllerPlant():
"""creates plant that includes only the robot and gripper, used for controllers."""
robot_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf")
gripper_sdf_path = FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf")
sim_timestep = 1e-3
plant_robot = MultibodyPlant(sim_timestep)
parser = Parser(plant=plant_robot)
parser.AddModelFromFile(robot_sdf_path)
parser.AddModelFromFile(gripper_sdf_path)
plant_robot.WeldFrames(
A=plant_robot.world_frame(),
B=plant_robot.GetFrameByName("iiwa_link_0"))
plant_robot.WeldFrames(
A=plant_robot.GetFrameByName("iiwa_link_7"),
B=plant_robot.GetFrameByName("body"),
X_AB=RigidTransform(RollPitchYaw(np.pi/2, 0, np.pi/2), np.array([0, 0, 0.114]))
)
plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0])
plant_robot.Finalize()
link_frame_indices = []
for i in range(8):
link_frame_indices.append(
plant_robot.GetFrameByName("iiwa_link_" + str(i)).index())
return plant_robot, link_frame_indices
def AddPlanarBinAndSimpleBox(plant,
mass=1.0,
mu=1.0,
width=0.2,
depth=0.05,
height=0.3):
parser = Parser(plant)
bin = parser.AddModelFromFile(FindResource("models/planar_bin.sdf"))
plant.WeldFrames(
plant.world_frame(), plant.GetFrameByName("bin_base", bin),
RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0),
[0, 0, -0.015]))
planar_joint_frame = plant.AddFrame(
FixedOffsetFrame(
"planar_joint_frame", plant.world_frame(),
RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2))))
# TODO(russt): make this a *random* box?
# TODO(russt): move random box to a shared py file.
box_instance = AddShape(plant, Box(width, depth, height), "box", mass, mu)
box_joint = plant.AddJoint(
PlanarJoint("box", planar_joint_frame,
plant.GetFrameByName("box", box_instance)))
box_joint.set_position_limits([-.5, -.1, -np.pi], [.5, .3, np.pi])
box_joint.set_velocity_limits([-2, -2, -2], [2, 2, 2])
box_joint.set_default_translation([0, depth / 2.0])
return box_instance
def build_block_diagram(actuators_off=False,
desired_lateral_velocity=0.0,
desired_height=3.0,
print_period=0.0):
builder = DiagramBuilder()
# Build the plant
plant = builder.AddSystem(MultibodyPlant(0.0005))
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
# Build the robot
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.Finalize()
plant.set_name('plant')
# Create a logger to log at 30hz
state_dim = plant.num_positions() + plant.num_velocities()
state_log = builder.AddSystem(SignalLogger(state_dim))
state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_state_output_port(), state_log.get_input_port(0))
state_log.set_name('state_log')
# The controller
controller = builder.AddSystem(
Hopper2dController(plant,
desired_lateral_velocity=desired_lateral_velocity,
desired_height=desired_height,
actuators_off=actuators_off,
print_period=print_period))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
controller.set_name('controller')
# Create visualizer
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-1, 10],
ylim=[-.2, 4.5],
show=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
visualizer.set_name('visualizer')
diagram = builder.Build()
return diagram
def setup_manipulation_station(T_world_objectInitial, zmq_url, T_world_targetBin, manipuland="foam", include_bin=True, include_hoop=False):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=1e-3))
station.SetupClutterClearingStation()
if manipuland is "foam":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/061_foam_brick.sdf",
T_world_objectInitial)
elif manipuland is "ball":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/sphere.sdf",
T_world_objectInitial)
elif manipuland is "bball":
station.AddManipulandFromFile(
"drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery
T_world_objectInitial)
elif manipuland is "rod":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/rod.sdf",
T_world_objectInitial)
station_plant = station.get_multibody_plant()
parser = Parser(station_plant)
if include_bin:
parser.AddModelFromFile("extra_bin.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin)
if include_hoop:
parser.AddModelFromFile("sdfs/hoop.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin)
station.Finalize()
frames_to_draw = {"gripper": {"body"}}
meshcat = None
if zmq_url is not None:
meshcat = ConnectMeshcatVisualizer(builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"),
delete_prefix_on_load=False,
frames_to_draw=frames_to_draw,
zmq_url=zmq_url)
diagram = builder.Build()
plant = station.get_multibody_plant()
context = plant.CreateDefaultContext()
gripper = plant.GetBodyByName("body")
initial_pose = plant.EvalBodyPoseInWorld(context, gripper)
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.01)
return initial_pose, meshcat
def RunSimulation(self, real_time_rate=1.0):
'''
Here we test using the NNSystem in a Simulator to drive
an acrobot.
'''
sdf_file = "assets/acrobot.sdf"
urdf_file = "assets/acrobot.urdf"
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant=plant, scene_graph=scene_graph)
parser.AddModelFromFile(sdf_file)
plant.Finalize(scene_graph)
# Add
nn_system = NNSystem(self.pytorch_nn_object)
builder.AddSystem(nn_system)
# NN -> plant
builder.Connect(nn_system.NN_out_output_port,
plant.get_actuation_input_port())
# plant -> NN
builder.Connect(plant.get_continuous_state_output_port(),
nn_system.NN_in_input_port)
# Add meshcat visualizer
meshcat = MeshcatVisualizer(scene_graph)
builder.AddSystem(meshcat)
# builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.GetInputPort("lcm_visualization"))
# build diagram
diagram = builder.Build()
meshcat.load()
# time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
# context = diagram.GetMutableSubsystemContext(
# self.station, simulator.get_mutable_context())
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 5.
simulator.StepTo(sim_duration)
print("stepping complete")
def Simulate2dHopper(x0,
duration,
desired_lateral_velocity=0.0,
print_period=0.0):
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.0005))
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
# Build the plant
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
# Create a logger to log at 30hz
state_dim = plant.num_positions() + plant.num_velocities()
state_log = builder.AddSystem(SignalLogger(state_dim))
state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_continuous_state_output_port(),
state_log.get_input_port(0))
# The controller
controller = builder.AddSystem(
Hopper2dController(plant,
desired_lateral_velocity=desired_lateral_velocity,
print_period=print_period))
builder.Connect(plant.get_continuous_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
# The diagram
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
plant_context.get_mutable_discrete_state_vector().SetFromVector(x0)
simulator.StepTo(duration)
return plant, controller, state_log
def do_generation_and_simulation(sim_time=10):
vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000")
scene_tree, satisfied_clearance = rejection_sample_feasible_tree(num_attempts=1000)
scene_tree, satisfied_feasibility = project_tree_to_feasibility(scene_tree, num_attempts=3)
serialize_scene_tree_to_yamls_and_sdfs(scene_tree, package_name='save', package_dir=".", remove_directory=True)
# Draw generated tree in meshcat.
#draw_scene_tree_meshcat(scene_tree, alpha=1.0, node_sphere_size=0.1)
# Draw its clearance geometry for debugging.
#draw_clearance_geometry_meshcat(scene_tree, alpha=0.3)
# Simulate the resulting scene, with a PR2 for scale.
builder, mbp, scene_graph = compile_scene_tree_to_mbp_and_sg(
scene_tree, timestep=0.001)
# Add PR2 and shift it back in front of where I know the table will be.
parser = Parser(mbp)
pr2_model_path = "/home/gizatt/drake/build/install/share/drake/examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf"
parser.package_map().PopulateUpstreamToDrake(pr2_model_path);
pr2_model_id = parser.AddModelFromFile(
file_name=pr2_model_path, model_name="PR2_for_scale")
# Get the tf of the robot spawn node, and put the PR2 at that xy location.
robot_spawn_tf = scene_tree.find_nodes_by_type(RobotSpawnLocation)[0].tf.numpy()
mbp.GetJointByName("x", model_instance=pr2_model_id).set_default_translation(robot_spawn_tf[0, 3])
mbp.GetJointByName("y", model_instance=pr2_model_id).set_default_translation(robot_spawn_tf[1, 3])
mbp.Finalize()
visualizer = ConnectMeshcatVisualizer(builder, scene_graph,
zmq_url="default")
diagram = builder.Build()
diag_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(mbp, diag_context)
# Fix input port for PR2 to zero.
actuation_port = mbp.get_actuation_input_port(model_instance=pr2_model_id)
nu = mbp.num_actuated_dofs(model_instance=pr2_model_id)
actuation_port.FixValue(mbp_context, np.zeros(nu))
sim = Simulator(diagram, diag_context)
sim.set_target_realtime_rate(1.0)
if not satisfied_clearance:
print("WARNING: SCENE TREE NOT SATISFYING CLEARANCE")
if not satisfied_feasibility:
print("WARNING: SCENE TREE NOT SATISFYING FEASIBILITY, SIM MAY FAIL")
try:
sim.AdvanceTo(sim_time)
except RuntimeError as e:
print("Encountered error in sim: ", e)
def build_pancake_flipper_plant(builder):
'''Creates a pancake_flipper MultibodyPlant. Returns the plant and
corresponding scene graph.
Inputs:
builder -- a DiagramBuilder
Outputs:
the MultibodyPlant object and corresponding SceneGraph representing
the pancake flipper
'''
# Instantiate the pancake flipper plant and the scene graph.
# The scene graph is a container for the geometries of all
# the physical systems in our diagram
pancake_flipper, scene_graph = AddMultibodyPlantSceneGraph(
builder,
time_step=0.0 # discrete update period, or zero for continuous systems
)
# parse the urdf and populate the plant
urdf_path = './models/pancake_flipper_massless_links_nofriction.urdf'
Parser(pancake_flipper).AddModelFromFile(urdf_path)
# Finalize the plant so we can use it
pancake_flipper.Finalize()
# Return the plant and scene graph, as promised
return pancake_flipper, scene_graph
def build_iiwa7_plant():
plant = MultibodyPlant(1e-3)
parser = Parser(plant=plant)
iiwa_drake_path = (
"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf"
)
iiwa_path = FindResourceOrThrow(iiwa_drake_path)
robot_model = parser.AddModelFromFile(iiwa_path)
# weld robot to world frame.
plant.WeldFrames(frame_on_parent_P=plant.world_frame(),
frame_on_child_C=plant.GetFrameByName("iiwa_link_0"),
X_PC=RigidTransform.Identity())
plant.Finalize()
return plant
def AddCameraBox(plant, X_WC, name="camera0", parent_frame=None):
# TODO(russt): could be smarter and increment the default camera name (by
# checking with the plant).
if not parent_frame:
parent_frame = plant.world_frame()
camera = Parser(plant).AddModelFromFile(
FindResource("models/camera_box.sdf"), name)
plant.WeldFrames(parent_frame, plant.GetFrameByName("base", camera), X_WC)
def kinematic_tree_example():
plant = MultibodyPlant(time_step=0.0)
parser = Parser(plant)
parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf"
))
parser.AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
g = Source(plant.GetTopologyGraphvizString())
print(g.format)
g.view()
def AddPlanarIiwa(plant):
urdf = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"planar_iiwa14_spheres_dense_elbow_collision.urdf")
parser = Parser(plant)
iiwa = parser.AddModelFromFile(urdf)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Set default positions:
q0 = [0.1, -1.2, 1.6]
index = 0
for joint_index in plant.GetJointIndices(iiwa):
joint = plant.get_mutable_joint(joint_index)
if isinstance(joint, RevoluteJoint):
joint.set_default_angle(q0[index])
index += 1
return iiwa
def AddIiwa(plant, collision_model="no_collision"):
sdf_path = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/iiwa7/"
f"iiwa7_{collision_model}.sdf")
parser = Parser(plant)
iiwa = parser.AddModelFromFile(sdf_path)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Set default positions:
q0 = [0.0, 0.1, 0, -1.2, 0, 1.6, 0]
index = 0
for joint_index in plant.GetJointIndices(iiwa):
joint = plant.get_mutable_joint(joint_index)
if isinstance(joint, RevoluteJoint):
joint.set_default_angle(q0[index])
index += 1
return iiwa
def setup_dot_diagram(builder, args):
''' Using an existing DiagramBuilder, adds a sim of the Dot
robot. Args comes from argparse.
The returned controller will need its first port connected to
a setpoint source.'''
print(
"TODO: load in servo calibration dict to a servo calibration object that gets shared"
)
with open(args.yaml_path, "r") as f:
config_dict = yaml.load(f, Loader=Loader)
sdf_path = os.path.join(os.path.dirname(args.yaml_path),
config_dict["sdf_path"])
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0005)
parser = Parser(plant)
model = parser.AddModelFromFile(sdf_path)
# Set initial pose floating above the ground.
plant.SetDefaultFreeBodyPose(plant.GetBodyByName("body"),
RigidTransform(p=[0., 0., 0.25]))
if args.welded:
plant.WeldFrames(plant.world_frame(),
plant.GetBodyByName("body").body_frame())
else:
add_ground(plant)
plant.Finalize()
controller = builder.AddSystem(ServoController(plant, config_dict))
# Fixed control-rate controller with a low pass filter on its torque output.
zoh = builder.AddSystem(
ZeroOrderHold(period_sec=0.001, vector_size=controller.n_servos))
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=0.02, size=controller.n_servos))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(1))
builder.Connect(controller.get_output_port(0), zoh.get_input_port(0))
builder.Connect(zoh.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
plant.get_actuation_input_port())
return plant, scene_graph, controller
def grasp_poses_example():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant, scene_graph)
grasp = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"
), "grasp")
brick = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
meshcat = ConnectMeshcatVisualizer(builder, scene_graph)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)
B_O = plant.GetBodyByName("base_link", brick)
X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)
B_Ggrasp = plant.GetBodyByName("body", grasp)
p_GgraspO = [0, 0.12, 0]
R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply(
RotationMatrix.MakeZRotation(np.pi / 2.0))
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
X_WGgrasp = X_WO.multiply(X_OGgrasp)
plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp)
# Open the fingers, too
plant.GetJointByName("left_finger_sliding_joint",
grasp).set_translation(plant_context, -0.054)
plant.GetJointByName("right_finger_sliding_joint",
grasp).set_translation(plant_context, 0.054)
meshcat.load()
diagram.Publish(context)
def AddTwoLinkIiwa(plant, q0=[0.1, -1.2]):
urdf = FindResource("models/two_link_iiwa14.urdf")
parser = Parser(plant)
parser.package_map().Add(
"iiwa_description",
os.path.dirname(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/package.xml")))
iiwa = parser.AddModelFromFile(urdf)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Set default positions:
index = 0
for joint_index in plant.GetJointIndices(iiwa):
joint = plant.get_mutable_joint(joint_index)
if isinstance(joint, RevoluteJoint):
joint.set_default_angle(q0[index])
index += 1
return iiwa
def num_positions_velocities_example():
plant = MultibodyPlant(time_step=0.0)
Parser(plant).AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
context = plant.CreateDefaultContext()
print(context)
print(f"plant.num_positions() = {plant.num_positions()}")
print(f"plant.num_velocities() = {plant.num_velocities()}")
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("sdf_path", help="path to sdf")
parser.add_argument("--interactive", action='store_true')
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
plant = MultibodyPlant(time_step=0.01)
plant.RegisterAsSourceForSceneGraph(scene_graph)
parser = Parser(plant)
model = parser.AddModelFromFile(args.sdf_path)
plant.Finalize()
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=scene_graph.get_query_output_port(),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.interactive:
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
else:
q0 = plant.GetPositions(plant.CreateDefaultContext())
sliders = builder.AddSystem(ConstantVectorSource(q0))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(
to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1E6)
def main():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
path = subprocess.check_output([
'venv/share/drake/common/resource_tool',
'-print_resource_path',
'drake/examples/multibody/cart_pole/cart_pole.sdf',
]).strip()
Parser(plant).AddModelFromFile(path)
plant.Finalize()
# Add to visualizer.
DrakeVisualizer.AddToBuilder(builder, scene_graph)
# Add controller.
controller = builder.AddSystem(BalancingLQR(plant))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
diagram = builder.Build()
# Set up a simulator to run this diagram.
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
# Simulate.
duration = 8.0
for i in range(5):
initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4)
print(f"Iteration: {i}. Initial: {initial_state}")
context.SetContinuousState(initial_state)
context.SetTime(0.0)
simulator.Initialize()
simulator.AdvanceTo(duration)
def AddWsg(plant, iiwa_model_instance, roll=np.pi / 2.0, welded=False):
parser = Parser(plant)
if welded:
parser.package_map().Add(
"wsg_50_description",
os.path.dirname(
FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/package.xml"))
)
gripper = parser.AddModelFromFile(
FindResource("models/schunk_wsg_50_welded_fingers.sdf"), "gripper")
else:
gripper = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/"
"wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf"))
X_7G = RigidTransform(RollPitchYaw(np.pi / 2.0, 0, roll), [0, 0, 0.114])
plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa_model_instance),
plant.GetFrameByName("body", gripper), X_7G)
return gripper
Then the problem will be reduced to the control of the pendulum only.
"""
# think of the builder as the construction site of our block diagram
builder = DiagramBuilder()
# instantiate the vibrating pendulum and the scene graph
# the scene graph is a container for the geometries of all the physical systems in our diagram
vibrating_pendulum, scene_graph = AddMultibodyPlantSceneGraph(
builder,
time_step=0. # discrete update period , set to zero since system is continuous
)
# parse the urdf and populate the vibrating pendulum
urdf_path = FindResource('models/vibrating_pendulum.urdf')
Parser(vibrating_pendulum).AddModelFromFile(urdf_path)
vibrating_pendulum.Finalize()
"""## Write the Controller
In this section we define two controllers:
1. An inner controller that makes the base oscillate with the harmonic motion. We wrote this for you.
2. The outer controller to make the pendulum spin at constant velocity. You will write part of this.
The final diagram will have the following structure:
"""
# this controller enforces the harmonic motion to the base
class InnerController(VectorSystem):
# unstable equilibrium point
x_star = [0, np.pi, 0, 0]
# weight matrices for the lqr controller
Q = np.eye(4)
R = np.eye(1)
"""Then we construct the block diagram with the cart-pole in closed loop with the LQR controller."""
# start construction site of our block diagram
builder = DiagramBuilder()
# instantiate the cart-pole and the scene graph
cartpole, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
urdf_path = FindResource('models/undamped_cartpole.urdf')
Parser(cartpole).AddModelFromFile(urdf_path)
cartpole.Finalize()
# set the operating point (vertical unstable equilibrium)
context = cartpole.CreateDefaultContext()
context.get_mutable_continuous_state_vector().SetFromVector(x_star)
# fix the input port to zero and get its index for the lqr function
cartpole.get_actuation_input_port().FixValue(context, [0])
input_i = cartpole.get_actuation_input_port().get_index()
# synthesize lqr controller directly from
# the nonlinear system and the operating point
lqr = LinearQuadraticRegulator(cartpole, context, Q, R, input_port_index=input_i)
lqr = builder.AddSystem(lqr)
RigidTransform())
RegisterVisualAndCollisionGeometry(
mbp, ground_body,
RigidTransform(p=[0, 0, -0.5]),
ground_shape, "ground", np.array([0.5, 0.5, 0.5, 1.]),
CoulombFriction(0.9, 0.8))
# Figure out what YCB objects we have available to add.
ycb_object_dir = os.path.join(
getDrakePath(), "manipulation/models/ycb/sdf/")
ycb_object_sdfs = os.listdir(ycb_object_dir)
ycb_object_sdfs = [os.path.join(ycb_object_dir, path)
for path in ycb_object_sdfs]
# Add random objects to the scene.
parser = Parser(mbp, scene_graph)
n_objects = np.random.randint(1, 12)
obj_ids = []
for k in range(n_objects):
obj_i = np.random.randint(len(ycb_object_sdfs))
parser.AddModelFromFile(
file_name=ycb_object_sdfs[obj_i],
model_name="obj_ycb_%03d" % k)
obj_ids.append(k+2)
mbp.Finalize()
# Optional: set up a meshcat visualizer for the scene.
#meshcat = builder.AddSystem(
# MeshcatVisualizer(scene_graph, draw_period=0.0333))
#builder.Connect(scene_graph.get_pose_bundle_output_port(),
import argparse
import numpy as np
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser,
PlanarSceneGraphVisualizer, Simulator)
from underactuated import FindResource, SliderSystem
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
file_name = FindResource("cartpole/cartpole.urdf")
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.5, 2.5], ylim=[-1, 2.5]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, "Force", -30., 30.))
builder.Connect(torque_system.get_output_port(0),
plant.get_actuation_input_port())
from pydrake.all import (AddMultibodyPlantSceneGraph,
DiagramBuilder,
Parser,
Simulator)
from underactuated import FindResource, PlanarSceneGraphVisualizer
# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()
builder.ExportInput(plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.8, 2.8],
ylim=[-2.8, 2.8]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
# Set the initial conditions
context = simulator.get_mutable_context()
# state is (theta1, theta2, theta1dot, theta2dot)
context.SetContinuousState([1., 1., 0., 0.])
def drake_trajectory_generation(file_name):
global x_cmd_drake
global u_cmd_drake
print(file_name)
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
global dircol
dircol= DirectCollocation(
plant,
context,
num_time_samples=11,
minimum_timestep=0.1,
maximum_timestep=0.4,
input_port_index=plant.get_actuation_input_port().get_index())
dircol.AddEqualTimeIntervalsConstraints()
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
final_state = (0., math.pi, 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state())
R = 10 # Cost on input "effort".weight
u = dircol.input()
dircol.AddRunningCost(R * u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
[0., 4.], np.column_stack((initial_state, final_state))) # yapf: disable
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
dircol.AddConstraintToAllKnotPoints(dircol.input()[1] <= 0)
dircol.AddConstraintToAllKnotPoints(dircol.input()[1] >= 0)
global result
global u_values
result = Solve(dircol)
assert result.is_success()
#plotphase_portrait()
fig1, ax1 = plt.subplots()
u_trajectory = dircol.ReconstructInputTrajectory(result)
u_knots = np.hstack([
u_trajectory.value(t) for t in np.linspace(u_trajectory.start_time(),
u_trajectory.end_time(), 400)
])#here the u_knots now is 2x400
#u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 400)
#u_lookup = np.vectorize(u_trajectory.value)
#now we have ndarray of u_values with 400 points for 4 seconds w/ 100hz pub frequency
#u_values = u_lookup(times)
#ax1.plot(times, u_values)
ax1.plot(times, u_knots[0])
ax1.plot(times, u_knots[1])
ax1.set_xlabel("time (seconds)")
ax1.set_ylabel("force (Newtons)")
ax1.set_title(' Direct collocation for Cartpole ')
print('here2')
plt.show()
print('here3')
#x_knots = np.hstack([
# x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
# x_trajectory.end_time(), 100)
#])
x_trajectory = dircol.ReconstructStateTrajectory(result)
x_knots = np.hstack([
x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
x_trajectory.end_time(), 400)
])
print(x_trajectory.start_time())
print(x_trajectory.end_time())
fig, ax = plt.subplots(4,1,figsize=(8,8))
plt.subplots_adjust(wspace =0, hspace =0.4)
#plt.tight_layout(3)#adjust total space
ax[0].set_title('state of direct collocation for Cartpole')
ax[0].plot(x_knots[0, :], x_knots[2, :], linewidth=2, color='b', linestyle='-')
ax[0].set_xlabel("state_dot(theta1_dot and t|heta2_dot)")
ax[0].set_ylabel("state(theta1 and theta2)");
ax[0].plot(x_knots[1, :], x_knots[3, :],color='r',linewidth=2,linestyle='--')
ax[0].legend(('theta1&theta1dot','theta2&theta2dot'));
ax[1].set_title('input u(t) of direct collocation for Cartpole')
# ax[1].plot(times,u_values, 'g')
ax[1].plot(times, u_knots[0])
ax[1].plot(times, u_knots[1])
ax[1].legend(('input u(t)'))
ax[1].set_xlabel("time")
ax[1].set_ylabel("u(t)")
ax[1].legend(('x joint ','thetajoint'))
ax[1].set_title('input x(t) of direct collocation for Cartpole')
ax[2].plot(times, x_knots[0, :])
ax[2].set_xlabel("time")
ax[2].set_ylabel("x(t)")
ax[2].set_title('input theta(t) of direct collocation for Cartpole')
ax[3].set_title('input theta(t) of direct collocation for Cartpole')
ax[3].plot(times, x_knots[1, :])
ax[3].set_xlabel("time")
ax[3].set_ylabel("theta(t)")
print('here4')
plt.show()
print('here5')
x_cmd_drake=x_knots
#return x_knots[0, :]#u_values
# u_cmd_drake=u_values
u_cmd_drake=u_knots
