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)