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)