def __init__(self):
VectorSystem.__init__(
self,
0, # Zero inputs.
1) # One output.
self.DeclareDiscreteState(1) # One state variable.
self.DeclarePeriodicDiscreteUpdate(1.0) # One second timestep.
def __init__(self):
VectorSystem.__init__(
self,
2, # two inputs
6) # six outputs
self._DeclareContinuousState(6) # six state variables
self.num_states = 6 # number of states
self.num_inputs = 2 # number of inputs
self.g = 9.81 # gravity
self.m = 0.15 # mass (kg)
self.S = 0.12 # wing area (m^2)
self.rho = 1.2 # air density ~SL
self.cbar = 0.12 # mean chord
self.I_y = 0.004 # pitching mom. of inertia
# pitching moment coefficients
self.CM_alpha = -0.08
self.CM_q = -0.2
self.CM_de = -0.04
self.mp_result = None # SolutionResult from trajOpt SNOPT program
self.K = None # LQR gain matrix at each knot point
self.S_lqr = None # LQR cost-to-go matrix at each knot point
self.poly_order = 4 # order of polynomial for SOS ROA estimation
def __init__(self):
self.output_size = 3
VectorSystem.__init__(
self,
0, # Zero inputs.
self.output_size) # One output.
self.DeclareContinuousState(1) # One state variable.
def __init__(self, min, max, value=0, description=""):
# 0 inputs, 1 output.
VectorSystem.__init__(self, 0, 1)
self.slider = FloatSlider(value=value,
description=description,
min=min,
max=max,
continuous_update=True)
if get_ipython() is not None:
display(self.slider)
def __init__(self, is_discrete):
# VectorSystem only supports pure Continuous or pure Discrete.
# Dimensions:
# 1 Input, 2 States, 3 Outputs.
VectorSystem.__init__(self, 1, 3)
self._is_discrete = is_discrete
if self._is_discrete:
self.DeclareDiscreteState(2)
else:
self.DeclareContinuousState(2)
# Record calls for testing.
self.has_called = []
def __init__(self, is_discrete):
# VectorSystem only supports pure Continuous or pure Discrete.
# Dimensions:
# 1 Input, 2 States, 3 Outputs.
VectorSystem.__init__(self, 1, 3)
self._is_discrete = is_discrete
if self._is_discrete:
self.DeclareDiscreteState(2)
else:
self.DeclareContinuousState(2)
# Record calls for testing.
self.has_called = []
def __init__(self, ax, title, min, max):
# 0 inputs, 1 output.
VectorSystem.__init__(self, 0, 1)
self.value = 0
self.slider = Slider(ax, title, min, max, valinit=self.value)
self.slider.on_changed(self.update)
def __init__(self):
# One input, one output, two state variables.
VectorSystem.__init__(self, 1, 1)
self._DeclareContinuousState(2)
def __init__(self,
robot,
lower_limit=-10.,
upper_limit=10.,
resolution=-1,
length=200,
update_period_sec=0.005,
window=None,
title=None):
""""
Args:
robot: A MultibodyPlant.
lower_limit: A scalar or vector of length robot.num_positions().
The lower limit of the slider will be the maximum
value of this number and any limit specified in the
Joint.
upper_limit: A scalar or vector of length robot.num_positions().
The upper limit of the slider will be the minimum
value of this number and any limit specified in the
Joint.
resolution: A scalar or vector of length robot.num_positions()
that specifies the discretization of the slider. Use
-1 (the default) to disable any rounding.
update_period_sec: Specifies how often the window update() method
gets called.
window: Optionally pass in a tkinter.Tk() object to add these
widgets to. Default behavior is to create a new
window.
title: The string that appears as the title of the gui
window. Use None to generate a default title. This
parameter is only used if a window==None.
"""
VectorSystem.__init__(self, 0, robot.num_positions())
def _reshape(x, num):
x = np.array(x)
assert len(x.shape) <= 1
return np.array(x) * np.ones(num)
lower_limit = _reshape(lower_limit, robot.num_positions())
upper_limit = _reshape(upper_limit, robot.num_positions())
resolution = _reshape(resolution, robot.num_positions())
if title is None:
if robot.num_model_instances() == 1:
title = robot.GetModelInstanceName(0) + " Joints"
else:
title = "Multibody Joints"
if window is None:
self.window = tk.Tk()
self.window.title(title)
else:
self.window = window
# Schedule window updates in either case (new or existing window):
self._DeclarePeriodicPublish(update_period_sec, 0.0)
self._slider = []
self._slider_position_start = []
context = robot.CreateDefaultContext()
state = robot.tree().GetPositionsAndVelocities(context)
self._default_position = state[:robot.num_positions()]
k = 0
for i in range(0, robot.num_joints()):
joint = robot.tree().get_joint(JointIndex(i))
low = joint.lower_limits()
upp = joint.upper_limits()
for j in range(0, joint.num_positions()):
self._slider_position_start.append(joint.position_start() + j)
self._slider.append(
tk.Scale(self.window,
from_=max(low[j], lower_limit[k]),
to=min(upp[j], upper_limit[k]),
resolution=resolution[k],
label=joint.name(),
length=length,
orient=tk.HORIZONTAL))
self._slider[k].pack()
k += 1
def __init__(self,
robot,
lower_limit=-10.,
upper_limit=10.,
resolution=-1,
update_period_sec=0.1,
title=None):
""""
Args:
robot: A MultibodyPlant.
lower_limit: A scalar or vector of length robot.num_positions().
The lower limit of the slider will be the maximum
value of this number and any limit specified in the
Joint.
upper_limit: A scalar or vector of length robot.num_positions().
The upper limit of the slider will be the minimum
value of this number and any limit specified in the
Joint.
resolution: A scalar or vector of length robot.num_positions()
that specifies the discretization of the slider. Use
-1 (the default) to disable any rounding.
update_period_sec: Specifies how often the gui update() method gets
called.
title: The string that appears as the title of the gui
window. Use None to generate a default title.
"""
VectorSystem.__init__(self, 0, robot.num_positions())
self._DeclarePeriodicPublish(update_period_sec, 0.0)
def _reshape(x, num):
x = np.array(x)
assert len(x.shape) <= 1
return np.array(x) * np.ones(num)
lower_limit = _reshape(lower_limit, robot.num_positions())
upper_limit = _reshape(upper_limit, robot.num_positions())
resolution = _reshape(resolution, robot.num_positions())
if title is None:
if robot.num_model_instances() == 1:
title = robot.GetModelInstanceName(0) + " Joints"
else:
title = "Multibody Joints"
self.root = tk.Tk()
self.root.title(title)
self.slider = []
k = 0
for i in range(0, robot.num_joints()):
joint = robot.tree().get_joint(JointIndex(i))
low = joint.lower_limits()
upp = joint.upper_limits()
for j in range(0, joint.num_positions()):
self.slider.append(
tk.Scale(self.root,
from_=max(low[j], lower_limit[k]),
to=min(upp[j], upper_limit[k]),
resolution=resolution[k],
label=joint.name(),
length=200,
orient=tk.HORIZONTAL))
self.slider[k].pack()
k += 1
def __init__(self):
VectorSystem.__init__(
self,
m, # No. of inputs.
n) # No. of output.
self._DeclareContinuousState(n)
def __init__(self, robot, lower_limit=-10., upper_limit=10.,
resolution=-1, length=200, update_period_sec=0.005,
window=None, title=None):
""""
Args:
robot: A MultibodyPlant.
lower_limit: A scalar or vector of length robot.num_positions().
The lower limit of the slider will be the maximum
value of this number and any limit specified in the
Joint.
upper_limit: A scalar or vector of length robot.num_positions().
The upper limit of the slider will be the minimum
value of this number and any limit specified in the
Joint.
resolution: A scalar or vector of length robot.num_positions()
that specifies the discretization of the slider. Use
-1 (the default) to disable any rounding.
update_period_sec: Specifies how often the window update() method
gets called.
window: Optionally pass in a tkinter.Tk() object to add these
widgets to. Default behavior is to create a new
window.
title: The string that appears as the title of the gui
window. Use None to generate a default title. This
parameter is only used if a window==None.
"""
VectorSystem.__init__(self, 0, robot.num_positions())
def _reshape(x, num):
x = np.array(x)
assert len(x.shape) <= 1
return np.array(x) * np.ones(num)
lower_limit = _reshape(lower_limit, robot.num_positions())
upper_limit = _reshape(upper_limit, robot.num_positions())
resolution = _reshape(resolution, robot.num_positions())
if title is None:
if robot.num_model_instances() == 1:
title = robot.GetModelInstanceName(0) + " Joints"
else:
title = "Multibody Joints"
if window is None:
self.window = tk.Tk()
self.window.title(title)
else:
self.window = window
# Schedule window updates in either case (new or existing window):
self._DeclarePeriodicPublish(update_period_sec, 0.0)
self._slider = []
self._slider_position_start = []
context = robot.CreateDefaultContext()
state = robot.GetPositionsAndVelocities(context)
self._default_position = state[:robot.num_positions()]
k = 0
for i in range(0, robot.num_joints()):
joint = robot.get_joint(JointIndex(i))
low = joint.lower_limits()
upp = joint.upper_limits()
for j in range(0, joint.num_positions()):
self._slider_position_start.append(joint.position_start() + j)
self._slider.append(tk.Scale(self.window,
from_=max(low[j],
lower_limit[k]),
to=min(upp[j], upper_limit[k]),
resolution=resolution[k],
label=joint.name(),
length=length,
orient=tk.HORIZONTAL))
self._slider[k].pack()
k += 1
def __init__(self):
# One input, one output, two state variables.
VectorSystem.__init__(self, 1, 2, direct_feedthrough=False)
self.DeclareContinuousState(2)
def __init__(self, port_size, slider_names=None,
lower_limit=-10., upper_limit=10.,
resolution=-1, length=200, update_period_sec=0.0166,
window=None, title="System inputs"):
"""
Args:
port_size: Size of the input port that's being controlled. This
is the number of sliders that will show up.
slider_names: A list of strings describing the names of the sliders
that should be displayed.
lower_limit: The value(s) for the lower limits of each slider. See
class documentation for more details.
upper_limit: The value(s) for the upper limits of each slider. See
class documentation for more details.
resolution: A scalar or vector of length port_size
that specifies the discretization that the slider will
be rounded to. Use -1 (the default) to disable any
rounding. For example, a resolution of 0.1 will round
to the nearest 0.1. See class documentation for more
details.
length: The length of the sliders in pixels.
update_period_sec: Specifies how often the window update() method
gets called. Smaller values will theoretically make
GUI values available to the simulation more quickly,
but may require the simulation to take more steps than
necessary. The default value is suitable for most
applications.
window: Optionally pass in a tkinter.Tk() object to add these
widgets to. Default behavior is to create a new
window.
title: The string that appears as the title of the gui
window. Default title is "System sliders" This
parameter is only used if window is None.
"""
VectorSystem.__init__(self, 0, port_size)
if window is None:
self.window = tk.Tk()
self.window.title(title)
else:
self.window = window
self.port_size = port_size
if slider_names is None:
slider_names = ["Index " + str(i) for i in range(self.port_size)]
if len(slider_names) != self.port_size:
raise ValueError(
f"Slider names size ({len(slider_names)}) doesn't "
f"match port size ({self.port_size})")
def input_to_vector(x, desc):
"""
Turn scalar inputs into vector of size self.port_size.
Throws error if vector input is the wrong size,
otherwise returning the vector.
Args:
x: scalar or vector input.
desc: string describing the vector, used in error message.
"""
if np.isscalar(x):
return np.repeat(x, self.port_size)
if len(x) == self.port_size:
return x
raise ValueError(
f"Size of {desc} ({len(x)}) doesn't "
f"match port size ({self.port_size})"
)
lower_limit = input_to_vector(lower_limit, "lower_limit")
upper_limit = input_to_vector(upper_limit, "upper_limit")
resolution = input_to_vector(resolution, "resolution")
# Schedule window updates in either case (new or existing window):
self.DeclarePeriodicEvent(update_period_sec, 0.0,
PublishEvent(self._update_window))
self._sliders = []
# TODO: support a scroll bar for larger input sizes
for i in range(self.port_size):
slider = tk.Scale(self.window,
from_=lower_limit[i],
to=upper_limit[i],
resolution=resolution[i],
label=slider_names[i],
length=length,
orient=tk.HORIZONTAL)
slider.pack()
self._sliders.append(slider)
def __init__(self, robot, lower_limit=-10., upper_limit=10.,
resolution=-1, length=200, update_period_sec=0.005,
window=None, title=None):
""""
Args:
robot: A MultibodyPlant.
lower_limit: A scalar or vector of length robot.num_positions().
The lower limit of the slider will be the maximum
value of this number and any limit specified in the
Joint.
upper_limit: A scalar or vector of length robot.num_positions().
The upper limit of the slider will be the minimum
value of this number and any limit specified in the
Joint.
resolution: A scalar or vector of length robot.num_positions()
that specifies the discretization of the slider. Use
-1 (the default) to disable any rounding.
update_period_sec: Specifies how often the window update() method
gets called.
window: Optionally pass in a tkinter.Tk() object to add these
widgets to. Default behavior is to create a new
window.
title: The string that appears as the title of the gui
window. Use None to generate a default title. This
parameter is only used if a window==None.
"""
VectorSystem.__init__(self, 0, robot.num_positions())
def _reshape(x, num):
x = np.array(x)
assert len(x.shape) <= 1
return np.array(x) * np.ones(num)
def _frame_config(event):
"""
Execute once to configure the canvas size,
canvas contains scrollbar and slider_frame.
"""
self.canvas.configure(scrollregion=self.canvas.bbox("all"),
width=200,
height=min(robot.num_joints()*60, 400))
lower_limit = _reshape(lower_limit, robot.num_positions())
upper_limit = _reshape(upper_limit, robot.num_positions())
resolution = _reshape(resolution, robot.num_positions())
if title is None:
if robot.num_model_instances() == 1:
title = robot.GetModelInstanceName(0) + " Joints"
else:
title = "Multibody Joints"
if window is None:
self.window = tk.Tk()
self.window.title(title)
else:
self.window = window
# Schedule window updates in either case (new or existing window):
self.DeclarePeriodicPublish(update_period_sec, 0.0)
self.window.geometry("220x410")
# Allow only y dimension resizable.
self.window.resizable(0, 1)
# Once window is created, create window_frame
# and canvas that host slider_frame and scrollbar.
self.window_frame = tk.Frame(self.window, relief=tk.GROOVE, bd=1)
self.window_frame.place(x=0, y=0)
self.canvas = tk.Canvas(self.window_frame)
# Init slider_frame that actually contain the sliders
# and scrollbar.
self.slider_frame = tk.Frame(self.canvas)
self.scrollbar = tk.Scrollbar(self.window_frame,
orient="vertical",
command=self.canvas.yview)
self.canvas.configure(yscrollcommand=self.scrollbar.set)
# Put scrollbar and slider_frame into canvas.
self.scrollbar.pack(side="right", fill="y")
self.canvas.pack(side="left")
self.canvas.create_window((0, 0),
window=self.slider_frame,
anchor='nw')
# Call _frame_config to configure canvas size.
self.slider_frame.bind("<Configure>", _frame_config)
self._slider = []
self._slider_position_start = []
context = robot.CreateDefaultContext()
state = robot.GetPositionsAndVelocities(context)
self._default_position = state[:robot.num_positions()]
k = 0
for i in range(0, robot.num_joints()):
joint = robot.get_joint(JointIndex(i))
low = joint.position_lower_limits()
upp = joint.position_upper_limits()
for j in range(0, joint.num_positions()):
self._slider_position_start.append(joint.position_start() + j)
self._slider.append(tk.Scale(self.slider_frame,
from_=max(low[j],
lower_limit[k]),
to=min(upp[j], upper_limit[k]),
resolution=resolution[k],
label=joint.name(),
length=length,
orient=tk.HORIZONTAL))
self._slider[k].pack()
k += 1
def __init__(self,
robot,
lower_limit=-10.,
upper_limit=10.,
resolution=-1,
length=200,
update_period_sec=0.005,
window=None,
title=None):
""""
Args:
robot: A MultibodyPlant.
lower_limit: A scalar or vector of length robot.num_positions().
The lower limit of the slider will be the maximum
value of this number and any limit specified in the
Joint.
upper_limit: A scalar or vector of length robot.num_positions().
The upper limit of the slider will be the minimum
value of this number and any limit specified in the
Joint.
resolution: A scalar or vector of length robot.num_positions()
that specifies the discretization of the slider. Use
-1 (the default) to disable any rounding.
update_period_sec: Specifies how often the window update() method
gets called.
window: Optionally pass in a tkinter.Tk() object to add these
widgets to. Default behavior is to create a new
window.
title: The string that appears as the title of the gui
window. Use None to generate a default title. This
parameter is only used if a window==None.
"""
VectorSystem.__init__(self, 0, robot.num_positions())
def _reshape(x, num):
x = np.array(x)
assert len(x.shape) <= 1
return np.array(x) * np.ones(num)
def _frame_config(event):
"""
Execute once to configure the canvas size,
canvas contains scrollbar and slider_frame.
"""
self.canvas.configure(scrollregion=self.canvas.bbox("all"),
width=200,
height=min(robot.num_joints() * 60, 400))
lower_limit = _reshape(lower_limit, robot.num_positions())
upper_limit = _reshape(upper_limit, robot.num_positions())
resolution = _reshape(resolution, robot.num_positions())
if title is None:
if robot.num_model_instances() == 1:
title = robot.GetModelInstanceName(0) + " Joints"
else:
title = "Multibody Joints"
if window is None:
self.window = tk.Tk()
self.window.title(title)
else:
self.window = window
# Schedule window updates in either case (new or existing window):
self.DeclarePeriodicPublish(update_period_sec, 0.0)
self.window.geometry("220x410")
# Allow only y dimension resizable.
self.window.resizable(0, 1)
# Once window is created, create window_frame
# and canvas that host slider_frame and scrollbar.
self.window_frame = tk.Frame(self.window, relief=tk.GROOVE, bd=1)
self.window_frame.place(x=0, y=0)
self.canvas = tk.Canvas(self.window_frame)
# Init slider_frame that actually contain the sliders
# and scrollbar.
self.slider_frame = tk.Frame(self.canvas)
self.scrollbar = tk.Scrollbar(self.window_frame,
orient="vertical",
command=self.canvas.yview)
self.canvas.configure(yscrollcommand=self.scrollbar.set)
# Put scrollbar and slider_frame into canvas.
self.scrollbar.pack(side="right", fill="y")
self.canvas.pack(side="left")
self.canvas.create_window((0, 0),
window=self.slider_frame,
anchor='nw')
# Call _frame_config to configure canvas size.
self.slider_frame.bind("<Configure>", _frame_config)
self._slider = []
self._slider_position_start = []
context = robot.CreateDefaultContext()
state = robot.GetPositionsAndVelocities(context)
self._default_position = state[:robot.num_positions()]
k = 0
for i in range(0, robot.num_joints()):
joint = robot.get_joint(JointIndex(i))
low = joint.position_lower_limits()
upp = joint.position_upper_limits()
for j in range(0, joint.num_positions()):
self._slider_position_start.append(joint.position_start() + j)
self._slider.append(
tk.Scale(self.slider_frame,
from_=max(low[j], lower_limit[k]),
to=min(upp[j], upper_limit[k]),
resolution=resolution[k],
label=joint.name(),
length=length,
orient=tk.HORIZONTAL))
self._slider[k].pack()
k += 1
def __init__(self):
VectorSystem.__init__(
self,
0, # Zero inputs.
1) # One output.
self._DeclareContinuousState(1) # One state variable.
def __init__(self):
VectorSystem.__init__(
self,
m, # No. of inputs.
n) # No. of output.
self.DeclareContinuousState(n) #OMKAR REMOVED UNDERSCORE
def __init__(self,
robot,
lower_limit=-10.,
upper_limit=10.,
resolution=0.01,
length=200,
update_period_sec=0.005):
""""
Args:
robot: A MultibodyPlant.
lower_limit: A scalar or vector of length robot.num_positions().
The lower limit of the slider will be the maximum
value of this number and any limit specified in the
Joint.
upper_limit: A scalar or vector of length robot.num_positions().
The upper limit of the slider will be the minimum
value of this number and any limit specified in the
Joint.
resolution: A scalar or vector of length robot.num_positions()
that specifies the discretization of the slider.
length: The length of the sliders.
update_period_sec: Specifies how often the window update() method
gets called.
"""
VectorSystem.__init__(self, 0, robot.num_positions())
# The widgets themselves have undeclared state. For now, we accept it,
# and simply disable caching on the output port.
self.get_output_port(0).disable_caching_by_default()
def _reshape(x, num):
x = np.array(x)
assert len(x.shape) <= 1
return np.array(x) * np.ones(num)
lower_limit = _reshape(lower_limit, robot.num_positions())
upper_limit = _reshape(upper_limit, robot.num_positions())
resolution = _reshape(resolution, robot.num_positions())
# Schedule window updates in either case (new or existing window):
self.DeclarePeriodicPublish(update_period_sec, 0.0)
self._slider = []
self._slider_position_start = []
context = robot.CreateDefaultContext()
self._default_position = robot.GetPositions(context)
k = 0
for i in range(0, robot.num_joints()):
joint = robot.get_joint(JointIndex(i))
low = joint.position_lower_limits()
upp = joint.position_upper_limits()
for j in range(0, joint.num_positions()):
index = joint.position_start() + j
description = joint.name()
if joint.num_positions() > 1:
description += f"[{j}]"
self._slider_position_start.append(index)
self._slider.append(
FloatSlider(value=self._default_position[index],
min=max(low[j], lower_limit[k]),
max=min(upp[j], upper_limit[k]),
step=resolution[k],
continuous_update=True,
description=description,
style={'description_width': 'initial'},
layout=Layout(width=f"'{length}'")))
display(self._slider[k])
k += 1
def __init__(self):
VectorSystem.__init__(self, 1, 1)
self._DeclareContinuousState(2)
def __init__(self):
# One input, one output, two state variables.
VectorSystem.__init__(self, 1, 2, direct_feedthrough=False)
self.DeclareContinuousState(2)
