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 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 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)
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())
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.])
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()
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)
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)
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()
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())
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)
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)
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()