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())
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()
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()
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()
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'