def Simulate2dRamone(x0, duration, desired_goal = 0.0, print_period = 0.0): builder = DiagramBuilder() tree = RigidBodyTree() AddModelInstanceFromUrdfFile("ramone_act.urdf", FloatingBaseType.kRollPitchYaw, None, tree) plant = builder.AddSystem(RigidBodyPlant(tree)) controller = builder.AddSystem( Ramone2dController(tree, desired_goal=desired_goal, print_period=print_period)) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) simulator.get_mutable_context().set_accuracy(1e-4) state = simulator.get_mutable_context().get_mutable_continuous_state_vector() state.SetFromVector(x0) simulator.StepTo(duration) return tree, controller, state_log, plant
def runPendulumExample(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort( plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.StepTo(args.duration)
def main(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3) body = add_body(plant, "welded_body") add_geometry(plant, body) plant.WeldFrames(plant.world_frame(), body.body_frame()) body = add_body(plant, "floating_body") add_geometry(plant, body) plant.SetDefaultFreeBodyPose(body, RigidTransform([1., 0, 1.])) plant.Finalize() ConnectDrakeVisualizer(builder, scene_graph) diagram = builder.Build() simulator = Simulator(diagram) context = simulator.get_context() # plant.SetFreeBodyPose(context, body, X_WB) # plant.SetFreeBodySpatialVelocity(body, V_WB, context) # Should look at, show 40sec to 50sec. # TODO(eric.cousineau): Without reset stats, it freezes? :( # simulator.AdvanceTo(40.) simulator.set_target_realtime_rate(100.) # simulator.ResetStatistics() dt = 0.1 while context.get_time() < 240.: simulator.AdvanceTo(context.get_time() + dt)
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 runPendulumExample(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort(plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.StepTo(args.duration)
def setup_manipulation_station(T_world_objectInitial, zmq_url, T_world_targetBin, manipuland="foam", include_bin=True, include_hoop=False): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=1e-3)) 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_bin: parser.AddModelFromFile("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() frames_to_draw = {"gripper": {"body"}} 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=False, frames_to_draw=frames_to_draw, zmq_url=zmq_url) diagram = builder.Build() plant = station.get_multibody_plant() context = plant.CreateDefaultContext() gripper = plant.GetBodyByName("body") initial_pose = plant.EvalBodyPoseInWorld(context, gripper) simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(0.01) return initial_pose, meshcat
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 RunSimulation(self, real_time_rate=1.0): ''' Here we test using the NNSystem in a Simulator to drive an acrobot. ''' sdf_file = "assets/acrobot.sdf" urdf_file = "assets/acrobot.urdf" builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant=plant, scene_graph=scene_graph) parser.AddModelFromFile(sdf_file) plant.Finalize(scene_graph) # Add nn_system = NNSystem(self.pytorch_nn_object) builder.AddSystem(nn_system) # NN -> plant builder.Connect(nn_system.NN_out_output_port, plant.get_actuation_input_port()) # plant -> NN builder.Connect(plant.get_continuous_state_output_port(), nn_system.NN_in_input_port) # Add meshcat visualizer meshcat = MeshcatVisualizer(scene_graph) builder.AddSystem(meshcat) # builder.Connect(scene_graph.GetOutputPort("lcm_visualization"), builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.GetInputPort("lcm_visualization")) # build diagram diagram = builder.Build() meshcat.load() # time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) # context = diagram.GetMutableSubsystemContext( # self.station, simulator.get_mutable_context()) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 5. simulator.StepTo(sim_duration) print("stepping complete")
def simulate_scene_tree(scene_tree, T, timestep=0.001, with_meshcat=False): builder, mbp, scene_graph = compile_scene_tree_to_mbp_and_sg( scene_tree, timestep=timestep) mbp.Finalize() if with_meshcat: visualizer = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url="default") diagram = builder.Build() diag_context = diagram.CreateDefaultContext() sim = Simulator(diagram) sim.set_target_realtime_rate(1.0) sim.AdvanceTo(T)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("-t1", default=0.05, help="Extend leg") parser.add_argument("-t2", default=0.5, help="Dwell at top") parser.add_argument("-t3", default=0.5, help="Contract leg") parser.add_argument("-t4", default=0.1, help="Wait at bottom") setup_argparse_for_setup_dot_diagram(parser) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() t1 = float(args.t1) t2 = float(args.t2) t3 = float(args.t3) t4 = float(args.t4) q_crouch = np.array([ 1600, 2100, 2000, 1600, 2100, 2000, 1400, 2100, 2000, 1400, 2100, 2000 ]) q_extend = np.array([ 1600, 1600, 2400, 1600, 1600, 2400, 1400, 1600, 2400, 1400, 1600, 2400 ]) breaks = np.cumsum([0., t1, t2, t3, t4]) samples = np.stack([q_crouch, q_extend, q_extend, q_crouch, q_crouch]).T trajectory = PiecewisePolynomial.FirstOrderHold(breaks, samples) builder = DiagramBuilder() plant, scene_graph, servo_controller = setup_dot_diagram(builder, args) trajectory_source = builder.AddSystem(TrajectoryLooper(trajectory)) builder.Connect(trajectory_source.get_output_port(0), servo_controller.get_input_port(0)) if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=scene_graph.get_query_output_port(), zmq_url=args.meshcat, open_browser=args.open_browser) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(1E6)
def jacobian_controller_example(): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation()) station.SetupClutterClearingStation() station.Finalize() controller = builder.AddSystem( PseudoInverseController(station.get_multibody_plant())) integrator = builder.AddSystem(Integrator(7)) 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"), controller.get_input_port()) meshcat = ConnectMeshcatVisualizer( builder, station.get_scene_graph(), output_port=station.GetOutputPort("pose_bundle")) diagram = builder.Build() simulator = Simulator(diagram) station_context = station.GetMyContextFromRoot( simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros((7, 1))) station.GetInputPort("wsg_position").FixValue(station_context, [0.1]) integrator.GetMyContextFromRoot(simulator.get_mutable_context( )).get_mutable_continuous_state_vector().SetFromVector( station.GetIiwaPosition(station_context)) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(0.01) return simulator
def runManipulationExample(args): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=0.005)) station.SetupDefaultStation() station.Finalize() plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() pose_bundle_output_port = station.GetOutputPort("pose_bundle") # Side-on view of the station. Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, Tview=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2], draw_period=0.1)) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the control inputs to zero. station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_position").FixValue( station_context, station.GetIiwaPosition(station_context)) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) station.GetInputPort("wsg_position").FixValue( station_context, np.zeros(1)) station.GetInputPort("wsg_force_limit").FixValue( station_context, [40.0]) simulator.StepTo(args.duration)
def main(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) path = subprocess.check_output([ 'venv/share/drake/common/resource_tool', '-print_resource_path', 'drake/examples/multibody/cart_pole/cart_pole.sdf', ]).strip() Parser(plant).AddModelFromFile(path) plant.Finalize() # Add to visualizer. DrakeVisualizer.AddToBuilder(builder, scene_graph) # Add controller. controller = builder.AddSystem(BalancingLQR(plant)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) diagram = builder.Build() # Set up a simulator to run this diagram. simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) context = simulator.get_mutable_context() # Simulate. duration = 8.0 for i in range(5): initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4) print(f"Iteration: {i}. Initial: {initial_state}") context.SetContinuousState(initial_state) context.SetTime(0.0) simulator.Initialize() simulator.AdvanceTo(duration)
def main(): parser = argparse.ArgumentParser(description=__doc__) setup_argparse_for_setup_dot_diagram(parser) parser.add_argument("--interactive", action='store_true') MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() q_init = np.array([ 1700, 2000, 2000, 1700, 2000, 2000, 1300, 2000, 2000, 1300, 2000, 2000 ]) builder = DiagramBuilder() plant, scene_graph, servo_controller = setup_dot_diagram(builder, args) if args.interactive: # Add sliders to set positions of the joints. sliders = builder.AddSystem(ServoSliders(servo_controller)) sliders.set_position(q_init) builder.Connect(sliders.get_output_port(0), servo_controller.get_input_port(0)) else: source = builder.AddSystem( ConstantVectorSource(np.zeros(servo_controller.nu))) builder.Connect(source.get_output_port(0), servo_controller.get_input_port(0)) if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=scene_graph.get_query_output_port(), zmq_url=args.meshcat, open_browser=args.open_browser) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(1E6)
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())
class Juggler: def __init__(self, kp=100, ki=1, kd=20, time_step=0.001, show_axis=False): """ Robotic Kuka IIWA juggler with paddle end effector Args: kp (int, optional): proportional gain. Defaults to 100. ki (int, optional): integral gain. Defaults to 1. kd (int, optional): derivative gain. Defaults to 20. time_step (float, optional): time step for internal manipulation station controller. Defaults to 0.001. show_axis (boolean, optional): whether or not to show the axis of reflection. """ self.time = 0 juggler_station = JugglerStation(kp=kp, ki=ki, kd=kd, time_step=time_step, show_axis=show_axis) self.builder = DiagramBuilder() self.station = self.builder.AddSystem(juggler_station.get_diagram()) self.position_log = [] self.velocity_log = [] self.visualizer = ConnectMeshcatVisualizer( self.builder, output_port=self.station.GetOutputPort("geometry_query"), zmq_url=zmq_url) self.plant = juggler_station.get_multibody_plant() # --------------------- ik_sys = self.builder.AddSystem(InverseKinematics(self.plant)) ik_sys.set_name("ik_sys") v_mirror = self.builder.AddSystem(VelocityMirror(self.plant)) v_mirror.set_name("v_mirror") w_tilt = self.builder.AddSystem(AngularVelocityTilt(self.plant)) w_tilt.set_name("w_tilt") integrator = self.builder.AddSystem(Integrator(7)) self.builder.Connect( self.station.GetOutputPort("iiwa_position_measured"), ik_sys.GetInputPort("iiwa_pos_measured")) self.builder.Connect(ik_sys.get_output_port(), integrator.get_input_port()) self.builder.Connect(integrator.get_output_port(), self.station.GetInputPort("iiwa_position")) self.builder.Connect( self.station.GetOutputPort("iiwa_position_measured"), w_tilt.GetInputPort("iiwa_pos_measured")) self.builder.Connect( self.station.GetOutputPort("iiwa_position_measured"), v_mirror.GetInputPort("iiwa_pos_measured")) self.builder.Connect( self.station.GetOutputPort("iiwa_velocity_estimated"), v_mirror.GetInputPort("iiwa_velocity_estimated")) self.builder.Connect( w_tilt.get_output_port(), ik_sys.GetInputPort("paddle_desired_angular_velocity")) self.builder.Connect(v_mirror.get_output_port(), ik_sys.GetInputPort("paddle_desired_velocity")) self.builder.ExportInput(v_mirror.GetInputPort("ball_pose"), "v_ball_pose") self.builder.ExportInput(v_mirror.GetInputPort("ball_velocity"), "v_ball_velocity") self.builder.ExportInput(w_tilt.GetInputPort("ball_pose"), "w_ball_pose") self.builder.ExportInput(w_tilt.GetInputPort("ball_velocity"), "w_ball_velocity") # Useful for debugging # desired_vel = self.builder.AddSystem(ConstantVectorSource([0, 0, 0])) # self.builder.Connect(desired_vel.get_output_port(), ik_sys.GetInputPort("paddle_desired_angular_velocity")) # --------------------- self.diagram = self.builder.Build() self.simulator = Simulator(self.diagram) self.simulator.set_target_realtime_rate(1.0) self.context = self.simulator.get_context() self.station_context = self.station.GetMyContextFromRoot(self.context) self.plant_context = self.plant.GetMyContextFromRoot(self.context) self.plant.SetPositions( self.plant_context, self.plant.GetModelInstanceByName("iiwa7"), [0, np.pi / 3, 0, -np.pi / 2, 0, -np.pi / 3, 0]) self.station.GetInputPort("iiwa_feedforward_torque").FixValue( self.station_context, np.zeros((7, 1))) iiwa_model_instance = self.plant.GetModelInstanceByName("iiwa7") iiwa_q = self.plant.GetPositions(self.plant_context, iiwa_model_instance) integrator.GetMyContextFromRoot( self.context).get_mutable_continuous_state_vector().SetFromVector( iiwa_q) def step(self, simulate=True, duration=0.1, final=True, verbose=False, display_pose=False): """ step the closed loop system Args: simulate (bool, optional): whether or not to visualize the command. Defaults to True. duration (float, optional): duration to complete command in simulation. Defaults to 0.1. final (bool, optional): whether or not this is the final command in the sequence; relevant for recording. Defaults to True. verbose (bool, optional): whether or not to print measured position change. Defaults to False. display_pose (bool, optional): whether or not to show the pose of the paddle in simulation. Defaults to False. """ ball_pose = self.plant.EvalBodyPoseInWorld( self.plant_context, self.plant.GetBodyByName("ball")).translation() ball_velocity = self.plant.EvalBodySpatialVelocityInWorld( self.plant_context, self.plant.GetBodyByName("ball")).translational() self.diagram.GetInputPort("v_ball_pose").FixValue( self.context, ball_pose) self.diagram.GetInputPort("v_ball_velocity").FixValue( self.context, ball_velocity) self.diagram.GetInputPort("w_ball_pose").FixValue( self.context, ball_pose) self.diagram.GetInputPort("w_ball_velocity").FixValue( self.context, ball_velocity) if display_pose: transform = self.plant.EvalBodyPoseInWorld( self.plant_context, self.plant.GetBodyByName("base_link")).GetAsMatrix4() AddTriad(self.visualizer.vis, name=f"paddle_{round(self.time, 1)}", prefix='', length=0.15, radius=.006) self.visualizer.vis[''][ f"paddle_{round(self.time, 1)}"].set_transform(transform) if simulate: self.visualizer.start_recording() self.simulator.AdvanceTo(self.time + duration) self.visualizer.stop_recording() self.position_log.append( self.station.GetOutputPort("iiwa_position_measured").Eval( self.station_context)) self.velocity_log.append( self.station.GetOutputPort("iiwa_velocity_estimated").Eval( self.station_context)) if verbose: print("Time: {}\nMeasured Position: {}\n\n".format( round(self.time, 3), np.around( self.station.GetOutputPort( "iiwa_position_measured").Eval( self.station_context), 3))) if len(self.position_log) >= 2 and np.equal( np.around(self.position_log[-1], 3), np.around(self.position_log[-2], 3)).all(): print("something went wrong, simulation terminated") final = True if final: self.visualizer.publish_recording() return self.time + duration self.time += duration
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
assert n == 1 q_default.append(default_positions[0]) return q_default q_iiwa0 = get_default_joint_positions(model_iiwa) gripper_position_input_port = station.GetInputPort("wsg_position") gripper_position_input_port.FixValue(context_station, [0.05]) iiwa_position_input_port = station.GetInputPort("iiwa_position") iiwa_position_input_port.FixValue(context_station, q_iiwa0) for name, model_list in models_object.items(): n_objects = len(model_names) x = 0.4 + 0.2 * np.random.rand(n_objects) y = -0.2 + 0.2 * np.random.rand(n_objects) z = 0.4 + 0.6 * np.random.rand(n_objects) X_WObject_list = [ RigidTransform(np.array([x[i], y[i], z[i]])) for i in range(n_objects) ] for i, model in enumerate(model_list): plant.SetFreeBodyPose(context_plant, plant.get_body(plant.GetBodyIndices(model)[0]), X_WObject_list[i]) #%% sim = Simulator(diagram, context) sim.set_target_realtime_rate(1.0) sim.AdvanceTo(2.0)
def RunSimulation(robobee_plantBS, control_law, x0=np.random.random((15, 1)), duration=30): robobee_controller = RobobeeController(control_law) # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() # The last pendulum plant we made is now owned by a deleted # system, so easiest path is for us to make a new one. plant = builder.AddSystem( RobobeePlantBS(m=robobee_plantBS.m, Ixx=robobee_plantBS.Ixx, Iyy=robobee_plantBS.Iyy, Izz=robobee_plantBS.Izz, g=robobee_plantBS.g, input_max=robobee_plantBS.input_max)) Rigidbody_selector = builder.AddSystem(RigidBodySelection()) print("1. Connecting plant and controller\n") controller = builder.AddSystem(robobee_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant print("2. Connecting plant to the logger\n") set_time_interval = 0.001 time_interval_multiple = 1000 publish_period = set_time_interval * time_interval_multiple input_log = builder.AddSystem(SignalLogger(4)) # input_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(controller.get_output_port(0), input_log.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(15)) # state_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Drake visualization print("3. Connecting plant output to DrakeVisualizer\n") rtree = RigidBodyTree( FindResourceOrThrow("drake/examples/robobee/robobee_arena.urdf"), FloatingBaseType.kQuaternion) #robobee_twobar lcm = DrakeLcm() visualizer = builder.AddSystem( DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True)) builder.Connect(plant.get_output_port(0), Rigidbody_selector.get_input_port(0)) builder.Connect(Rigidbody_selector.get_output_port(0), visualizer.get_input_port(0)) print("4. Building diagram\n") diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. print("5. Create simulation\n") simulator = Simulator(diagram, context) simulator.Initialize() # simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(1) simulator.get_integrator().set_fixed_step_mode(True) simulator.get_integrator().set_maximum_step_size(0.05) # Simulate for the requested duration. simulator.StepTo(duration) return input_log, state_log
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.]) simulator.StepTo(args.duration)
def project_tree_to_feasibility(tree, constraints=[], jitter_q=None, do_forward_sim=False, zmq_url=None, prefix="projection", timestep=0.001, T=1.): # Mutates tree into tree with bodies in closest # nonpenetrating configuration. builder, mbp, sg, node_to_free_body_ids_map, body_id_to_node_map = \ compile_scene_tree_to_mbp_and_sg(tree, timestep=timestep) mbp.Finalize() # Connect visualizer if requested. Wrap carefully to keep it # from spamming the console. if zmq_url is not None: with open(os.devnull, 'w') as devnull: with contextlib.redirect_stdout(devnull): visualizer = ConnectMeshcatVisualizer(builder, sg, zmq_url=zmq_url, prefix=prefix) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext(mbp, diagram_context) q0 = mbp.GetPositions(mbp_context) nq = len(q0) if nq == 0: logging.warn("Generated MBP had no positions.") return tree # Set up projection NLP. ik = InverseKinematics(mbp, mbp_context) q_dec = ik.q() prog = ik.prog() # It's always a projection, so we always have this # Euclidean norm error between the optimized q and # q0. prog.AddQuadraticErrorCost(np.eye(nq), q0, q_dec) # Nonpenetration constraint. ik.AddMinimumDistanceConstraint(0.001) # Other requested constraints. for constraint in constraints: constraint.add_to_ik_prog(tree, ik, mbp, mbp_context, node_to_free_body_ids_map) # Initial guess, which can be slightly randomized by request. q_guess = q0 if jitter_q: q_guess = q_guess + np.random.normal(0., jitter_q, size=q_guess.size) prog.SetInitialGuess(q_dec, q_guess) # Solve. solver = SnoptSolver() options = SolverOptions() logfile = "/tmp/snopt.log" os.system("rm %s" % logfile) options.SetOption(solver.id(), "Print file", logfile) options.SetOption(solver.id(), "Major feasibility tolerance", 1E-3) options.SetOption(solver.id(), "Major optimality tolerance", 1E-3) options.SetOption(solver.id(), "Major iterations limit", 300) result = solver.Solve(prog, None, options) if not result.is_success(): logging.warn("Projection failed.") print("Logfile: ") with open(logfile) as f: print(f.read()) qf = result.GetSolution(q_dec) mbp.SetPositions(mbp_context, qf) # If forward sim is requested, do a quick forward sim to get to # a statically stable config. if do_forward_sim: sim = Simulator(diagram, diagram_context) sim.set_target_realtime_rate(1000.) sim.AdvanceTo(T) # Reload poses back into tree free_bodies = mbp.GetFloatingBaseBodies() for body_id, node in body_id_to_node_map.items(): if body_id not in free_bodies: continue node.tf = drake_tf_to_torch_tf( mbp.GetFreeBodyPose(mbp_context, mbp.get_body(body_id))) return tree
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)
def project_tree_to_feasibility_via_sim(tree, constraints=[], zmq_url=None, prefix="projection", timestep=0.0005, T=10.): # Mutates tree into tree with bodies in closest # nonpenetrating configuration. builder, mbp, sg, node_to_free_body_ids_map, body_id_to_node_map = \ compile_scene_tree_to_mbp_and_sg(tree, timestep=timestep) mbp.Finalize() # Connect visualizer if requested. Wrap carefully to keep it # from spamming the console. if zmq_url is not None: with open(os.devnull, 'w') as devnull: with contextlib.redirect_stdout(devnull): visualizer = ConnectMeshcatVisualizer(builder, sg, zmq_url=zmq_url, prefix=prefix) # Forward sim under langevin forces force_source = builder.AddSystem( DecayingForceToDesiredConfigSystem( mbp, mbp.GetPositions(mbp.CreateDefaultContext()))) builder.Connect(mbp.get_state_output_port(), force_source.get_input_port(0)) builder.Connect(force_source.get_output_port(0), mbp.get_applied_spatial_force_input_port()) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext(mbp, diagram_context) q0 = mbp.GetPositions(mbp_context) nq = len(q0) if nq == 0: logging.warn("Generated MBP had no positions.") return tree # Make 'safe' initial configuration that randomly arranges objects vertically #k = 0 #all_pos = [] #for node in tree: # for body_id in node_to_free_body_ids_map[node]: # body = mbp.get_body(body_id) # tf = mbp.GetFreeBodyPose(mbp_context, body) # tf = RigidTransform(p=tf.translation() + np.array([0., 0., 1. + k*0.5]), R=tf.rotation()) # mbp.SetFreeBodyPose(mbp_context, body, tf) # k += 1 sim = Simulator(diagram, diagram_context) sim.set_target_realtime_rate(1000) sim.AdvanceTo(T) # Reload poses back into tree free_bodies = mbp.GetFloatingBaseBodies() for body_id, node in body_id_to_node_map.items(): if body_id not in free_bodies: continue node.tf = drake_tf_to_torch_tf( mbp.GetFreeBodyPose(mbp_context, mbp.get_body(body_id))) return tree