def __init__(self, actuators):
LeafSystem.__init__(self)
self.num_actuators = len(actuators)
self.actuators_init = np.ones(self.num_actuators) * 0.0
self.robot_command_port_index = self._DeclareAbstractInputPort(
'robot_command_port',
AbstractValue.Make(littledog_command_t)).get_index()
self.desired_effort_port_indices = self._DeclareVectorOutputPort(
BasicVector(self.num_actuators), self.OutputActuation).get_index()
def __init__(self, rbtree, kp, ki, kd):
LeafSystem.__init__(self)
self.rb_tree = rbtree
self.num_controlled_q_ = self.rb_tree.get_num_actuators()
self.kp = kp
self.ki = ki
self.kd = kd
self.robot_state_port_index = self._DeclareAbstractInputPort(
'robot_state_port', AbstractValue.Make(robot_state_t)).get_index()
# self.state_ref_port_index = self._DeclareInputPort('State_Ref_Port', PortDataType.kVectorValued,
# self.num_controlled_q_ * 2).get_index()
self.robot_command_port_index = self._DeclareAbstractOutputPort(
'robot_command_port',
self._Allocator,
self._OutputCommand,
).get_index()
def _Allocator(self):
return AbstractValue.Make(robot_state_t)
def _Allocator(self):
return AbstractValue.Make(littledog_command_t)