def __init__(self):
LeafSystem.__init__(self)
self.DeclareVectorInputPort("touchdown_angle", BasicVector(1))
self.DeclareContinuousState(BasicVector(np.zeros(8)), 4, 4, 0)
self.DeclareVectorOutputPort("state", BasicVector(8),
self.CopyStateOut)
# Parameters from Geyer05, p.23
self.mass = 80. # kg
self.r0 = 1. # m
self.gravity = 9.81 # m/s^2
# Define spring constant in terms of the dimensionless number.
# Definition in section 2.4.3, values in figure 2.4.
# Note: Geyer05 says 10.8 (which doesn't work? -- I get no fixed pts).
dimensionless_spring_constant = 10.7
self.stiffness = (dimensionless_spring_constant * self.mass *
self.gravity / self.r0)
self.last_apex = None # placeholder for writing return map result.
self.touchdown_witness = self.MakeWitnessFunction(
"touchdown", WitnessFunctionDirection.kPositiveThenNonPositive,
self.foot_height, UnrestrictedUpdateEvent(self.touchdown))
self.takeoff_witness = self.MakeWitnessFunction(
"takeoff", WitnessFunctionDirection.kPositiveThenNonPositive,
self.leg_compression, UnrestrictedUpdateEvent(self.takeoff))
self.apex_witness = self.MakeWitnessFunction(
"apex", WitnessFunctionDirection.kPositiveThenNonPositive,
self.apex, PublishEvent(self.publish_apex))
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
self.called_continuous = False
self.called_discrete = False
self.called_initialize = False
self.called_per_step = False
self.called_periodic = False
self.called_getwitness = False
self.called_witness = False
self.called_guard = False
self.called_reset = False
self.called_system_reset = False
# Ensure we have desired overloads.
self.DeclarePeriodicPublish(1.0)
self.DeclarePeriodicPublish(1.0, 0)
self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.)
self.DeclarePeriodicDiscreteUpdate(period_sec=1.0,
offset_sec=0.)
self.DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=self._on_initialize))
self.DeclarePerStepEvent(
event=PublishEvent(trigger_type=TriggerType.kPerStep,
callback=self._on_per_step))
self.DeclarePeriodicEvent(
period_sec=1.0,
offset_sec=0.0,
event=PublishEvent(trigger_type=TriggerType.kPeriodic,
callback=self._on_periodic))
self.DeclareContinuousState(2)
self.DeclareDiscreteState(1)
# Ensure that we have inputs / outputs to call direct
# feedthrough.
self.DeclareInputPort(kUseDefaultName,
PortDataType.kVectorValued, 1)
self.DeclareVectorInputPort(name="test_input",
model_vector=BasicVector(1),
random_type=None)
self.DeclareVectorOutputPort("noop",
BasicVector(1),
noop,
prerequisites_of_calc=set(
[self.nothing_ticket()]))
self.witness = self.MakeWitnessFunction(
"witness", WitnessFunctionDirection.kCrossesZero,
self._witness)
# Test bindings for both callback function signatures.
self.reset_witness = self.MakeWitnessFunction(
"reset", WitnessFunctionDirection.kCrossesZero,
self._guard, UnrestrictedUpdateEvent(self._reset))
self.system_reset_witness = self.MakeWitnessFunction(
"system reset", WitnessFunctionDirection.kCrossesZero,
self._guard,
UnrestrictedUpdateEvent(
system_callback=self._system_reset))
def __init__(self):
LeafSystem.__init__(self)
self.DeclareVectorInputPort("u", BasicVector(4))
self.DeclareContinuousState(BasicVector(np.zeros(14)), 7, 7, 0)
self.DeclareVectorOutputPort("state", BasicVector(10),
self.CopyStateOut)
# Parameters from Geyer05, p.23
self.mass = np.array([0.5, 0.5, 0.5, 0.5, 0.5]) # kg
self.length = np.array([0.5, 0.5, 0.5, 0.5, 0.5]) # m
self.gravity = 9.81 # m/s^2
self.inertia = self.mass * self.length
self.left_impact_witness = self.MakeWitnessFunction(
"impact", WitnessFunctionDirection.kPositiveThenNonPositive,
self.left_foot_height, UnrestrictedUpdateEvent(self.switch))
self.right_impact_witness = self.MakeWitnessFunction(
"impact", WitnessFunctionDirection.kPositiveThenNonPositive,
self.right_foot_height, UnrestrictedUpdateEvent(self.switch))
def __init__(self, initial_value):
super().__init__()
self._input = self.DeclareAbstractInputPort(
"A", AbstractValue.Make(initial_value))
self.DeclareAbstractState(AbstractValue.Make(initial_value))
self.DeclareAbstractOutputPort(
'Q', lambda: AbstractValue.Make(initial_value),
self._calc_output_value, {self.all_state_ticket()})
self.DeclarePerStepEvent(
UnrestrictedUpdateEvent(self._move_input_to_state))
def __init__(self,
mbp,
symbol_list,
primitive_list,
dfa_json_file,
update_period=0.1):
LeafSystem.__init__(self)
self.mbp = mbp
# Our version of MBP context, which we'll modify
# in the publish method.
self.mbp_context = mbp.CreateDefaultContext()
self.set_name('task_execution_system')
# Take robot state vector as input.
self.DeclareVectorInputPort(
"mbp_state_vector",
BasicVector(mbp.num_positions() + mbp.num_velocities()))
# State of system is current RPYXYZ and gripper goals
self.DeclareAbstractState(AbstractValue.Make(self.TESState()))
self.DeclarePeriodicEvent(period_sec=update_period,
offset_sec=0.0,
event=UnrestrictedUpdateEvent(
self.UpdateAbstractState))
# Produce RPYXYZ goals + gripper goals
self.DeclareVectorOutputPort("rpy_xyz_desired", BasicVector(6),
self.CopyRpyXyzDesiredOut)
self.DeclareVectorOutputPort("wsg_position", BasicVector(1),
self.CopyWsgPositionOut)
# Load the JSON file
with open(dfa_json_file, 'r') as f:
json_data = json.load(f)
# Figure out which states in the JSON dict are environment
# symbols, and which are primitives that we can execute.
self.environment_symbol_indices = []
self.environment_symbols = []
self.action_primitive_indices = []
self.action_primitives = []
for var_i, var_name in enumerate(json_data["variables"]):
# Split into symbols and primitives.
found_symbol = None
found_primitive = None
for symbol in symbol_list:
if symbol.name == var_name:
if found_symbol is not None:
raise ValueError(
"Ambiguous matching symbol names provided for symbol {}."
.format(var_name))
found_symbol = symbol
for primitive in primitive_list:
if primitive.name == var_name:
if found_primitive is not None:
raise ValueError(
"Ambiguous matching primitive names provided for symbol {}."
.format(var_name))
found_primitive = primitive
if found_primitive and found_symbol:
raise ValueError(
"Both matching symbol AND primitive found for symbol {}.".
format(var_name))
if not found_primitive and not found_symbol:
raise ValueError(
"No matching symbol or primitive found for symbol {}.".
format(var_name))
if found_symbol is not None:
self.environment_symbol_indices.append(var_i)
self.environment_symbols.append(found_symbol)
elif found_primitive:
self.action_primitive_indices.append(var_i)
self.action_primitives.append(found_primitive)
# And now build the ultimate lookup dictionary containing,
# at each node name:
# ( [environment symbol truth vector for this state],
# [possibly-empty list of primitives that can be taken,
# [next state names]] )
self.state_lookup_table = {}
for node_name in json_data["nodes"].keys():
node_symbols = []
node_state = np.array(json_data["nodes"][node_name]["state"])
next_node_names = [
str(x) for x in json_data["nodes"][node_name]["trans"]
]
node_primitives = []
for i, prim in zip(self.action_primitive_indices,
self.action_primitives):
if node_state[i]:
node_primitives.append(prim)
self.state_lookup_table[node_name] = (
node_state[self.environment_symbol_indices], node_primitives,
next_node_names)