def test_abstract_input_port_fix_string(self):
model_value = AbstractValue.Make("")
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
input_port = system.get_input_port(0)
# Fix to a literal.
input_port.FixValue(context, "Alpha")
value = input_port.Eval(context)
self.assertEqual(type(value), type(model_value.get_value()))
self.assertEqual(value, "Alpha")
# Fix to a type-erased string.
input_port.FixValue(context, AbstractValue.Make("Bravo"))
value = input_port.Eval(context)
self.assertEqual(type(value), type(model_value.get_value()))
self.assertEqual(value, "Bravo")
# Fix to a non-string.
with self.assertRaises(RuntimeError):
# A RuntimeError occurs when the Context detects that the
# type-erased Value objects are incompatible.
input_port.FixValue(context, AbstractValue.Make(1))
with self.assertRaises(TypeError):
# A TypeError occurs when pybind Value.set_value cannot match any
# overload for how to assign the argument into the erased storage.
input_port.FixValue(context, 1)
with self.assertRaises(TypeError):
input_port.FixValue(context, np.array([2.]))
def test_abstract_input_port_fix_string(self):
model_value = AbstractValue.Make("")
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
input_port = system.get_input_port(0)
# Fix to a literal.
input_port.FixValue(context, "Alpha")
value = input_port.Eval(context)
self.assertEqual(type(value), type(model_value.get_value()))
self.assertEqual(value, "Alpha")
# Fix to a type-erased string.
input_port.FixValue(context, AbstractValue.Make("Bravo"))
value = input_port.Eval(context)
self.assertEqual(type(value), type(model_value.get_value()))
self.assertEqual(value, "Bravo")
# Fix to a non-string.
with self.assertRaises(RuntimeError):
# A RuntimeError occurs when the Context detects that the
# type-erased Value objects are incompatible.
input_port.FixValue(context, AbstractValue.Make(1))
with self.assertRaises(TypeError):
# A TypeError occurs when pybind Value.set_value cannot match any
# overload for how to assign the argument into the erased storage.
input_port.FixValue(context, 1)
with self.assertRaises(TypeError):
input_port.FixValue(context, np.array([2.]))
def test_abstract_input_port_eval(self):
model_value = AbstractValue.Make("Hello World")
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
fixed = context.FixInputPort(0, copy.copy(model_value))
self.assertIsInstance(fixed.GetMutableData(), AbstractValue)
input_port = system.get_input_port(0)
value = input_port.Eval(context)
self.assertEqual(type(value), type(model_value.get_value()))
self.assertEqual(value, model_value.get_value())
value_abs = input_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(value_abs.get_value(), model_value.get_value())
def test_abstract_input_port_eval(self):
model_value = AbstractValue.Make("Hello World")
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
fixed = context.FixInputPort(0, copy.copy(model_value))
self.assertIsInstance(fixed.GetMutableData(), AbstractValue)
input_port = system.get_input_port(0)
value = input_port.Eval(context)
self.assertEqual(type(value), type(model_value.get_value()))
self.assertEqual(value, model_value.get_value())
value_abs = input_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(value_abs.get_value(), model_value.get_value())
def test_vector_input_port_fix(self):
np_zeros = np.array([0.])
model_value = AbstractValue.Make(BasicVector(np_zeros))
system = PassThrough(len(np_zeros))
context = system.CreateDefaultContext()
input_port = system.get_input_port(0)
# Fix to a scalar.
input_port.FixValue(context, 1.)
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np.array([1.]))
# Fix to an ndarray.
input_port.FixValue(context, np.array([2.]))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np.array([2.]))
# Fix to a BasicVector.
input_port.FixValue(context, BasicVector([3.]))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np.array([3.]))
# Fix to a type-erased BasicVector.
input_port.FixValue(context, AbstractValue.Make(BasicVector([4.])))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np.array([4.]))
# Fix to wrong-sized vector.
with self.assertRaises(RuntimeError):
input_port.FixValue(context, np.array([0., 1.]))
with self.assertRaises(RuntimeError):
input_port.FixValue(
context, AbstractValue.Make(BasicVector([0., 1.])))
# Fix to a non-vector.
with self.assertRaises(TypeError):
# A TypeError occurs when pybind Value.set_value cannot match any
# overload for how to assign the argument into the erased storage.
input_port.FixValue(context, "string")
with self.assertRaises(RuntimeError):
# A RuntimeError occurs when the Context detects that the
# type-erased Value objects are incompatible.
input_port.FixValue(context, AbstractValue.Make("string"))
def test_vector_input_port_fix(self):
np_zeros = np.array([0.])
model_value = AbstractValue.Make(BasicVector(np_zeros))
system = PassThrough(len(np_zeros))
context = system.CreateDefaultContext()
input_port = system.get_input_port(0)
# Fix to a scalar.
input_port.FixValue(context, 1.)
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np.array([1.]))
# Fix to an ndarray.
input_port.FixValue(context, np.array([2.]))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np.array([2.]))
# Fix to a BasicVector.
input_port.FixValue(context, BasicVector([3.]))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np.array([3.]))
# Fix to a type-erased BasicVector.
input_port.FixValue(context, AbstractValue.Make(BasicVector([4.])))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np.array([4.]))
# Fix to wrong-sized vector.
with self.assertRaises(RuntimeError):
input_port.FixValue(context, np.array([0., 1.]))
with self.assertRaises(RuntimeError):
input_port.FixValue(
context, AbstractValue.Make(BasicVector([0., 1.])))
# Fix to a non-vector.
with self.assertRaises(TypeError):
# A TypeError occurs when pybind Value.set_value cannot match any
# overload for how to assign the argument into the erased storage.
input_port.FixValue(context, "string")
with self.assertRaises(RuntimeError):
# A RuntimeError occurs when the Context detects that the
# type-erased Value objects are incompatible.
input_port.FixValue(context, AbstractValue.Make("string"))
def test_abstract_pass_through(self):
model_value = AbstractValue.Make("Hello world")
system = PassThrough(model_value)
context = system.CreateDefaultContext()
system.get_input_port(0).FixValue(context, model_value)
output = system.AllocateOutput()
input_eval = system.EvalAbstractInput(context, 0)
compare_value(self, input_eval, model_value)
system.CalcOutput(context, output)
output_value = output.get_data(0)
compare_value(self, output_value, model_value)
def test_vector_pass_through(self):
model_value = BasicVector([1., 2, 3])
system = PassThrough(model_value.size())
context = system.CreateDefaultContext()
system.get_input_port(0).FixValue(context, model_value)
output = system.AllocateOutput()
input_eval = system.EvalVectorInput(context, 0)
compare_value(self, input_eval, model_value)
system.CalcOutput(context, output)
output_value = output.get_vector_data(0)
compare_value(self, output_value, model_value)
def test_vector_input_port_eval(self):
np_value = np.array([1., 2., 3.])
model_value = AbstractValue.Make(BasicVector(np_value))
system = PassThrough(len(np_value))
context = system.CreateDefaultContext()
context.FixInputPort(0, np_value)
input_port = system.get_input_port(0)
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np_value)
value_abs = input_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray)
np.testing.assert_equal(value_abs.get_value().get_value(), np_value)
basic = input_port.EvalBasicVector(context)
self.assertEqual(type(basic), BasicVector)
self.assertEqual(type(basic.get_value()), np.ndarray)
np.testing.assert_equal(basic.get_value(), np_value)
def test_vector_input_port_eval(self):
np_value = np.array([1., 2., 3.])
model_value = AbstractValue.Make(BasicVector(np_value))
system = PassThrough(len(np_value))
context = system.CreateDefaultContext()
context.FixInputPort(0, np_value)
input_port = system.get_input_port(0)
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np_value)
value_abs = input_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray)
np.testing.assert_equal(value_abs.get_value().get_value(), np_value)
basic = input_port.EvalBasicVector(context)
self.assertEqual(type(basic), BasicVector)
self.assertEqual(type(basic.get_value()), np.ndarray)
np.testing.assert_equal(basic.get_value(), np_value)
def test_abstract_input_port_fix_object(self):
# The port type is py::object, not any specific C++ type.
model_value = AbstractValue.Make(object())
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
input_port = system.get_input_port(0)
# Fix to a type-erased py::object.
input_port.FixValue(context, AbstractValue.Make(object()))
# Fix to an int.
input_port.FixValue(context, 1)
value = input_port.Eval(context)
self.assertEqual(type(value), int)
self.assertEqual(value, 1)
# Fixing to an explicitly-typed Value instantiation is an error ...
with self.assertRaises(RuntimeError):
input_port.FixValue(context, AbstractValue.Make("string"))
# ... but implicit typing works just fine.
input_port.FixValue(context, "string")
value = input_port.Eval(context)
self.assertEqual(type(value), str)
self.assertEqual(value, "string")
def test_abstract_input_port_fix_object(self):
# The port type is py::object, not any specific C++ type.
model_value = AbstractValue.Make(object())
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
input_port = system.get_input_port(0)
# Fix to a type-erased py::object.
input_port.FixValue(context, AbstractValue.Make(object()))
# Fix to an int.
input_port.FixValue(context, 1)
value = input_port.Eval(context)
self.assertEqual(type(value), int)
self.assertEqual(value, 1)
# Fixing to an explicitly-typed Value instantation is an error ...
with self.assertRaises(RuntimeError):
input_port.FixValue(context, AbstractValue.Make("string"))
# ... but implicit typing works just fine.
input_port.FixValue(context, "string")
value = input_port.Eval(context)
self.assertEqual(type(value), str)
self.assertEqual(value, "string")
def test_default_vector_pass_through(self):
model_value = [1., 2, 3]
system = PassThrough(value=model_value)
context = system.CreateDefaultContext()
np.testing.assert_array_equal(model_value,
system.get_output_port().Eval(context))
def add_arm_gripper(self, arm_name, arm_path, arm_base, X_arm,
gripper_path, arm_ee, gripper_base, X_gripper):
# Add arm
parser = Parser(self._mbp, self._sg)
arm_model_id = parser.AddModelFromFile(arm_path, arm_name)
arm_base_frame = self._mbp.GetFrameByName(arm_base, arm_model_id)
self._mbp.WeldFrames(self._mbp.world_frame(), arm_base_frame, X_arm)
self._model_ids[arm_name] = arm_model_id
# Add gripper
gripper_name = arm_name+"_gripper"
arm_end_frame = self._mbp.GetFrameByName(arm_ee, arm_model_id)
self.add_floating_gripper(gripper_name, gripper_path, arm_end_frame, gripper_base, X_gripper)
# Add arm controller stack
ctrl_plant = MultibodyPlant(0)
parser = Parser(ctrl_plant)
ctrl_arm_id = parser.AddModelFromFile(arm_path, arm_name)
arm_base_frame = ctrl_plant.GetFrameByName(arm_base, ctrl_arm_id)
ctrl_plant.WeldFrames(ctrl_plant.world_frame(), arm_base_frame, X_arm)
gripper_equivalent = ctrl_plant.AddRigidBody(
gripper_name+"_equivalent", ctrl_arm_id,
self.calculate_ee_composite_inertia(gripper_path))
arm_end_frame = ctrl_plant.GetFrameByName(arm_ee, ctrl_arm_id)
ctrl_plant.WeldFrames(arm_end_frame, gripper_equivalent.body_frame(),
X_gripper)
ctrl_plant.Finalize()
self._control_mbp[arm_name] = ctrl_plant
arm_num_positions = ctrl_plant.num_positions(ctrl_arm_id)
kp = 4000*np.ones(arm_num_positions)
ki = 0 * np.ones(arm_num_positions)
kd = 5*np.sqrt(kp)
arm_controller = self._builder.AddSystem(InverseDynamicsController(
ctrl_plant, kp, ki, kd, False))
adder = self._builder.AddSystem(Adder(2, arm_num_positions))
state_from_position = self._builder.AddSystem(
StateInterpolatorWithDiscreteDerivative(
arm_num_positions, self._mbp.time_step(), True))
# Add command pass through and state splitter
arm_command = self._builder.AddSystem(PassThrough(arm_num_positions))
state_split = self._builder.AddSystem(Demultiplexer(
2*arm_num_positions, arm_num_positions))
def finalize_func():
builder = self._builder
# Export positions commanded
command_input_name = arm_name + "_position"
command_output_name = arm_name + "_position_commanded"
self._port_names.extend([command_input_name, command_output_name])
builder.ExportInput(arm_command.get_input_port(0), command_input_name)
builder.ExportOutput(arm_command.get_output_port(0), command_output_name)
# Export arm state ports
builder.Connect(self._mbp.get_state_output_port(arm_model_id),
state_split.get_input_port(0))
arm_q_name = arm_name + "_position_measured"
arm_v_name = arm_name + "_velocity_estimated"
arm_state_name = arm_name + "_state_measured"
self._port_names.extend([arm_q_name, arm_v_name, arm_state_name])
builder.ExportOutput(state_split.get_output_port(0), arm_q_name)
builder.ExportOutput(state_split.get_output_port(1), arm_v_name)
builder.ExportOutput(self._mbp.get_state_output_port(arm_model_id),
arm_state_name)
# Export controller stack ports
builder.Connect(self._mbp.get_state_output_port(arm_model_id),
arm_controller.get_input_port_estimated_state())
builder.Connect(arm_controller.get_output_port_control(),
adder.get_input_port(0))
builder.Connect(adder.get_output_port(0),
self._mbp.get_actuation_input_port(arm_model_id))
builder.Connect(state_from_position.get_output_port(0),
arm_controller.get_input_port_desired_state())
builder.Connect(arm_command.get_output_port(0),
state_from_position.get_input_port(0))
torque_input_name = arm_name + "_feedforward_torque"
torque_output_cmd_name = arm_name + "_torque_commanded"
torque_output_est_name = arm_name + "_torque_measured"
self._port_names.extend([torque_input_name, torque_output_cmd_name,
torque_output_est_name])
builder.ExportInput(adder.get_input_port(1), torque_input_name)
builder.ExportOutput(adder.get_output_port(0), torque_output_cmd_name)
builder.ExportOutput(adder.get_output_port(0), torque_output_est_name)
external_torque_name = arm_name + "_torque_external"
self._port_names.append(external_torque_name)
builder.ExportOutput(
self._mbp.get_generalized_contact_forces_output_port(arm_model_id),
external_torque_name)
self._finalize_functions.append(finalize_func)
