def test_linearize_pendulum_kane_nonminimal(): # Create generalized coordinates and speeds for this non-minimal realization # q1, q2 = N.x and N.y coordinates of pendulum # u1, u2 = N.x and N.y velocities of pendulum q1, q2 = dynamicsymbols('q1:3') q1d, q2d = dynamicsymbols('q1:3', level=1) u1, u2 = dynamicsymbols('u1:3') u1d, u2d = dynamicsymbols('u1:3', level=1) L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum theta1 = atan(q2/q1) A = N.orientnew('A', 'axis', [theta1, N.z]) # Locate the pendulum mass P = pN.locatenew('P1', q1*N.x + q2*N.y) pP = Particle('pP', P, m) # Calculate the kinematic differential equations kde = Matrix([q1d - u1, q2d - u2]) dq_dict = solve(kde, [q1d, q2d]) # Set velocity of point P P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict)) # Configuration constraint is length of pendulum f_c = Matrix([P.pos_from(pN).magnitude() - L]) # Velocity constraint is that the velocity in the A.x direction is # always zero (the pendulum is never getting longer). f_v = Matrix([P.vel(N).express(A).dot(A.x)]) f_v.simplify() # Acceleration constraints is the time derivative of the velocity constraint f_a = f_v.diff(t) f_a.simplify() # Input the force resultant at P R = m*g*N.x # Derive the equations of motion using the KanesMethod class. KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1], u_dependent=[u1], configuration_constraints=f_c, velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde) with warns_deprecated_sympy(): (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Set the operating point to be straight down, and non-moving q_op = {q1: L, q2: 0} u_op = {u1: 0, u2: 0} ud_op = {u1d: 0, u2d: 0} A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True, simplify=True) assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]]) assert B == Matrix([])
def linearize(self, op_point=None, A_and_B=False, simplify=False): """Linearize the system about the operating point. Note that q_op, u_op, qd_op, ud_op must satisfy the equations of motion. These may be either symbolic or numeric. Parameters ---------- op_point : dict or iterable of dicts, optional Dictionary or iterable of dictionaries containing the operating point conditions. These will be substituted in to the linearized system before the linearization is complete. Leave blank if you want a completely symbolic form. Note that any reduction in symbols (whether substituted for numbers or expressions with a common parameter) will result in faster runtime. A_and_B : bool, optional If A_and_B=False (default), (M, A, B) is returned for forming [M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True, (A, B) is returned for forming dx = [A]x + [B]r, where x = [q_ind, u_ind]^T. simplify : bool, optional Determines if returned values are simplified before return. For large expressions this may be time consuming. Default is False. Potential Issues ---------------- Note that the process of solving with A_and_B=True is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. More values may then be substituted in to these matrices later on. The state space form can then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where P = Linearizer.perm_mat. """ # Run the setup if needed: if not self._setup_done: self._setup() # Compose dict of operating conditions if isinstance(op_point, dict): op_point_dict = op_point elif isinstance(op_point, Iterable): op_point_dict = {} for op in op_point: op_point_dict.update(op) else: op_point_dict = {} # Extract dimension variables l, m, n, o, s, k = self._dims # Rename terms to shorten expressions M_qq = self._M_qq M_uqc = self._M_uqc M_uqd = self._M_uqd M_uuc = self._M_uuc M_uud = self._M_uud M_uld = self._M_uld A_qq = self._A_qq A_uqc = self._A_uqc A_uqd = self._A_uqd A_qu = self._A_qu A_uuc = self._A_uuc A_uud = self._A_uud B_u = self._B_u C_0 = self._C_0 C_1 = self._C_1 C_2 = self._C_2 # Build up Mass Matrix # |M_qq 0_nxo 0_nxk| # M = |M_uqc M_uuc 0_mxk| # |M_uqd M_uud M_uld| if o != 0: col2 = Matrix([zeros(n, o), M_uuc, M_uud]) if k != 0: col3 = Matrix([zeros(n + m, k), M_uld]) if n != 0: col1 = Matrix([M_qq, M_uqc, M_uqd]) if o != 0 and k != 0: M = col1.row_join(col2).row_join(col3) elif o != 0: M = col1.row_join(col2) else: M = col1 elif k != 0: M = col2.row_join(col3) else: M = col2 M_eq = msubs(M, op_point_dict) # Build up state coefficient matrix A # |(A_qq + A_qu*C_1)*C_0 A_qu*C_2| # A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2| # |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2| # Col 1 is only defined if n != 0 if n != 0: r1c1 = A_qq if o != 0: r1c1 += (A_qu * C_1) r1c1 = r1c1 * C_0 if m != 0: r2c1 = A_uqc if o != 0: r2c1 += (A_uuc * C_1) r2c1 = r2c1 * C_0 else: r2c1 = Matrix() if o - m + k != 0: r3c1 = A_uqd if o != 0: r3c1 += (A_uud * C_1) r3c1 = r3c1 * C_0 else: r3c1 = Matrix() col1 = Matrix([r1c1, r2c1, r3c1]) else: col1 = Matrix() # Col 2 is only defined if o != 0 if o != 0: if n != 0: r1c2 = A_qu * C_2 else: r1c2 = Matrix() if m != 0: r2c2 = A_uuc * C_2 else: r2c2 = Matrix() if o - m + k != 0: r3c2 = A_uud * C_2 else: r3c2 = Matrix() col2 = Matrix([r1c2, r2c2, r3c2]) else: col2 = Matrix() if col1: if col2: Amat = col1.row_join(col2) else: Amat = col1 else: Amat = col2 Amat_eq = msubs(Amat, op_point_dict) # Build up the B matrix if there are forcing variables # |0_(n + m)xs| # B = |B_u | if s != 0 and o - m + k != 0: Bmat = zeros(n + m, s).col_join(B_u) Bmat_eq = msubs(Bmat, op_point_dict) else: Bmat_eq = Matrix() # kwarg A_and_B indicates to return A, B for forming the equation # dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T, if A_and_B: A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq) if Bmat_eq: B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq) else: # Bmat = Matrix([]), so no need to sub B_cont = Bmat_eq if simplify: A_cont.simplify() B_cont.simplify() return A_cont, B_cont # Otherwise return M, A, B for forming the equation # [M]dx = [A]x + [B]r, where x = [q, u]^T else: if simplify: M_eq.simplify() Amat_eq.simplify() Bmat_eq.simplify() return M_eq, Amat_eq, Bmat_eq
def linearize(self, op_point=None, A_and_B=False, simplify=False): """Linearize the system about the operating point. Note that q_op, u_op, qd_op, ud_op must satisfy the equations of motion. These may be either symbolic or numeric. Parameters ---------- op_point : dict or iterable of dicts, optional Dictionary or iterable of dictionaries containing the operating point conditions. These will be substituted in to the linearized system before the linearization is complete. Leave blank if you want a completely symbolic form. Note that any reduction in symbols (whether substituted for numbers or expressions with a common parameter) will result in faster runtime. A_and_B : bool, optional If A_and_B=False (default), (M, A, B) is returned for forming [M]*[q, u]^T = [A]*[q_ind, u_ind]^T + [B]r. If A_and_B=True, (A, B) is returned for forming dx = [A]x + [B]r, where x = [q_ind, u_ind]^T. simplify : bool, optional Determines if returned values are simplified before return. For large expressions this may be time consuming. Default is False. Potential Issues ---------------- Note that the process of solving with A_and_B=True is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. More values may then be substituted in to these matrices later on. The state space form can then be found as A = P.T*M.LUsolve(A), B = P.T*M.LUsolve(B), where P = Linearizer.perm_mat. """ # Run the setup if needed: if not self._setup_done: self._setup() # Compose dict of operating conditions if isinstance(op_point, dict): op_point_dict = op_point elif isinstance(op_point, Iterable): op_point_dict = {} for op in op_point: op_point_dict.update(op) else: op_point_dict = {} # Extract dimension variables l, m, n, o, s, k = self._dims # Rename terms to shorten expressions M_qq = self._M_qq M_uqc = self._M_uqc M_uqd = self._M_uqd M_uuc = self._M_uuc M_uud = self._M_uud M_uld = self._M_uld A_qq = self._A_qq A_uqc = self._A_uqc A_uqd = self._A_uqd A_qu = self._A_qu A_uuc = self._A_uuc A_uud = self._A_uud B_u = self._B_u C_0 = self._C_0 C_1 = self._C_1 C_2 = self._C_2 # Build up Mass Matrix # |M_qq 0_nxo 0_nxk| # M = |M_uqc M_uuc 0_mxk| # |M_uqd M_uud M_uld| if o != 0: col2 = Matrix([zeros(n, o), M_uuc, M_uud]) if k != 0: col3 = Matrix([zeros(n + m, k), M_uld]) if n != 0: col1 = Matrix([M_qq, M_uqc, M_uqd]) if o != 0 and k != 0: M = col1.row_join(col2).row_join(col3) elif o != 0: M = col1.row_join(col2) else: M = col1 elif k != 0: M = col2.row_join(col3) else: M = col2 M_eq = msubs(M, op_point_dict) # Build up state coefficient matrix A # |(A_qq + A_qu*C_1)*C_0 A_qu*C_2| # A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2| # |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2| # Col 1 is only defined if n != 0 if n != 0: r1c1 = A_qq if o != 0: r1c1 += (A_qu * C_1) r1c1 = r1c1 * C_0 if m != 0: r2c1 = A_uqc if o != 0: r2c1 += (A_uuc * C_1) r2c1 = r2c1 * C_0 else: r2c1 = Matrix() if o - m + k != 0: r3c1 = A_uqd if o != 0: r3c1 += (A_uud * C_1) r3c1 = r3c1 * C_0 else: r3c1 = Matrix() col1 = Matrix([r1c1, r2c1, r3c1]) else: col1 = Matrix() # Col 2 is only defined if o != 0 if o != 0: if n != 0: r1c2 = A_qu * C_2 else: r1c2 = Matrix() if m != 0: r2c2 = A_uuc * C_2 else: r2c2 = Matrix() if o - m + k != 0: r3c2 = A_uud * C_2 else: r3c2 = Matrix() col2 = Matrix([r1c2, r2c2, r3c2]) else: col2 = Matrix() if col1: if col2: Amat = col1.row_join(col2) else: Amat = col1 else: Amat = col2 Amat_eq = msubs(Amat, op_point_dict) # Build up the B matrix if there are forcing variables # |0_(n + m)xs| # B = |B_u | if s != 0 and o - m + k != 0: Bmat = zeros(n + m, s).col_join(B_u) Bmat_eq = msubs(Bmat, op_point_dict) else: Bmat_eq = Matrix() # kwarg A_and_B indicates to return A, B for forming the equation # dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T, if A_and_B: A_cont = self.perm_mat.T * M_eq.LUsolve(Amat_eq) if Bmat_eq: B_cont = self.perm_mat.T * M_eq.LUsolve(Bmat_eq) else: # Bmat = Matrix([]), so no need to sub B_cont = Bmat_eq if simplify: A_cont.simplify() B_cont.simplify() return A_cont, B_cont # Otherwise return M, A, B for forming the equation # [M]dx = [A]x + [B]r, where x = [q, u]^T else: if simplify: M_eq.simplify() Amat_eq.simplify() Bmat_eq.simplify() return M_eq, Amat_eq, Bmat_eq
def test_linearize_pendulum_kane_nonminimal(): # Create generalized coordinates and speeds for this non-minimal realization # q1, q2 = N.x and N.y coordinates of pendulum # u1, u2 = N.x and N.y velocities of pendulum q1, q2 = dynamicsymbols('q1:3') q1d, q2d = dynamicsymbols('q1:3', level=1) u1, u2 = dynamicsymbols('u1:3') u1d, u2d = dynamicsymbols('u1:3', level=1) L, m, t = symbols('L, m, t') g = 9.8 # Compose world frame N = ReferenceFrame('N') pN = Point('N*') pN.set_vel(N, 0) # A.x is along the pendulum theta1 = atan(q2/q1) A = N.orientnew('A', 'axis', [theta1, N.z]) # Locate the pendulum mass P = pN.locatenew('P1', q1*N.x + q2*N.y) pP = Particle('pP', P, m) # Calculate the kinematic differential equations kde = Matrix([q1d - u1, q2d - u2]) dq_dict = solve(kde, [q1d, q2d]) # Set velocity of point P P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict)) # Configuration constraint is length of pendulum f_c = Matrix([P.pos_from(pN).magnitude() - L]) # Velocity constraint is that the velocity in the A.x direction is # always zero (the pendulum is never getting longer). f_v = Matrix([P.vel(N).express(A).dot(A.x)]) f_v.simplify() # Acceleration constraints is the time derivative of the velocity constraint f_a = f_v.diff(t) f_a.simplify() # Input the force resultant at P R = m*g*N.x # Derive the equations of motion using the KanesMethod class. KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1], u_dependent=[u1], configuration_constraints=f_c, velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde) with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=SymPyDeprecationWarning) (fr, frstar) = KM.kanes_equations([(P, R)], [pP]) # Set the operating point to be straight down, and non-moving q_op = {q1: L, q2: 0} u_op = {u1: 0, u2: 0} ud_op = {u1d: 0, u2d: 0} A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True, new_method=True, simplify=True) assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]]) assert B == Matrix([])