def _add_trajectory_source(self, traj):
# Create the trajectory source
source = self.builder.AddSystem(TrajectorySource(traj))
pos2pose = self.builder.AddSystem(MultibodyPositionToGeometryPose(self.plant, input_multibody_state=True))
# Wire the source to the scene graph
self.builder.Connect(source.get_output_port(0), pos2pose.get_input_port())
self.builder.Connect(pos2pose.get_output_port(), self.scenegraph.get_source_pose_port(self.plant.get_source_id()))
def animate(plant, x_trajectory):
builder = DiagramBuilder()
source = builder.AddSystem(TrajectorySource(x_trajectory))
builder.AddSystem(scene_graph)
pos_to_pose = builder.AddSystem(
MultibodyPositionToGeometryPose(plant, input_multibody_state=True))
builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port())
builder.Connect(pos_to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2, 2],
ylim=[-1.25, 2],
ax=ax[0]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
simulator = Simulator(builder.Build())
simulator.AdvanceTo(x_trajectory.end_time())
# 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)
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert (result.is_success())
x_trajectory = dircol.ReconstructStateTrajectory(result)
builder = DiagramBuilder()
source = builder.AddSystem(TrajectorySource(x_trajectory))
scene_graph = builder.AddSystem(SceneGraph())
AcrobotGeometry.AddToBuilder(builder, source.get_output_port(0), scene_graph)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
simulator = Simulator(builder.Build())
simulator.AdvanceTo(x_trajectory.end_time())
u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100)
u_lookup = np.vectorize(u_trajectory.value)
u_values = u_lookup(times)
plt.figure()
# interpolate state values for animation
time_breaks_opt = np.array([sum(h_opt[:t]) for t in range(T+1)])
x_opt_poly = PiecewisePolynomial.FirstOrderHold(time_breaks_opt, x_opt.T)
# parse urdf with scene graph
compass_gait = MultibodyPlant(time_step=0)
scene_graph = SceneGraph()
compass_gait.RegisterAsSourceForSceneGraph(scene_graph)
file_name = FindResource('models/compass_gait_limit_cycle.urdf')
Parser(compass_gait).AddModelFromFile(file_name)
compass_gait.Finalize()
# build block diagram and drive system state with
# the trajectory from the optimization problem
builder = DiagramBuilder()
source = builder.AddSystem(TrajectorySource(x_opt_poly))
builder.AddSystem(scene_graph)
pos_to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(compass_gait, input_multibody_state=True))
builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port())
builder.Connect(pos_to_pose.get_output_port(), scene_graph.get_source_pose_port(compass_gait.get_source_id()))
# add visualizer
xlim = [-.75, 1.]
ylim = [-.2, 1.5]
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph, xlim=xlim, ylim=ylim, show=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0))
simulator = Simulator(builder.Build())
# generate and display animation
visualizer.start_recording()
simulator.AdvanceTo(x_opt_poly.end_time())
def makeDiagram(n_quadrotors,
n_balls,
use_visualizer=False,
trajectory_u=None,
trajectory_x=None,
trajectory_K=None):
builder = DiagramBuilder()
# Setup quadrotor plants and controllers
quadrotor_plants = []
quadrotor_controllers = []
for i in range(n_quadrotors):
new_quad = Quadrotor2D(n_quadrotors=n_quadrotors - 1, n_balls=n_balls)
new_quad.set_name('quad_' + str(i))
plant = builder.AddSystem(new_quad)
quadrotor_plants.append(plant)
# Setup ball plants
ball_plants = []
for i in range(n_balls):
new_ball = Ball2D(n_quadrotors=n_quadrotors, n_balls=n_balls - 1)
new_ball.set_name('ball_' + str(i))
plant = builder.AddSystem(new_ball)
ball_plants.append(plant)
# Connect all plants so that each object (quadrotor or ball) has access to all other object states as inputs
for i in range(n_quadrotors):
for j in range(n_quadrotors):
if i == j:
continue
k = j if j < i else j - 1
builder.Connect(quadrotor_plants[j].get_output_port(0),
quadrotor_plants[i].GetInputPort('quad_' + str(k)))
for j in range(n_balls):
builder.Connect(ball_plants[j].get_output_port(0),
quadrotor_plants[i].GetInputPort('ball_' + str(j)))
for i in range(n_balls):
for j in range(n_quadrotors):
builder.Connect(quadrotor_plants[j].get_output_port(0),
ball_plants[i].GetInputPort('quad_' + str(j)))
for j in range(n_balls):
if i == j:
continue
k = j if j < i else j - 1
builder.Connect(ball_plants[j].get_output_port(0),
ball_plants[i].GetInputPort('ball_' + str(k)))
# Setup visualization
if use_visualizer:
visualizer = builder.AddSystem(
Visualizer(n_quadrotors=n_quadrotors, n_balls=n_balls))
visualizer.set_name('visualizer')
for i in range(n_quadrotors):
builder.Connect(quadrotor_plants[i].get_output_port(0),
visualizer.get_input_port(i))
for i in range(n_balls):
builder.Connect(ball_plants[i].get_output_port(0),
visualizer.get_input_port(n_quadrotors + i))
# Setup trajectory source
if trajectory_x is not None and trajectory_u is not None and trajectory_K is not None:
demulti_u = builder.AddSystem(Demultiplexer(2 * n_quadrotors, 2))
demulti_u.set_name('feedforward input')
demulti_x = builder.AddSystem(Demultiplexer(6 * n_quadrotors, 6))
demulti_x.set_name('reference trajectory')
demulti_K = builder.AddSystem(Demultiplexer(12 * n_quadrotors, 12))
demulti_K.set_name('time-varying K')
for i in range(n_quadrotors):
ltv_lqr = builder.AddSystem(LTVController(6, 2))
ltv_lqr.set_name('LTV LQR ' + str(i))
builder.Connect(demulti_x.get_output_port(i),
ltv_lqr.get_input_port(0))
builder.Connect(quadrotor_plants[i].get_output_port(0),
ltv_lqr.get_input_port(1))
builder.Connect(demulti_u.get_output_port(i),
ltv_lqr.get_input_port(2))
builder.Connect(demulti_K.get_output_port(i),
ltv_lqr.get_input_port(3))
builder.Connect(ltv_lqr.get_output_port(0),
quadrotor_plants[i].get_input_port(0))
source_u = builder.AddSystem(TrajectorySource(trajectory_u))
source_u.set_name('source feedforward input trajectory')
source_x = builder.AddSystem(TrajectorySource(trajectory_x))
source_x.set_name('source reference trajectory')
demulti_source_x = builder.AddSystem(
Demultiplexer([6 * n_quadrotors, 4 * n_balls]))
demulti_source_x.set_name('quad and ball trajectories')
source_K = builder.AddSystem(TrajectorySource(trajectory_K))
source_K.set_name('source time-varying K')
builder.Connect(source_u.get_output_port(0),
demulti_u.get_input_port(0))
builder.Connect(source_x.get_output_port(0),
demulti_source_x.get_input_port(0))
builder.Connect(demulti_source_x.get_output_port(0),
demulti_x.get_input_port(0))
builder.Connect(source_K.get_output_port(0),
demulti_K.get_input_port(0))
else:
demulti_u = builder.AddSystem(Demultiplexer(2 * n_quadrotors, 2))
demulti_u.set_name('quadrotor input')
for i in range(n_quadrotors):
builder.Connect(demulti_u.get_output_port(i),
quadrotor_plants[i].get_input_port(0))
builder.ExportInput(demulti_u.get_input_port(0))
diagram = builder.Build()
return diagram
def BuildAndSimulateTrajectory(
q_traj,
g_traj,
get_gripper_controller,
T_world_objectInitial,
T_world_targetBin,
zmq_url,
time_step,
include_target_bin=True,
include_hoop=False,
manipuland="foam"
):
"""Simulate trajectory for manipulation station.
@param q_traj: Trajectory class used to initialize TrajectorySource for joints.
@param g_traj: Trajectory class used to initialize TrajectorySource for gripper.
"""
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=time_step)) #1e-3 or 1e-4 probably
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_target_bin:
parser.AddModelFromFile("sdfs/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()
# iiwa joint trajectory - predetermined trajectory
q_traj_system = builder.AddSystem(TrajectorySource(q_traj))
builder.Connect(q_traj_system.get_output_port(),
station.GetInputPort("iiwa_position"))
# gripper - closed loop controller
gctlr = builder.AddSystem(get_gripper_controller(station_plant))
gctlr.set_name("GripperControllerUsingIiwaState")
builder.Connect(station.GetOutputPort("iiwa_position_measured"), gctlr.GetInputPort("iiwa_position"))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), gctlr.GetInputPort("iiwa_velocity"))
builder.Connect(gctlr.get_output_port(), station.GetInputPort("wsg_position"))
loggers = dict(
state=builder.AddSystem(SignalLogger(31)),
v_est=builder.AddSystem(SignalLogger(7))
)
builder.Connect(station.GetOutputPort("plant_continuous_state"),
loggers["state"].get_input_port())
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
loggers["v_est"].get_input_port())
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=True,
frames_to_draw={"gripper":{"body"}},
zmq_url=zmq_url)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
return simulator, station_plant, meshcat, loggers
h_opt = npzfile['h_opt'] t = np.cumsum(h_opt) # Add a zero at the start for t t = np.hstack(([0], t)) T = len(t) - 1 q_opt = npzfile['q_opt'] qd_opt = npzfile['qd_opt'] # Also get the initial state q_opt_0 = q_opt[0, :] # get the nominal control input as well for comparison at the end u_opt = npzfile['u_opt'] # Extract the flipper trajectory q_opt_flipper = q_opt[:, [0, 2, 4]] q_opt_poly = PiecewisePolynomial.FirstOrderHold(t, q_opt_flipper.T) control_source_q = builder.AddSystem(TrajectorySource(q_opt_poly)) # Extract the flipper velocity qd_opt_flipper = qd_opt[:, [0, 2, 4]] qd_opt_poly = PiecewisePolynomial.FirstOrderHold(t, qd_opt_flipper.T) control_source_v = builder.AddSystem(TrajectorySource(qd_opt_poly)) # Make an inverse dynamics controller for tracking the trajectory # We need to make a new plant that has only the flipper, since the pancake # is underactuated flipper_only_plant = MultibodyPlant(time_step=0) file_name = './models/pancake_flipper_only.urdf' Parser(flipper_only_plant).AddModelFromFile(file_name) flipper_only_plant.Finalize() # And we can make an inverse dynamics controller based on that plant, # with some specified PID gains Kp = np.array([1, 10, 10])
X_G = {
"initial":
plant.EvalBodyPoseInWorld(temp_plant_context, plant.GetBodyByName("body"))
}
X_G, times = make_gripper_frames(X_G, X_O)
print(
f"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute."
)
# Make the trajectories
traj_p_G = make_gripper_position_trajectory(X_G, times)
traj_v_G = traj_p_G.MakeDerivative()
traj_R_G = make_gripper_orientation_trajectory(X_G, times)
traj_w_G = traj_R_G.MakeDerivative()
v_G_source = builder.AddSystem(TrajectorySource(traj_v_G))
v_G_source.set_name("v_WG")
w_G_source = builder.AddSystem(TrajectorySource(traj_w_G))
w_G_source.set_name("omega_WG")
controller = builder.AddSystem(PseudoInverseController(plant))
controller.set_name("PseudoInverseController")
builder.Connect(v_G_source.get_output_port(), controller.GetInputPort("v_WG"))
builder.Connect(w_G_source.get_output_port(),
controller.GetInputPort("omega_WG"))
integrator = builder.AddSystem(Integrator(7))
integrator.set_name("integrator")
builder.Connect(controller.get_output_port(), integrator.get_input_port())
builder.Connect(integrator.get_output_port(),
station.GetInputPort("iiwa_position"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed, world_frame_1, tree_1)
tree_2 = RigidBodyTree()
AddFlatTerrainToWorld(tree_2, 100, 10)
world_frame_2 = RigidBodyFrame("world_frame", tree_1.world(), [0, 0, 0], [0, 0, 0])
AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed, world_frame_2, tree_2)
builder = DiagramBuilder()
# input trajectory
tspan = [0., TBD]
ts = np.linspace(tspan[0], tspan[-1], TBD)
q_des = TBD
q_traj = PiecewisePolynomial.Cubic(ts, q_des, np.zeros(7), np.zeros(7))
x_init = np.vstack((q_traj.value(tspan[0]),q_traj.derivative(1).value(tspan[0])))
source = builder.AddSystem(TrajectorySource(q_traj, output_derivative_order=1))
# controller
kp = 100 * np.ones(7)
kd = 10 * np.ones(7)
ki = 0 * np.ones(7)
controller = builder.AddSystem(InverseDynamicsController(robot=tree_1, kp=kp, ki=ki, kd=kd, has_reference_acceleration=False))
# plant
plant = RigidBodyPlant(tree_2, 0.0005)
kuka = builder.AddSystem(plant)
# visualizer
lcm = DrakeLcm()
visualizer = builder.AddSystem(DrakeVisualizer(tree=tree_1, lcm=lcm, enable_playback=True))
visualizer.set_publish_period(.001)