def urdfViz(plant):
"""
Visualize the position/orientation of the vehicle along a trajectory using the PlanarRigidBodyVisualizer and
a basic .urdf file representing the vehicle, start, goal, and state constraints.
"""
tree = RigidBodyTree(FindResource("/notebooks/6832-code/ben_uav.urdf"),
FloatingBaseType.kRollPitchYaw)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-1, 15], ylim=[-0.5, 2.5])
tf = plant.ttraj[-1]
times = np.linspace(0, tf, 200)
# organize the relevant states for the visualizer
posn = np.zeros((13, len(times)))
for i, t in enumerate(times):
x = plant.xdtraj_poly.value(t)
u = plant.udtraj_poly.value(t)
posn[0:6, i] = 0
posn[6, i] = (x[0] - plant.xdtraj[:,0].min()) / 14 # x
posn[7, i] = 0 # z
posn[8, i] = x[1] # y
posn[9, i] = 0 # yz plane
posn[10, i] = np.pi / 2 - x[4] # pitch down
posn[11, i] = 0 # yaw
posn[12, i] = -u[1] # tail angle
test_poly = PiecewisePolynomial.FirstOrderHold(times, posn)
return vis.animate(test_poly, repeat=False)
def animate_cartpole(policy, duration=10.):
# Animate the resulting policy.
builder = DiagramBuilder()
tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf",
FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
plant_system = builder.AddSystem(plant)
# TODO(russt): add wrap-around logic to barycentric mesh
# (so the policy has it, too)
class WrapTheta(VectorSystem):
def __init__(self):
VectorSystem.__init__(self, 4, 4)
def _DoCalcVectorOutput(self, context, input, state, output):
output[:] = input
twoPI = 2.*math.pi
output[1] = output[1] - twoPI * math.floor(output[1] / twoPI)
wrap = builder.AddSystem(WrapTheta())
builder.Connect(plant_system.get_output_port(0), wrap.get_input_port(0))
vi_policy = builder.AddSystem(policy)
builder.Connect(wrap.get_output_port(0), vi_policy.get_input_port(0))
builder.Connect(vi_policy.get_output_port(0), plant_system.get_input_port(0))
logger = builder.AddSystem(SignalLogger(4))
logger._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(plant_system.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
state.SetFromVector([-1., math.pi-1, 1., -1.])
# Do the sim.
simulator.StepTo(duration)
# Visualize the result as a video.
vis = PlanarRigidBodyVisualizer(tree, xlim=[-12.5, 12.5], ylim=[-1, 2.5])
ani = vis.animate(logger, repeat=True)
# plt.show()
# Things added to get visualizations in an ipynb
plt.close(vis.fig)
HTML(ani.to_html5_video())
def playbackMotion(data1, data2, data3, data4, times):
data = np.concatenate((data2, data1, data4, data3), axis=0)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(Player(data, times))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
#print(robot.get_output_port(0).size())
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.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)
simulator.set_publish_every_time_step(True)
# Simulate for 10 seconds
simulator.StepTo(times[-1] + 0.5)
def ConstructVisualizer():
from underactuated import PlanarRigidBodyVisualizer
tree = RigidBodyTree()
AddModelInstancesFromSdfString(
open("raibert_hopper_2d.sdf", 'r').read(), FloatingBaseType.kFixed,
None, tree)
viz = PlanarRigidBodyVisualizer(tree, xlim=[-5, 5], ylim=[-5, 5])
viz.fig.set_size_inches(10, 5)
return viz
def simulateRobot(time, B, v_command):
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(RigidBodyPlant(tree))
controller = builder.AddSystem(DController(tree, B, v_command))
builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.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)
simulator.set_publish_every_time_step(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
def simulate_acrobot():
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
saturation = builder.AddSystem(Saturation(min_value=[-10], max_value=[10]))
builder.Connect(saturation.get_output_port(0), acrobot.get_input_port(0))
wrapangles = WrapToSystem(4)
wrapangles.set_interval(0, 0, 2. * math.pi)
wrapangles.set_interval(1, -math.pi, math.pi)
wrapto = builder.AddSystem(wrapangles)
builder.Connect(acrobot.get_output_port(0), wrapto.get_input_port(0))
controller = builder.AddSystem(BalancingLQR())
builder.Connect(wrapto.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
saturation.get_input_port(0))
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
parser = argparse.ArgumentParser()
parser.add_argument("-N",
"--trials",
type=int,
help="Number of trials to run.",
default=5)
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run each sim.",
default=4.0)
args = parser.parse_args()
print(AcrobotPlant)
for i in range(args.trials):
context.set_time(0.)
state.SetFromVector(UprightState().CopyToVector() +
0.05 * np.random.randn(4, ))
simulator.StepTo(args.duration)
params.set_slope(args.slope)
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
rimless_wheel = builder.AddSystem(RimlessWheel())
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,
xlim=[-8., 8.],
ylim=[-2., 3.],
figsize_multiplier=3))
builder.Connect(rimless_wheel.get_output_port(1), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
diagram.GetMutableSubsystemContext(
rimless_wheel, context).get_numeric_parameter(0).set_slope(args.slope)
context.SetAccuracy(1e-4)
context.SetContinuousState([args.initial_angle, args.initial_angular_velocity])
simulator.AdvanceTo(args.duration)
type=float,
help="Duration to run sim.",
default=20.0)
parser.add_argument("-Q",
"--initial_angle",
type=float,
help="Initial angle of the stance leg (in radians).",
default=0.0)
parser.add_argument("-V",
"--initial_angular_velocity",
type=float,
help="Initial angular velocity of the stance leg "
"(in radians/sec).",
default=5.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree, xlim=[-8., 8.], ylim=[-4., 4.]))
builder.Connect(rimless_wheel.get_output_port(1), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
simulator.get_mutable_context().set_accuracy(1e-4)
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
state.SetFromVector([args.initial_angle, args.initial_angular_velocity])
simulator.StepTo(args.duration)
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
hip_torque = builder.AddSystem(ConstantVectorSource([0.0]))
builder.Connect(hip_torque.get_output_port(0), compass_gait.get_input_port(0))
parser = argparse.ArgumentParser()
parser.add_argument("-T", "--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,
xlim=[-1., 5.],
ylim=[-1., 2.],
figsize_multiplier=2))
builder.Connect(compass_gait.get_output_port(1), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
context.SetAccuracy(1e-4)
context.SetContinuousState([0., 0., 0.4, -2.])
simulator.StepTo(args.duration)
type=float,
help="Gravitational constant (m/s^2).",
default=9.8)
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
controller = builder.AddSystem(Controller(tree, args.gravity))
builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree, xlim=[-2.8, 2.8], ylim=[-2.8, 2.8]))
builder.Connect(robot.get_output_port(0), 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)
simulator.set_publish_every_time_step(False)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
state.SetFromVector((1., 0., 0.2, 0.)) # (θ₁, θ₂, θ̇₁, θ̇₂)
# Simulate
simulator.StepTo(args.duration)
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
rimless_wheel = builder.AddSystem(RimlessWheel())
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
xlim=[-8., 8.],
ylim=[-2., 3.],
figsize_multiplier=3))
builder.Connect(rimless_wheel.get_output_port(1), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
diagram.GetMutableSubsystemContext(
rimless_wheel, context).get_numeric_parameter(0).set_slope(args.slope)
context.SetAccuracy(1e-4)
context.SetContinuousState([args.initial_angle, args.initial_angular_velocity])
simulator.StepTo(args.duration)
def getPredictedMotion(B, v_command, time):
#object_positions = object_positions + 0.1
#manipulator_positions = manipulator_positions + 0.1
#object_velocities = object_velocities + 0.1
#manipulator_velocities = manipulator_velocities + 0.1
#object_positions = object_positions[:, range(0,object_positions.shape[1],2)]
#manipulator_positions = manipulator_positions[:, range(0,manipulator_positions.shape[1],2)]
#object_velocities = object_velocities[:, range(0,object_velocities.shape[1],2)]
#manipulator_velocities = manipulator_velocities[:, range(0,manipulator_velocities.shape[1],2)]
#times = times[range(0,times.size,2)]
#import pdb; pdb.set_trace()
step = 0.01
A = 10 * np.eye(3)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
#robot = builder.AddSystem(RigidBodyPlant(tree))
robot = builder.AddSystem(
QuasiStaticRigidBodyPlant(tree, step, A, np.linalg.inv(B)))
controller = builder.AddSystem(QController(tree, v_command, step))
#builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.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)
simulator.set_publish_every_time_step(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_discrete_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
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)
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])
ani = vis.animate(x_trajectory, repeat=True)
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()
plt.plot(times, u_values)
plt.xlabel('time (seconds)')
plt.ylabel('force (Newtons)')
plt.show()
integrator.set_fixed_step_mode(True) integrator.set_maximum_step_size(0.001) #%% # simulate context = simulator.get_mutable_context() x0 = np.array([0., 0., 3., 0.]) # since the ZOH block at t=0 returns its internal state, I have to compute the first feedback "offline" state_c = context.get_mutable_continuous_state_vector() state_c.SetFromVector(x0) state_d = context.get_mutable_discrete_state_vector() u0 = np.empty(pwa.nu) controller._DoCalcVectorOutput(context, x0, None, u0) state_d.SetFromVector(u0) # run sim simulator.StepTo(3.) #%% # visualize viz = PlanarRigidBodyVisualizer(tree, xlim=[-1, 1], ylim=[-2.5, 3.]) viz.fig.set_size_inches(10, 5) ani = viz.animate(state_log, 30, repeat=True) plt.close(viz.fig) HTML(ani.to_html5_video()) # print 'hi Ash'
tree = RigidBodyTree(FindResource("cartpole/cartpole.urdf"),
FloatingBaseType.kFixed)
builder = DiagramBuilder()
cartpole = builder.AddSystem(RigidBodyPlant(tree))
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(
PlanarRigidBodyVisualizer(tree, xlim=[-2.5, 2.5], ylim=[-1, 2.5]))
builder.Connect(cartpole.get_output_port(0), 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), cartpole.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
state.SetFromVector([0., 1., 0., 0.])
simulator.StepTo(args.duration)
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
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)
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])
ani = vis.animate(x_trajectory, repeat=True)
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()
plt.plot(times, u_values)
plt.xlabel('time (seconds)')
plt.ylabel('force (Newtons)')
plt.show()
tree = RigidBodyTree(FindResource("cartpole/cartpole.urdf"),
FloatingBaseType.kFixed)
builder = DiagramBuilder()
cartpole = builder.AddSystem(RigidBodyPlant(tree))
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(PlanarRigidBodyVisualizer(tree,
xlim=[-2.5, 2.5],
ylim=[-1, 2.5]))
builder.Connect(cartpole.get_output_port(0), 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),
cartpole.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
context.SetContinuousState([0., 1., 0., 0.])
def test_enumeration_planner():
tree = RigidBodyTree()
AddModelInstanceFromUrdfFile("resources/cartpole_disturbance.urdf",
FloatingBaseType.kFixed,
None,
tree)
AddModelInstanceFromUrdfFile("resources/wall.urdf",
FloatingBaseType.kFixed,
None,
tree)
plant = RigidBodyPlant(tree)
context = plant.CreateDefaultContext()
#d = 1.
d = 2.
#d = 5.
#d = 10.
#d = 0.
'''
input_weight = 1.
goal_weight = 30.
goal_x = 1.
def running_cost(x, u, t):
return input_weight * (u[0]**2 + u[1]**2) + goal_weight * (x[0] - goal_x)**2
def final_cost(x, u, t):
return 0.
'''
input_weight = 1.
pole_upright_weight = 5.
#goal_weight = 50.
goal_weight = 30.
goal_x = 1.
def running_cost(x, u, t):
return input_weight * (u[0]**2 + u[1]**2) + pole_upright_weight * x[1]**2
def final_cost(x, u, t):
return goal_weight * (x[0] - goal_x)**2
def final_state_constraint(x):
pole_length = 0.5
wall_x = 2.
return x[0] + pole_length * sym.sin(x[1]) - wall_x
signed_dist_funcs = init_signed_dist_funcs()
print("Disturbance is {} N".format(d))
planner = Planner(plant, context, running_cost, final_cost, signed_dist_funcs, final_state_constraint)
initial_state = (0., 0., 0., 0.)
#tmin = 0.5
tmin = 4.
T = 8.
#dt = 0.1
#dc = 0.1
dt = 0.5
dc = 0.35
x_traj_nc, x_traj_c = planner.plan(initial_state, tmin, T, dt, dc, d)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-2.5, 2.5], ylim=[-1, 2.5])
ani = vis.animate(x_traj_nc, repeat=True)
ani.save('disturbance_{}N.mp4'.format(d))
plt.show()
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
builder = DiagramBuilder()
acrobot = builder.AddSystem(RigidBodyPlant(tree))
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(PlanarRigidBodyVisualizer(tree,
xlim=[-4., 4.],
ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, 'Torque', -5., 5.))
builder.Connect(torque_system.get_output_port(0),
acrobot.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
context.SetContinuousState([1., 0., 0., 0.])
def test_planner():
tree = RigidBodyTree()
# AddModelInstanceFromUrdfFile("resources/cartpole2.urdf",
# FloatingBaseType.kFixed,
# None,
# tree)
AddModelInstanceFromUrdfFile("resources/cartpole_disturbance.urdf",
FloatingBaseType.kFixed,
None,
tree)
AddModelInstanceFromUrdfFile("resources/wall.urdf",
FloatingBaseType.kFixed,
None,
tree)
plant = RigidBodyPlant(tree)
context = plant.CreateDefaultContext()
#d = 5.
#d = 0.
# d = 100
# context.FixInputPort(0, BasicVector([d, 0.]))
input_weight = 1.
goal_weight = 30.
#goal_weight = 5.
#goal_x = 2.
goal_x = 1.
def running_cost(x, u, t):
return input_weight * (u[0]**2 + u[1]**2) + goal_weight * (x[0] - goal_x)**2
def final_cost(x, u, t):
return 0.
signed_dist_funcs = init_signed_dist_funcs()
def final_state_constraint(x):
pole_length = 0.5
wall_x = 2.
return x[0] + pole_length * sym.sin(x[1]) - wall_x
#final_state_constraint = None
planner = Planner(plant, context, running_cost, final_cost, signed_dist_funcs, final_state_constraint)
#initial_state = (0., math.pi/2, 0., 0.)
initial_state = (0., 0., 0., 0.)
#final_state = (1., 3*math.pi/2., 0., 0.)
final_state = None
#final_state = (1.7, 0.64350111, 0.0, 0.0)
#final_state = (1.5, 1.57079633, 0.0, 0.0)
#duration_bounds = (3., 3.)
#duration_bounds = (8., 8.)
duration_bounds = (5., 5.)
d = 0
x_traj, total_cost = planner._solve_traj_opt(initial_state, final_state, duration_bounds, d, verbose=True)
# from pydrake.all import PiecewisePolynomial
# tmp = PiecewisePolynomial.FirstOrderHold([0., 1.],
# np.column_stack((initial_state,
# initial_state)))
# x_traj = x_traj + tmp
duration = x_traj.end_time() - x_traj.start_time()
print("Trajectory duration is {} s".format(duration))
vis = PlanarRigidBodyVisualizer(tree, xlim=[-2.5, 2.5], ylim=[-1, 2.5])
ani = vis.animate(x_traj, repeat=True)
ani.save('out.mp4')
plt.show()
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
xlim=[-1., 5.],
ylim=[-1., 2.],
figsize_multiplier=2))
builder.Connect(compass_gait.get_output_port(1), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
context.set_accuracy(1e-4)
context.SetContinuousState([0., 0., 0.4, -2.])
simulator.StepTo(args.duration)
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
result = Solve(dircol)
assert result.is_success()
x_trajectory = dircol.ReconstructStateTrajectory(result)
fig, ax = plt.subplots(2, 1)
vis = PlanarRigidBodyVisualizer(tree,
xlim=[-1.5, 1.5],
ylim=[-.5, 2],
ax=ax[0])
ani = vis.animate(x_trajectory, repeat=True)
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)
ax[1].plot(times, u_values)
ax[1].set_xlabel('time (seconds)')
ax[1].set_ylabel('force (Newtons)')
plt.show()