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 main(): assert kCassiePositions == 23 assert kCassieVelocities == 22 assert kCassieStates == 23 + 22 assert kCassieActuators == 10 assert abs(kCassieFourBarDistance - 0.5012) < 1e-8 assert p_midfoot.shape == (3, ) assert CassieURDFType.kStandardCassie == 0 assert CassieURDFType.kSoftSpringsCassie == 1 assert CassieURDFType.kActiveSpringsCassie == 2 assert CassieURDFType.kActiveAnkleCassie == 3 assert CassieURDFType.kFixedSpringCassie == 4 assert CassieFixedPointState().shape == (45, ) assert CassieFixedPointState( FloatingBaseType.kRollPitchYaw).shape == (44, ) assert CassieFixedPointState(FloatingBaseType.kFixed).shape == (32, ) assert CassieFixedPointTorque().shape == (10, ) assert GetFourBarHipMountPoint().shape == (3, ) assert GetFourBarHeelMountPoint().shape == (3, ) baseState = BaseStateFromFullState(CassieFixedPointState()) assert baseState.shape == (13, ) rtree = getCassieTree() assert rtree is not None assert getCassieTree(CassieURDFType.kStandardCassie) is not None assert getCassieTree(CassieURDFType.kSoftSpringsCassie) is not None assert getCassieTree(CassieURDFType.kActiveSpringsCassie) is not None assert getCassieTree(CassieURDFType.kActiveAnkleCassie) is not None assert getCassieTree(CassieURDFType.kFixedSpringCassie) is not None plant = RigidBodyPlant(rtree) setDefaultContactParams(plant)
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 Simulate2dBallAndBeam(x0, duration): builder = DiagramBuilder() # Load in the ball and beam from a description file. tree = RigidBodyTree() AddModelInstancesFromSdfString( open("ball_and_beam.sdf", 'r').read(), FloatingBaseType.kFixed, None, tree) # A RigidBodyPlant wraps a RigidBodyTree to allow # forward dynamical simulation. plant = builder.AddSystem(RigidBodyPlant(tree)) # Spawn a controller and hook it up controller = builder.AddSystem(BallAndBeam2dController(tree)) 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 log at 30hz state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Create a simulator diagram = builder.Build() simulator = Simulator(diagram) # Don't limit realtime rate for this sim, since we # produce a video to render it after simulating the whole thing. #simulator.set_target_realtime_rate(100.0) simulator.set_publish_every_time_step(False) # Force the simulator to use a fixed-step integrator, # which is much faster for this stiff system. (Due to the # spring-model of collision, the default variable-timestep # integrator will take very short steps. I've chosen the step # size here to be fast while still being stable in most situations.) integrator = simulator.get_mutable_integrator() integrator.set_fixed_step_mode(True) integrator.set_maximum_step_size(0.001) # Set the initial state state = simulator.get_mutable_context( ).get_mutable_continuous_state_vector() state.SetFromVector(x0) # Simulate! simulator.StepTo(duration) return tree, controller, state_log
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 simulate_and_log_policy_system(policy_system, expmt, ic=None): global tree global logger expmt_settings = { "pendulum": { 'constructor_or_path': PendulumPlant, 'state_dim': 2, 'twoPI_boundary_condition_state_idxs': (0,), 'initial_state': [0.1, 0.0], }, "acrobot": { 'constructor_or_path': "/opt/underactuated/src/acrobot/acrobot.urdf", 'state_dim': 4, 'twoPI_boundary_condition_state_idxs': (0, 1), 'initial_state': [0.5, 0.5, 0.0, 0.0], }, "cartpole": { 'constructor_or_path': "/opt/underactuated/src/cartpole/cartpole.urdf", 'state_dim': 4, 'twoPI_boundary_condition_state_idxs': (1,), 'initial_state': [0.5, 0.5, 0.0, 0.0], }, } assert expmt in expmt_settings.keys() # Animate the resulting policy. settings = expmt_settings[expmt] builder = DiagramBuilder() constructor_or_path = settings['constructor_or_path'] if not isinstance(constructor_or_path, str): plant = constructor_or_path() else: tree = RigidBodyTree(constructor_or_path, 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, settings['state_dim'], settings['state_dim']) def _DoCalcVectorOutput(self, context, input, state, output): output[:] = input twoPI = 2.*math.pi for idx in settings['twoPI_boundary_condition_state_idxs']: # output[idx] += math.pi output[idx] = output[idx]# - twoPI * math.floor(output[idx] / twoPI) # output[idx] -= math.pi wrap = builder.AddSystem(WrapTheta()) builder.Connect(plant_system.get_output_port(0), wrap.get_input_port(0)) vi_policy = builder.AddSystem(policy_system) 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)) x_logger = builder.AddSystem(SignalLogger(settings['state_dim'])) x_logger._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant_system.get_output_port(0), x_logger.get_input_port(0)) u_logger = builder.AddSystem(SignalLogger(1)) u_logger._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(vi_policy.get_output_port(0), u_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() if ic is None: ic = settings['initial_state'] state.SetFromVector(ic) return simulator, tree, x_logger, u_logger
kd = .1 # Cancel double pend dynamics and inject single pend dynamics. torque[:] = Cv - tauG + \ M.dot([-self.g / length * sin(q[0]) - b * v[0], -kp * q[1] + kd * v[1]]) # Load the double pendulum from Universal Robot Description Format tree = RigidBodyTree(FindResource("double_pendulum/double_pendulum.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)) parser = argparse.ArgumentParser() parser.add_argument("-g", "--gravity", 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))
rbt, pbrv = setupDoublePendulumExample() timestep = 0.0 set_initial_state = True elif model == "val": rbt, pbrv = setupValkyrieExample() timestep = 0.001 # Setting initial state does not work for Timestepping RBT # in current configuration. It's probably something simple, # but I just want to hack the Val planar viz into working... set_initial_state = False else: print "Unrecognized model %s." % model parser.print_usage() exit(1) rbplant = RigidBodyPlant(rbt, timestep) nx = rbt.get_num_positions() + rbt.get_num_velocities() builder = DiagramBuilder() rbplant_sys = builder.AddSystem(rbplant) torque = args.torque torque_system = builder.AddSystem( ConstantVectorSource( np.ones((rbt.get_num_actuators(), 1)) * torque)) builder.Connect(torque_system.get_output_port(0), rbplant_sys.get_input_port(0)) print('Simulating with constant torque = ' + str(torque) + ' Newton-meters') # Visualize
def SimulateHand(duration=10., num_fingers=3, mu=0.5, manipuland_sdf="models/manipuland_box.sdf", initial_manipuland_pose=np.array([1.5, 0., 0.]), n_grasp_search_iters=100, manipuland_trajectory_callback=None, control_period=0.0333, print_period=1.0): ''' Given a great many passthrough arguments (see docs for HandController and usage example in set_5_mpc.ipynb), constructs a simulation of a num_fingers-fingered hand and simulates it for duration seconds from a specified initial manipuland pose. Returns: (hand, plant, controller, state_log, contact_log) hand: The RigidBodyTree of the complete hand. plant: The RigidBodyPlant that owns the hand RBT controller: The HandController object state_log: A SignalLogger that has logged the output of the state output port of plant. contact_log: A PlanarHandContactLogger object that has logged the output of the contact results output port of the plant. ''' builder = DiagramBuilder() tree = BuildHand(num_fingers, manipuland_sdf) num_finger_links = 3 # from sdf num_hand_q = num_finger_links * num_fingers # Generate the nominal posture for the hand # First link wide open, next links at right angles x_nom = np.zeros(2 * num_finger_links * num_fingers + 6) for i in range(num_fingers): if i < num_fingers / 2: x_nom[(num_finger_links*i):(num_finger_links*(i+1))] = \ np.array([2, 1.57, 1.57]) else: x_nom[(num_finger_links*i):(num_finger_links*(i+1))] = \ -np.array([2, 1.57, 1.57]) # Drop in the initial manipuland location x_nom[num_hand_q:(num_hand_q + 3)] = initial_manipuland_pose # A RigidBodyPlant wraps a RigidBodyTree to allow # forward dynamical simulation. It handles e.g. collision # modeling. plant = builder.AddSystem(RigidBodyPlant(tree)) # Alter the ground material used in simulation to make # it dissipate more energy (to make the object less bouncy) # and stickier (to make it easier to hold) and softer # (again to make it less bouncy) allmaterials = CompliantMaterial() allmaterials.set_youngs_modulus(1E6) # default 1E9 allmaterials.set_dissipation(1.0) # default 0.32 allmaterials.set_friction(0.9) # default 0.9. plant.set_default_compliant_material(allmaterials) # Spawn a controller and hook it up controller = builder.AddSystem( HandController( tree, x_nom=x_nom, num_fingers=num_fingers, mu=mu, n_grasp_search_iters=n_grasp_search_iters, manipuland_trajectory_callback=manipuland_trajectory_callback, control_period=control_period, print_period=print_period)) nq = controller.nq qinit, info = controller.ComputeTargetPosture(x_nom, x_nom[(nq - 3):nq], backoff_distance=0.0) if info != 1: print "Warning: initial condition IK solve returned info ", info xinit = np.zeros(x_nom.shape) xinit[0:(nq - 3)] = qinit[0:-3] xinit[(nq - 3):nq] = x_nom[(nq - 3):nq] builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) for i in range(num_fingers): builder.Connect(controller.get_output_port(i), plant.get_input_port(i)) # Create a state logger to log at 30hz state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # And a contact result logger, same rate contact_log = builder.AddSystem(PlanarHandContactLogger(controller, plant)) contact_log.DeclarePeriodicPublish(0.0333, 0.0) builder.Connect(plant.contact_results_output_port(), contact_log.get_input_port(0)) # Create a simulator diagram = builder.Build() simulator = Simulator(diagram) # Don't limit realtime rate for this sim, since we # produce a video to render it after simulating the whole thing. # simulator.set_target_realtime_rate(100.0) simulator.set_publish_every_time_step(False) # Force the simulator to use a fixed-step integrator, # which is much faster for this stiff system. (Due to the # spring-model of collision, the default variable-timestep # integrator will take very short steps. I've chosen the step # size here to be fast while still being stable in most situations.) integrator = simulator.get_mutable_integrator() integrator.set_fixed_step_mode(True) integrator.set_maximum_step_size(0.005) # Set the initial state state = simulator.get_mutable_context().\ get_mutable_continuous_state_vector() state.SetFromVector(xinit) # Simulate! simulator.StepTo(duration) return tree, plant, controller, state_log, contact_log
import argparse import numpy as np from pydrake.all import (DiagramBuilder, FloatingBaseType, RigidBodyPlant, RigidBodyTree, Simulator, VectorSystem) from underactuated import (FindResource, PlanarRigidBodyVisualizer, SliderSystem) 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))
# Record the history of sim in a set of (state_log, rbt) slices # so that we can reconstruct / animate it at the end. sim_slices = [] x[0:q0.shape[0]] = q0 t = 0 mrbv = None while 1: mrbv = kuka_utils.ReinitializableMeshcatRigidBodyVisualizer( rbt, draw_timestep=0.01, old_mrbv=mrbv, clear_vis=(mrbv is None)) # (wait while the visualizer warms up and loads in the models) mrbv.draw(x) # Make our RBT into a plant for simulation rbplant = RigidBodyPlant(rbt) allmaterials = CompliantMaterial() allmaterials.set_youngs_modulus(1E8) # default 1E9 allmaterials.set_dissipation(0.8) # default 0.32 allmaterials.set_friction(0.9) # default 0.9. rbplant.set_default_compliant_material(allmaterials) # Build up our simulation by spawning controllers and loggers # and connecting them to our plant. builder = DiagramBuilder() # The diagram takes ownership of all systems # placed into it. rbplant_sys = builder.AddSystem(rbplant) # Spawn the controller that drives the Kuka to its # desired posture.
import os.path from pydrake.all import (DiagramBuilder, FloatingBaseType, RigidBodyPlant, RigidBodyTree, Simulator, VectorSystem, ConstantVectorSource, CompliantMaterial, CompliantContactModelParameters, DrakeVisualizer, AddFlatTerrainToWorld) from pydrake.multibody.rigid_body_tree import (FloatingBaseType, RigidBodyFrame, RigidBodyTree) from pydrake.lcm import DrakeLcm from cassie_common import * import numpy as np rtree = getCassieTree() plant = RigidBodyPlant(rtree, 0.0005) builder = DiagramBuilder() cassie = builder.AddSystem(plant) setDefaultContactParams(cassie) # precomputed standing fixed point state # q = getNominalStandingConfiguration() # qd = [0. for i in xrange(rtree.get_num_velocities())] x = CassieFixedPointState() # Setup visualizer lcm = DrakeLcm() visualizer = builder.AddSystem( DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
def plant_system(self): ''' Implements the plant_system method in DrakeEnv by constructing a RigidBodyPlant ''' self.rbp = RigidBodyPlant(self.tree) return self.rbp
import argparse import numpy as np from pydrake.all import (DiagramBuilder, FloatingBaseType, RigidBodyPlant, RigidBodyTree, Simulator, VectorSystem) from underactuated import (FindResource, PlanarRigidBodyVisualizer, SliderSystem) tree = RigidBodyTree("PlanarWalker.urdf", FloatingBaseType.kFixed) builder = DiagramBuilder() plant = RigidBodyPlant(tree) acrobot = builder.AddSystem(plant) 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)
def make_real_dircol_mp(expmt="cartpole", seed=1776): global tree global plant global context global dircol # TODO: use the seed in some meaningful way: # https://github.com/RobotLocomotion/drake/blob/master/systems/stochastic_systems.h assert expmt in ("cartpole", "acrobot") # expmt = "cartpole" # State: (x, theta, x_dot, theta_dot) Input: x force # expmt = "acrobot" # State: (theta1, theta2, theta1_dot, theta2_dot) Input: Elbow torque if expmt == "cartpole": tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf", FloatingBaseType.kFixed) plant = RigidBodyPlant(tree) else: tree = RigidBodyTree("/opt/underactuated/src/acrobot/acrobot.urdf", FloatingBaseType.kFixed) plant = AcrobotPlant() context = plant.CreateDefaultContext() if expmt == "cartpole": dircol = DirectCollocation(plant, context, num_time_samples=21, minimum_timestep=0.1, maximum_timestep=0.4) else: dircol = DirectCollocation(plant, context, num_time_samples=21, minimum_timestep=0.05, maximum_timestep=0.2) dircol.AddEqualTimeIntervalsConstraints() if expmt == "acrobot": # Add input limits. torque_limit = 8.0 # N*m. u = dircol.input() dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0]) dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit) initial_state = (0., 0., 0., 0.) dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state()) # More elegant version is blocked on drake #8315: # dircol.AddLinearConstraint(dircol.initial_state() == initial_state) if expmt == "cartpole": final_state = (0., math.pi, 0., 0.) else: final_state = (math.pi, 0., 0., 0.) dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state()) # dircol.AddLinearConstraint(dircol.final_state() == final_state) # R = 10 # Cost on input "effort". # u = dircol.input() # dircol.AddRunningCost(R*u[0]**2) # 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))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) return dircol, tree
def VI(u_cost=180.**2): tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf", FloatingBaseType.kFixed) plant = RigidBodyPlant(tree) simulator = Simulator(plant) options = DynamicProgrammingOptions() def min_time_cost(context): x = context.get_continuous_state_vector().CopyToVector() u = plant.EvalVectorInput(context, 0).CopyToVector() x[1] = x[1] - math.pi if x.dot(x) < .1: # seeks get x to (math.pi, 0., 0., 0.) return 0. return 1. + 2*x.dot(x)/(10**2+math.pi**2+10**2+math.pi**2) + u.dot(u)/(u_cost) def quadratic_regulator_cost(context): x = context.get_continuous_state_vector().CopyToVector() x[1] = x[1] - math.pi u = plant.EvalVectorInput(context, 0).CopyToVector() return 2*x.dot(x)/(10**2+math.pi**2+10**2+math.pi**2) + u.dot(u)/(u_cost) if (True): cost_function = min_time_cost input_limit = 360. options.convergence_tol = 0.001 state_steps = 19 input_steps = 19 else: cost_function = quadratic_regulator_cost input_limit = 250. options.convergence_tol = 0.01 state_steps = 19 input_steps = 19 ####### SETTINGS ####### My cartpole linspaces are off?????? # State: (x, theta, x_dot, theta_dot) # Previous Best... (min. time) (3) xbins = np.linspace(-10., 10., state_steps) thetabins = np.hstack((np.linspace(0., math.pi-0.2, 8), np.linspace(math.pi-0.2, math.pi+0.2, 11), np.linspace(math.pi+0.2, 8, 2*math.pi))) xdotbins = np.linspace(-10., 10., state_steps) thetadotbins = np.linspace(-10., 10., state_steps) timestep = 0.01 # Test 1 (4) xbins = np.linspace(-10., 10., state_steps) thetabins = np.hstack((np.linspace(0., math.pi-0.12, 8), np.linspace(math.pi-0.12, math.pi+0.12, 11), np.linspace(math.pi+0.12, 8, 2*math.pi))) xdotbins = np.linspace(-10., 10., state_steps+2) thetadotbins = np.hstack((np.linspace(-10., -1.5, 9), np.linspace(-1.5, 1.5, 11), np.linspace(1.5, 10., 9))) # timestep = 0.001 <- wasn't active... # Test 2 - Test 1 was worse? WOW I HAD A BUG! - in my last np.linspace (5) SWEET!!! xbins = np.linspace(-10., 10., state_steps) thetabins = np.hstack((np.linspace(0., math.pi-0.2, 10), np.linspace(math.pi-0.2, math.pi+0.2, 9), np.linspace(math.pi+0.2, 2*math.pi, 10))) xdotbins = np.linspace(-10., 10., state_steps+2) thetadotbins = np.linspace(-10., 10., state_steps) timestep = 0.01 input_limit = 1000. # test_stabilize_top7 for the higher input_limit version options.periodic_boundary_conditions = [ PeriodicBoundaryCondition(1, 0., 2.*math.pi), ] state_grid = [set(xbins), set(thetabins), set(xdotbins), set(thetadotbins)] input_grid = [set(np.linspace(-input_limit, input_limit, input_steps))] # Input: x force print("VI with u_cost={} beginning".format(u_cost)) policy, cost_to_go = FittedValueIteration(simulator, cost_function, state_grid, input_grid, timestep, options) print("VI with u_cost={} completed!".format(u_cost)) save_policy("u_cost={:.0f}_torque_limit={:.0f}".format(u_cost, input_limit), policy, cost_to_go, state_grid) return policy, cost_to_go
def Simulate2dHopper(x0, duration, desired_lateral_velocity=0.0, print_period=0.0): builder = DiagramBuilder() # Load in the hopper from a description file. # It's spawned with a fixed floating base because # the robot description file includes the world as its # root link -- it does this so that I can create a robot # system with planar dynamics manually. (Drake doesn't have # a planar floating base type accessible right now that I # know about -- it only has 6DOF floating base types.) tree = RigidBodyTree() AddModelInstancesFromSdfString( open("raibert_hopper_2d.sdf", 'r').read(), FloatingBaseType.kFixed, None, tree) # A RigidBodyPlant wraps a RigidBodyTree to allow # forward dynamical simulation. It handles e.g. collision # modeling. plant = builder.AddSystem(RigidBodyPlant(tree)) # Alter the ground material used in simulation to make # it dissipate more energy (to make the hopping more critical) # and stickier (to make the hopper less likely to slip). allmaterials = CompliantMaterial() allmaterials.set_youngs_modulus(1E8) # default 1E9 allmaterials.set_dissipation(1.0) # default 0.32 allmaterials.set_friction(1.0) # default 0.9. plant.set_default_compliant_material(allmaterials) # Spawn a controller and hook it up controller = builder.AddSystem( Hopper2dController(tree, desired_lateral_velocity=desired_lateral_velocity, 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)) # Create a logger to log at 30hz state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Create a simulator diagram = builder.Build() simulator = Simulator(diagram) # Don't limit realtime rate for this sim, since we # produce a video to render it after simulating the whole thing. #simulator.set_target_realtime_rate(100.0) simulator.set_publish_every_time_step(False) # Force the simulator to use a fixed-step integrator, # which is much faster for this stiff system. (Due to the # spring-model of collision, the default variable-timestep # integrator will take very short steps. I've chosen the step # size here to be fast while still being stable in most situations.) integrator = simulator.get_mutable_integrator() integrator.set_fixed_step_mode(True) integrator.set_maximum_step_size(0.0005) # Set the initial state state = simulator.get_mutable_context( ).get_mutable_continuous_state_vector() state.SetFromVector(x0) # Simulate! simulator.StepTo(duration) return tree, controller, state_log
def make_dircol_cartpole(ic=(-1., 0., 0., 0.), num_samples=21, min_timestep=0.0001, max_timestep=1., warm_start="linear", seed=1776, should_vis=False, torque_limit=250., target_traj=None, **kwargs): global dircol global plant global context tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf", FloatingBaseType.kFixed) plant = RigidBodyPlant(tree) context = plant.CreateDefaultContext() dircol = DirectCollocation( plant, context, num_time_samples=num_samples, # minimum_timestep=0.01, maximum_timestep=0.01) minimum_timestep=min_timestep, maximum_timestep=max_timestep) # dircol.AddEqualTimeIntervalsConstraints() # torque_limit = input_limit # N*m. # torque_limit = 64. u = dircol.input() dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0]) dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit) initial_state = ic dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state()) final_state = np.array([0., math.pi, 0., 0.]).astype(np.double) dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state()) # R = 100 # Cost on input "effort". u = dircol.input() x = dircol.state() # t = dircol.time() # let's add 100*t (seconds) to get in that min-time component! denom1 = float(10**2 + math.pi**2 + 10**2 + math.pi**2) denom2 = float(180**2) #denom1 = 10**2+math.pi**2+10**2+math.pi**2 #denom2 = 180**2 # dircol.AddRunningCost(u.dot(u)/denom2) # dircol.AddRunningCost(2*(x-final_state).dot(x-final_state)/denom1) dircol.AddRunningCost(1 + 2. * (x - final_state).dot(x - final_state) / denom1 + u.dot(u) / denom2) # Add a final cost equal to the total duration. #dircol.AddFinalCost(dircol.time()) # Enabled to sim min time cost? if warm_start == "linear": initial_u_trajectory = PiecewisePolynomial() initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], np.column_stack((initial_state, final_state))) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) elif warm_start == "random": assert isinstance(seed, int) np.random.seed(seed) breaks = np.linspace(0, 4, num_samples).reshape( (-1, 1)) # using num_time_samples u_knots = np.random.rand( 1, num_samples) - 0.5 # num_inputs vs num_samples? x_knots = np.random.rand( 2, num_samples) - 0.5 # num_states vs num_samples? initial_u_trajectory = PiecewisePolynomial.Cubic( breaks, u_knots, False) initial_x_trajectory = PiecewisePolynomial.Cubic( breaks, x_knots, False) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) elif warm_start == "target": assert target_traj != [], "Need a valid target for warm starting" (breaks, x_knots, u_knots) = target_traj #(breaks, u_knots, x_knots) = target_traj initial_u_trajectory = PiecewisePolynomial.Cubic( breaks.T, u_knots.T, False) initial_x_trajectory = PiecewisePolynomial.Cubic( breaks.T, x_knots.T, False) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) def cb(decision_vars): global vis_cb_counter vis_cb_counter += 1 if vis_cb_counter % 10 != 0: return # Get the total cost all_costs = dircol.EvalBindings(dircol.GetAllCosts(), decision_vars) # Get the total cost of the constraints. # Additionally, the number and extent of any constraint violations. violated_constraint_count = 0 violated_constraint_cost = 0 constraint_cost = 0 for constraint in dircol.GetAllConstraints(): val = dircol.EvalBinding(constraint, decision_vars) # Consider switching to DoCheckSatisfied if you can find the binding... nudge = 1e-1 # This much constraint violation is not considered bad... lb = constraint.evaluator().lower_bound() ub = constraint.evaluator().upper_bound() good_lb = np.all(np.less_equal(lb, val + nudge)) good_ub = np.all(np.greater_equal(ub, val - nudge)) if not good_lb or not good_ub: # print("{} <= {} <= {}".format(lb, val, ub)) violated_constraint_count += 1 # violated_constraint_cost += np.sum(np.abs(val)) if not good_lb: violated_constraint_cost += np.sum(np.abs(lb - val)) if not good_ub: violated_constraint_cost += np.sum(np.abs(val - ub)) constraint_cost += np.sum(np.abs(val)) print("total cost: {: .2f} | \tconstraint {: .2f} \tbad {}, {: .2f}". format(sum(all_costs), constraint_cost, violated_constraint_count, violated_constraint_cost)) #dircol.AddVisualizationCallback(cb, dircol.decision_variables()) def MyVisualization(sample_times, values): def state_to_tip_coord(state_vec): # State: (x, theta, x_dot, theta_dot) x, theta, _, _ = state_vec pole_length = 0.5 # manually looked this up #return (x-pole_length*np.sin(theta), pole_length-np.cos(theta)) return (x - pole_length * np.sin(theta), pole_length * (-np.cos(theta))) global vis_cb_counter vis_cb_counter += 1 if vis_cb_counter % 30 != 0: return coords = [state_to_tip_coord(state) for state in values.T] x, y = zip(*coords) plt.plot(x, y, '-o', label=vis_cb_counter) #plt.show() # good? #if should_vis: if False: plt.figure() plt.title('Tip trajectories') plt.xlabel('x') plt.ylabel('x_dot') dircol.AddStateTrajectoryCallback(MyVisualization) from pydrake.all import (SolverType) # dircol.SetSolverOption(SolverType.kSnopt, 'Major feasibility tolerance', 1.0e-6) # default="1.0e-6" # dircol.SetSolverOption(SolverType.kSnopt, 'Major optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1 # dircol.SetSolverOption(SolverType.kSnopt, 'Minor feasibility tolerance', 1.0e-6) # default="1.0e-6" # dircol.SetSolverOption(SolverType.kSnopt, 'Minor optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1 # dircol.SetSolverOption(SolverType.kSnopt, 'Time limit (secs)', 12.0) # default="9999999.0" # Very aggressive cutoff... dircol.SetSolverOption( SolverType.kSnopt, 'Major step limit', 0.1) # default="2.0e+0" # HUGE!!! default takes WAY too huge steps dircol.SetSolverOption(SolverType.kSnopt, 'Time limit (secs)', 15.0) # default="9999999.0" # was 15 # dircol.SetSolverOption(SolverType.kSnopt, 'Reduced Hessian dimension', 10000) # Default="min{2000, n1 + 1}" # dircol.SetSolverOption(SolverType.kSnopt, 'Hessian updates', 30) # Default="10" dircol.SetSolverOption(SolverType.kSnopt, 'Major iterations limit', 9300000) # Default="9300" dircol.SetSolverOption(SolverType.kSnopt, 'Minor iterations limit', 50000) # Default="500" dircol.SetSolverOption(SolverType.kSnopt, 'Iterations limit', 50 * 10000) # Default="10000" # Factoriztion? # dircol.SetSolverOption(SolverType.kSnopt, 'QPSolver Cholesky', True) # Default="*Cholesky/CG/QN" return dircol
# AddModelInstanceFromUrdfFile("models/kuka_table.urdf",FloatingBaseType.kFixed, world_frame, tree) kuka_urdf_path = getKukaUrdfPath(collision_type=collision_type) # table_top_frame = tree.findFrame("kuka_table_top") # AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed,table_top_frame, tree) AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed,world_frame, tree) builder = DiagramBuilder() # build the experiment system tree = RigidBodyTree() AddFlatTerrainToWorld(tree, 100, 10) addKukaSetup(tree,collision_type='spheres') # create the diagram plant = RigidBodyPlant(tree, 0.0001) # 2000 Hz deltas = [-0.66667,-0.33333,0.0,0.5,1.0] for delta in deltas: # print "delta: " + str(delta) for i in range(tree.get_num_positions()+tree.get_num_velocities()): # print "index: " + str(i) xk = np.zeros(tree.get_num_positions()+tree.get_num_velocities()) # xk = np.array([0.3689,0.4607,0.9816,0.1564,0.8555,0.6448,0.3763,0.1909,0.4283,0.4820,0.1206,0.5895,0.2262,0.3846]) xk[i] += delta str_out = '' for j in range(tree.get_num_positions()+tree.get_num_velocities()): str_out += "%.6f " % xk[j] print str_out + "\n" kinsol = tree.doKinematics(xk[:tree.get_num_positions()],xk[tree.get_num_positions():]) massMatrix = tree.massMatrix(kinsol)
# 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) # build the diagram builder.Connect(source.get_output_port(0), controller.get_input_port(1)) builder.Connect(controller.get_output_port(0), kuka.get_input_port(0)) builder.Connect(kuka.get_output_port(0), visualizer.get_input_port(0)) builder.Connect(kuka.get_output_port(0), controller.get_input_port(0)) diagram = builder.Build() # run the simulator
from pydrake.all import (DirectCollocation, FloatingBaseType, PiecewisePolynomial, RigidBodyTree, RigidBodyPlant, SolutionResult) #The following imports come from Mark Petersen's quadrotor example from pydrake.all import (ConstantVectorSource, DiagramBuilder, FramePoseVector, MeshcatVisualizer, SceneGraph, Simulator) from pydrake.all import (MultibodyPlant, Parser) from underactuated import (FindResource, PlanarRigidBodyVisualizer) #Create Plant tree = RigidBodyTree( FindResource("diff_drive/diff_drive.urdf"), FloatingBaseType.kRollPitchYaw ) #Use same floating base as 2D quadrotor, or perhaps 3D, kRollPitchYaw will likely be best plant = RigidBodyPlant(tree) context = plant.CreateDefaultContext() builder = DiagramBuilder() diff_drive = builder.AddSystem(RigidBodyPlant(tree)) diagram = builder.Build() context = simulator.get_mutable_context() x0 = np.zeros((4, 1)) context.SetContinuousState(x0) #Setup Controller #Setup Visualization #Run Simulation