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())
# Plan a robot motion to maneuver from the initial posture
# to a posture that we know should grab the object.
# (Grasp planning is left as an exercise :))
q0 = rbt.getZeroConfiguration()
qtraj, info = kuka_ik.plan_grasping_trajectory(
rbt,
q0=q0,
target_reach_pose=np.array([0.6, 0., 1.0, -0.75, 0., -1.57]),
target_grasp_pose=np.array([0.8, 0., 0.9, -0.75, 0., -1.57]),
n_knots=20,
reach_time=1.5,
grasp_time=2.0)
# Make our RBT into a plant for simulation
rbplant = RigidBodyPlant(rbt)
rbplant.set_name("Rigid Body Plant")
# 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)
# Create a high-level state machine to guide the robot
# motion...
manip_state_machine = builder.AddSystem(
kuka_controllers.ManipStateMachine(rbt, rbplant_sys, qtraj))
builder.Connect(rbplant_sys.state_output_port(),
manip_state_machine.robot_state_input_port)
class RigidBodyTreeEnv(drake_env.DrakeEnv):
'''
Implements a DrakeEnv for models specified by a RigidBodyTree. Constructs
the RigidBodyPlant for simulation and DrakeVisualizer for visualization.
'''
def __init__(self, model_path, floating_base_type):
self.model_path = get_full_model_path(model_path)
self.tree = RigidBodyTree(self.model_path, floating_base_type)
self._visualizer = None
super(RigidBodyTreeEnv, self).__init__()
@property
def visualizer(self):
if self._visualizer is None:
self._visualizer = MeshcatVisualizerRBT(self.tree)
return self._visualizer
def plant_system(self):
'''
Implements the plant_system method in DrakeEnv by constructing a RigidBodyPlant
'''
self.rbp = RigidBodyPlant(self.tree)
return self.rbp
def get_input_port_action(self):
'''
Returns the system input port that corresponds to the action
'''
return self.rbp.actuator_command_input_port()
def get_observation(self):
return self.get_state()
def get_state(self):
'''
Returns the system output port that corresponds to the observation
'''
rbp_context = self.simulator.get_context()
return rbp_context.get_continuous_state().get_vector().get_value()
@property
def action_space(self):
return gym.spaces.Box(*self.action_limits, dtype=np.float32)
@property
def observation_space(self):
return gym.spaces.Box(*self.observation_limits, dtype=np.float32)
@property
def action_limits(self):
'''
Subclasses should implement their own action limits
'''
raise NotImplementedError
@property
def observation_limits(self):
'''
Subclasses should implement their own observation limits
'''
raise NotImplementedError
def render(self, mode='human', close=False):
'''
Notifies the visualizer to redraw the image.
'''
self.visualizer.draw(self.get_state())
def plant_system(self):
'''
Implements the plant_system method in DrakeEnv by constructing a RigidBodyPlant
'''
self.rbp = RigidBodyPlant(self.tree)
return self.rbp
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
# 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
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))
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))
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
x_min = np.array([-3., theta_min - theta_step, -10., -10.])
x_max = np.array([3., theta_max + theta_step, 10., 10.])
X = Polyhedron.from_bounds(x_min, x_max)
#%%
# boundaries for the input --force on the cart-- (despite of the mode the inpu has to lie in U)
u_min = np.array([-100.])
u_max = np.array([100.])
U = Polyhedron.from_bounds(u_min, u_max)
#%%
# parse RigidBodyPlant from urdf
tree = RigidBodyTree("cart_pole.urdf", FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
#%%
# sampling time
h = .05
# get piecewise affine system
pwa = pwa_from_RigidBodyPlant(plant, linearization_points, X, U, h)
#%%
# parameters of the quadratic controller
N = 20
Q = np.eye(pwa.nx) / 100.
R = np.eye(pwa.nu) / 100.
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
RigidBodyTree, Simulator, VectorSystem,
ConstantVectorSource, CompliantMaterial,
CompliantContactModelParameters, DrakeVisualizer,
AddFlatTerrainToWorld)
from pydrake.multibody.rigid_body_tree import (FloatingBaseType,
RigidBodyFrame, RigidBodyTree)
from pydrake.lcm import DrakeLcm
import numpy as np
rtree = RigidBodyTree(
FindResourceOrThrow("drake/examples/simple_four_bar/FourBar.urdf"),
FloatingBaseType.kFixed)
plant = RigidBodyPlant(rtree)
builder = DiagramBuilder()
cassie = builder.AddSystem(plant)
# Setup visualizer
lcm = DrakeLcm()
visualizer = builder.AddSystem(
DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
builder.Connect(cassie.get_output_port(0), visualizer.get_input_port(0))
# Zero inputs -- passive forward simulation
u0 = ConstantVectorSource(np.zeros(rtree.get_num_actuators()))
null_controller = builder.AddSystem(u0)
builder.Connect(null_controller.get_output_port(0), cassie.get_input_port(0))
import math
import numpy as np
import matplotlib.pyplot as plt
from pydrake.all import (DirectCollocation, FloatingBaseType,
PiecewisePolynomial, RigidBodyTree, RigidBodyPlant,
Solve)
from underactuated import (FindResource, PlanarRigidBodyVisualizer)
tree = RigidBodyTree(FindResource("cartpole/cartpole.urdf"),
FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
context = plant.CreateDefaultContext()
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.1,
maximum_timestep=0.4)
dircol.AddEqualTimeIntervalsConstraints()
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)
final_state = (0., math.pi, 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state())
# dircol.AddLinearConstraint(dircol.final_state() == final_state)
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)
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
# 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.
# FloatingBaseType.kFixed)
# tree = RigidBodyTree(FindResource("cubli/cubli.sdf"),
# FloatingBaseType.kFixed)
builder = DiagramBuilder()
# AddModelInstancesFromSdfString(
# open("underactuated/src/cubli/cubli.sdf", 'r').read(),
# FloatingBaseType.kFixed,
# None, tree)
# tree
tree = RigidBodyTree(FindResource("cubli/cubli.urdf"), FloatingBaseType.kFixed)
#
# AddFlatTerrainToWorld(tree, 1000, -1)
# plant = RigidBodyPlant(tree, timestep)
plant = RigidBodyPlant(tree)
nx = tree.get_num_positions() + tree.get_num_velocities()
allmaterials = CompliantMaterial()
allmaterials.set_youngs_modulus(1E9) # default 1E9
allmaterials.set_dissipation(1.0) # default 0.32
allmaterials.set_friction(1.0) # default 0.9.
plant.set_default_compliant_material(allmaterials)
context = plant.CreateDefaultContext()
print(tree.get_num_positions())
# ETHAN
robot = builder.AddSystem(plant)
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
# 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)
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))
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
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
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
