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_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_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)