def experiment(start_state, force_schedule, sleeptime=None):
global experiment_count
experiment_count += 1
if experiment_count % 10 == 0:
print(f"{experiment_count} experiments run so far...")
builder = DiagramBuilder()
mbp, sg = make_mbp(builder)
mbp.Finalize()
DrakeVisualizer.AddToBuilder(builder, sg, lcm=global_lcm_ftw)
thrusters = add_thrusters(builder, mbp)
breaks = [0]
for _, t in force_schedule:
breaks.append(breaks[-1] + t)
forces = np.array([f for f, t in force_schedule] + [force_schedule[-1][0]])
force_traj = PiecewisePolynomial.ZeroOrderHold(breaks, forces.transpose())
controller = builder.AddSystem(TrajectorySource(force_traj))
builder.Connect(controller.get_output_port(),
thrusters.get_command_input_port())
diagram = builder.Build()
simulator = Simulator(diagram)
mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context())
mbp.SetPositionsAndVelocities(mbp_context, start_state)
for step in range(int(1000 * breaks[-1])):
simulator.AdvanceTo(0.001 * step)
mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context())
if sleeptime: time.sleep(sleeptime)
return mbp.GetPositionsAndVelocities(
diagram.GetSubsystemContext(mbp, simulator.get_context()))
def buildTrajectorySource(ts, knots):
#good_steps = np.diff(np.hstack([ts_raw, np.array(ts_raw[-1])])) >= 0.001
#ts = ts_raw[good_steps]
#knots = knots_raw[:, good_steps]
ppt = PiecewisePolynomial.FirstOrderHold(ts, knots)
return TrajectorySource(trajectory=ppt,
output_derivative_order=0,
zero_derivatives_beyond_limits=True)
def test_trajectory_source(self):
ppt = PiecewisePolynomial.FirstOrderHold(
[0., 1.], [[2., 3.], [2., 1.]])
system = TrajectorySource(trajectory=ppt,
output_derivative_order=0,
zero_derivatives_beyond_limits=True)
context = system.CreateDefaultContext()
output = system.AllocateOutput()
def mytest(input, expected):
context.SetTime(input)
system.CalcOutput(context, output)
self.assertTrue(np.allclose(output.get_vector_data(
0).CopyToVector(), expected))
mytest(0.0, (2.0, 2.0))
mytest(0.5, (2.5, 1.5))
mytest(1.0, (3.0, 1.0))
assert kp.shape[0] == num_joint_kuka
return kp, ki, kd
iiwa_kp,iiwa_ki,iiwa_kd = kuka_PID_params()
controller = builder.AddSystem(RbtInverseDynamicsController(rigid_body_tree, iiwa_kp, iiwa_ki, iiwa_kd, False))
builder.Connect(robot_plant.get_continuous_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
robot_plant.get_actuation_input_port())
traj_pp = MakeControlledKukaPlan()
traj_src = builder.AddSystem(TrajectorySource(traj_pp, 1)) #outputs q + v
traj_src.set_name("trajectory_source")
builder.Connect(traj_src.get_output_port(0),
controller.get_input_port(1))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(robot_plant,diagram_context)
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(1)
simulator.Initialize()
simulator.get_mutable_integrator().set_target_accuracy(1e-3)