def test_linear_affine_system(self): # Just make sure linear system is spelled correctly. A = np.identity(2) B = np.array([[0], [1]]) f0 = np.array([[0], [0]]) C = np.array([[0, 1]]) D = [0] y0 = [0] system = LinearSystem(A, B, C, D) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_mutable_continuous_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), 0.) Co = ControllabilityMatrix(system) self.assertEqual(Co.shape, (2, 2)) self.assertFalse(IsControllable(system)) self.assertFalse(IsControllable(system, 1e-6)) Ob = ObservabilityMatrix(system) self.assertEqual(Ob.shape, (2, 2)) self.assertFalse(IsObservable(system)) system = AffineSystem(A, B, f0, C, D, y0, .1) self.assertEqual(system.get_input_port(0), system.get_input_port()) self.assertEqual(system.get_output_port(0), system.get_output_port()) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_discrete_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), .1) system.get_input_port(0).FixValue(context, 0) linearized = Linearize(system, context) self.assertTrue((linearized.A() == A).all()) taylor = FirstOrderTaylorApproximation(system, context) self.assertTrue((taylor.y0() == y0).all()) system = MatrixGain(D=A) self.assertTrue((system.D() == A).all())
def test_linear_affine_system(self): # Just make sure linear system is spelled correctly. A = np.identity(2) B = np.array([[0], [1]]) f0 = np.array([[0], [0]]) C = np.array([[0, 1]]) D = [0] y0 = [0] system = LinearSystem(A, B, C, D) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_mutable_continuous_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), 0.) x0 = np.array([1, 2]) system.configure_default_state(x0=x0) system.SetDefaultContext(context) np.testing.assert_equal( context.get_continuous_state_vector().CopyToVector(), x0) generator = RandomGenerator() system.SetRandomContext(context, generator) np.testing.assert_equal( context.get_continuous_state_vector().CopyToVector(), x0) system.configure_random_state(covariance=np.eye(2)) system.SetRandomContext(context, generator) self.assertNotEqual( context.get_continuous_state_vector().CopyToVector()[1], x0[1]) Co = ControllabilityMatrix(system) self.assertEqual(Co.shape, (2, 2)) self.assertFalse(IsControllable(system)) self.assertFalse(IsControllable(system, 1e-6)) Ob = ObservabilityMatrix(system) self.assertEqual(Ob.shape, (2, 2)) self.assertFalse(IsObservable(system)) system = AffineSystem(A, B, f0, C, D, y0, .1) self.assertEqual(system.get_input_port(0), system.get_input_port()) self.assertEqual(system.get_output_port(0), system.get_output_port()) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_discrete_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), .1) system.get_input_port(0).FixValue(context, 0) linearized = Linearize(system, context) self.assertTrue((linearized.A() == A).all()) taylor = FirstOrderTaylorApproximation(system, context) self.assertTrue((taylor.y0() == y0).all()) system = MatrixGain(D=A) self.assertTrue((system.D() == A).all()) system = TrajectoryAffineSystem(PiecewisePolynomial(A), PiecewisePolynomial(B), PiecewisePolynomial(f0), PiecewisePolynomial(C), PiecewisePolynomial(D), PiecewisePolynomial(y0), .1) self.assertEqual(system.get_input_port(0), system.get_input_port()) self.assertEqual(system.get_output_port(0), system.get_output_port()) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_discrete_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) for t in np.linspace(0., 1., 5): self.assertTrue((system.A(t) == A).all()) self.assertTrue((system.B(t) == B).all()) self.assertTrue((system.f0(t) == f0).all()) self.assertTrue((system.C(t) == C).all()) self.assertEqual(system.D(t), D) self.assertEqual(system.y0(t), y0) self.assertEqual(system.time_period(), .1) x0 = np.array([1, 2]) system.configure_default_state(x0=x0) system.SetDefaultContext(context) np.testing.assert_equal( context.get_discrete_state_vector().CopyToVector(), x0) generator = RandomGenerator() system.SetRandomContext(context, generator) np.testing.assert_equal( context.get_discrete_state_vector().CopyToVector(), x0) system.configure_random_state(covariance=np.eye(2)) system.SetRandomContext(context, generator) self.assertNotEqual( context.get_discrete_state_vector().CopyToVector()[1], x0[1]) system = TrajectoryLinearSystem(A=PiecewisePolynomial(A), B=PiecewisePolynomial(B), C=PiecewisePolynomial(C), D=PiecewisePolynomial(D), time_period=0.1) self.assertEqual(system.time_period(), .1) system.configure_default_state(x0=np.array([1, 2])) system.configure_random_state(covariance=np.eye(2))
def add_floating_gripper(self, gripper_name, gripper_path, arm_end_frame, gripper_base, X_gripper): parser = Parser(self._mbp, self._sg) gripper_model_id = parser.AddModelFromFile(gripper_path, gripper_name) self._model_ids[gripper_name] = gripper_model_id ee_base_frame = self._mbp.GetFrameByName(gripper_base, gripper_model_id) if arm_end_frame is not None: self._mbp.WeldFrames(arm_end_frame, ee_base_frame, X_gripper) # Add gripper controller stack gripper_controller = self._builder.AddSystem( SchunkWsgPositionController(0.001, 4000, 5)) mbp_to_gripper_gain = np.array([[-1., 1., 0., 0.], [0., 0., -1., 1.]]) mbp_to_gripper_state = self._builder.AddSystem( MatrixGain(mbp_to_gripper_gain)) def finalize_func(): builder = self._builder builder.Connect(gripper_controller.get_generalized_force_output_port(), self._mbp.get_actuation_input_port(gripper_model_id)) # Add Gripper ports if arm_end_frame is None: # Gripper is not welded # State goes [_, r, p, y, x, y, z, g1, g2, dr, dp, dy, dx, dy, dz, dg1, dg2] # Gripper state demux = self._builder.AddSystem(Demultiplexer([1, 6, 2, 6, 2])) mux = self._builder.AddSystem(Multiplexer(input_sizes=[2, 2])) builder.Connect(self._mbp.get_state_output_port(gripper_model_id), demux.get_input_port(0)) builder.Connect(demux.get_output_port(2), mux.get_input_port(0)) builder.Connect(demux.get_output_port(4), mux.get_input_port(1)) builder.Connect(mux.get_output_port(0), gripper_controller.get_state_input_port()) builder.Connect(mux.get_output_port(0), mbp_to_gripper_state.get_input_port(0)) else: builder.Connect(self._mbp.get_state_output_port(gripper_model_id), gripper_controller.get_state_input_port()) builder.Connect(self._mbp.get_state_output_port(gripper_model_id), mbp_to_gripper_state.get_input_port(0)) gripper_position_name = gripper_name + "_position" gripper_force_limit_name = gripper_name + "_force_limit" gripper_state_name = gripper_name + "_state_measured" gripper_force_meas_name = gripper_name + "_force_measured" gripper_gen_force_meas_name = gripper_name + "_gen_force_measured" gripper_full_state = gripper_name + "_full_state" self._port_names.extend([gripper_position_name, gripper_force_limit_name, gripper_state_name, gripper_force_meas_name, gripper_gen_force_meas_name, gripper_full_state]) builder.ExportInput(gripper_controller.get_desired_position_input_port(), gripper_position_name) builder.ExportInput(gripper_controller.get_force_limit_input_port(), gripper_force_limit_name) builder.ExportOutput(mbp_to_gripper_state.get_output_port(0), gripper_state_name) builder.ExportOutput(gripper_controller.get_grip_force_output_port(), gripper_force_meas_name) builder.ExportOutput(gripper_controller.get_generalized_force_output_port(), gripper_gen_force_meas_name) builder.ExportOutput(self._mbp.get_state_output_port(gripper_model_id), gripper_full_state) if arm_end_frame is None: gripper_spatial_position = gripper_name + "_spatial_position" gripper_spatial_velocity = gripper_name + "_spatial_velocity" self._port_names.extend([gripper_spatial_position, gripper_spatial_velocity]) builder.ExportOutput(demux.get_output_port(1), gripper_spatial_position) builder.ExportOutput(demux.get_output_port(3), gripper_spatial_velocity) self._finalize_functions.append(finalize_func)