Python Matrix.cross方法代码示例

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import cross [as 别名]
def get_dp_bc_params(state):
    dp_bc_xy = Matrix(lam_bc_com(state[1], state[2]))
    dp_bc_xy_inertial = Matrix(lam_bc_com_inertial(state[0], state[1], state[2]))
    dp_bc_length = (dp_bc_xy[0]**2 + dp_bc_xy[1]**2)**0.5
    dp_bc_angle = -atan(dp_bc_xy[0]/dp_bc_xy[1]) #angle with respect to angle one
    dp_bc_ang_vel = (dp_bc_xy.cross(Matrix(lam_bc_com_dot(state[1], state[2], state[4], state[5]))))/norm(dp_bc_xy)**2
    dp_bc_params_dict = dict(zip(['bc_length', 'bc_theta', 'bc_ang_vel', 'bc_loc'], [dp_bc_length, dp_bc_angle, dp_bc_ang_vel[2], dp_bc_xy_inertial])) 
    return dp_bc_params_dict

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import cross [as 别名]
def test_util():
    v1 = Matrix(1,3,[1,2,3])
    v2 = Matrix(1,3,[3,4,5])
    assert v1.cross(v2) == Matrix(1,3,[-2,4,-2])
    assert v1.norm() == sqrt(14)
    # cofactor
    assert eye(3) == eye(3).cofactorMatrix()
    test = Matrix([[1,3,2],[2,6,3],[2,3,6]])
    assert test.cofactorMatrix() == Matrix([[27,-6,-6],[-12,2,3],[-3,1,0]])
    test = Matrix([[1,2,3],[4,5,6],[7,8,9]])
    assert test.cofactorMatrix() == Matrix([[-3,6,-3],[6,-12,6],[-3,6,-3]])

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import cross [as 别名]
def get_sp_des_xy_acc(params, state): #returns desired com x and y acceleration based off of PD control of the center of mass pendulum
    global k_p, k_v
    com_vel_xy = Matrix(lam_com_dot_eq(state[0], state[1], state[2], state[3], state[4], state[5]))
    com_ang_vel = (params.get('sp_com_xy').cross(com_vel_xy))/(norm(params.get('sp_com_xy')))**2
    com_ang_vel = com_ang_vel[2]
    com_u = -k_p*params.get('sp_theta') + -k_v*com_ang_vel #pd torque control

#    com_ang_acc = Matrix([0,0, lam_sp_eq(params.get('sp_length'), params.get('sp_theta'), com_u)]) #angular acceleration resulting from pd torque control
    com_ang_acc = Matrix([0,0, lam_sp_eq_torque_only(params.get('sp_length'), com_u)])
    com_ang_acc_xy = (com_ang_acc.cross(params.get('sp_com_xy')))
    com_ang_acc_dict = dict(zip(['ang_acc_x', 'ang_acc_y'], [com_ang_acc_xy[0], com_ang_acc_xy[1]]))
#    com_ang_acc_dict = dict(zip(['ang_acc_x', 'ang_acc_y'], [com_ang_acc_xy[0], 0.0]))
    return com_ang_acc_dict

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import cross [as 别名]

#.........这里部分代码省略.........

    # test_LUsolve
    A = SMatrix([[2,3,5],
                [3,6,2],
                [8,3,6]])
    x = SMatrix(3,1,[3,7,5])
    b = A*x
    soln = A.LUsolve(b)
    assert soln == x
    A = SMatrix([[0,-1,2],
                [5,10,7],
                [8,3,4]])
    x = SMatrix(3,1,[-1,2,5])
    b = A*x
    soln = A.LUsolve(b)
    assert soln == x

    # test_inverse
    A = eye(4)
    assert A.inv() == eye(4)
    assert A.inv("LU") == eye(4)
    assert A.inv("ADJ") == eye(4)
    A = SMatrix([[2,3,5],
                [3,6,2],
                [8,3,6]])
    Ainv = A.inv()
    assert A*Ainv == eye(3)
    assert A.inv("LU") == Ainv
    assert A.inv("ADJ") == Ainv

    # test_cross
    v1 = Matrix(1,3,[1,2,3])
    v2 = Matrix(1,3,[3,4,5])
    assert v1.cross(v2) == Matrix(1,3,[-2,4,-2])
    assert v1.norm(v1) == 14

    # test_cofactor
    assert eye(3) == eye(3).cofactorMatrix()
    test = SMatrix([[1,3,2],[2,6,3],[2,3,6]])
    assert test.cofactorMatrix() == SMatrix([[27,-6,-6],[-12,2,3],[-3,1,0]])
    test = SMatrix([[1,2,3],[4,5,6],[7,8,9]])
    assert test.cofactorMatrix() == SMatrix([[-3,6,-3],[6,-12,6],[-3,6,-3]])

    # test_jacobian
    x = Symbol('x')
    y = Symbol('y')
    L = SMatrix(1,2,[x**2*y, 2*y**2 + x*y])
    syms = [x,y]
    assert L.jacobian(syms) == Matrix([[2*x*y, x**2],[y, 4*y+x]])

    L = SMatrix(1,2,[x, x**2*y**3])
    assert L.jacobian(syms) == SMatrix([[1, 0], [2*x*y**3, x**2*3*y**2]])

    # test_QR
    A = Matrix([[1,2],[2,3]])
    Q, S = A.QRdecomposition()
    R = Rational
    assert Q == Matrix([[5**R(-1,2), (R(2)/5)*(R(1)/5)**R(-1,2)], [2*5**R(-1,2), (-R(1)/5)*(R(1)/5)**R(-1,2)]])
    assert S == Matrix([[5**R(1,2), 8*5**R(-1,2)], [0, (R(1)/5)**R(1,2)]])
    assert Q*S == A
    assert Q.T * Q == eye(2)

    # test nullspace
    # first test reduced row-ech form
    R = Rational

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import cross [as 别名]
  M=WRI*OHM;
  Qg[3] = atan2(M*(cos(Qg[0])*ay - sin(Qg[0])*ax) , M*(cos(Qg[0])*cos(Qg[1]+Qg[2])*ax + sin(Qg[0])*cos(Qg[1]+Qg[2])*ay - sin(Qg[1]+Qg[2])*az))
  Qg[4] = atan2((cos(Qg[0])*cos(Qg[1]+Qg[2])*cos(Qg[3]) - sin(Qg[0])*sin(Qg[3]))*ax + (sin(Qg[0])*cos(Qg[1]+Qg[2])*cos(Qg[3]) + cos(Qg[0])*sin(Qg[3]))*ay - cos(Qg      [3])*sin(Qg[1]+Qg[2])*az , cos(Qg[0])*sin(Qg[1]+Qg[2])*ax + sin(Qg[0])*sin(Qg[1]+Qg[2])*ay + cos(Qg[1]+Qg[2])*az)
  Qg[5] = atan2((-sin(Qg[0])*cos(Qg[3]) - cos(Qg[0])*cos(Qg[1]+Qg[2])*sin(Qg[3]))*nx + (cos(Qg[0])*cos(Qg[3]) - sin(Qg[0])*cos(Qg[1]+Qg[2])*sin(Qg[3]))*ny + sin(Qg[1]+Qg[2])*sin(Qg[3])*nz, (-sin(Qg[0])*cos(Qg[3]) - cos(Qg[0])*cos(Qg[1]+Qg[2])*sin(Qg[3]))*sx + (cos(Qg[0])*cos(Qg[3]) - sin(Qg[0])*cos(Qg[1]+Qg[2])*sin(Qg[3]))*sy + sin(Qg[1]+Qg[2])*sin(Qg[3])*sz)
  if(Qg[1]<=np.pi and Qg[1]>=np.pi/2):
    Qg[1]=Qg[1]-2*np.pi
  if(Qg[2]<=-np.pi/2 and Qg[2]>=-np.pi):
    Qg[2]=Qg[2]+2*np.pi
  return Qg
  
#Now Lets try finding the Jacobian
# First Let us try the cross-product method

P0=(A[0]*A[1]*A[2]*A[3]*A[4]*A[5])[0:3,3]
Z0=Matrix([[0],[0],[1]])
J0=Z0.cross(P0).transpose().col_join(Z0)

P1=(A[0]*A[1]*A[2]*A[3]*A[4]*A[5]-A[0])[0:3,3]
Z1=A[0][0:3,2]
J1=Z1.cross(P1).transpose().col_join(Z1)

P2=(A[0]*A[1]*A[2]*A[3]*A[4]*A[5]-A[0]*A[1])[0:3,3]
Z2=(A[0]*A[1])[0:3,2]
J2=Z2.cross(P2).transpose().col_join(Z2)

P3=(A[0]*A[1]*A[2]*A[3]*A[4]*A[5]-A[0]*A[1]*A[2])[0:3,3]
Z3=(A[0]*A[1]*A[2])[0:3,2]
J3=Z3.cross(P3).transpose().col_join(Z3)

P4=(A[0]*A[1]*A[2]*A[3]*A[4]*A[5]-A[0]*A[1]*A[2]*A[3])[0:3,3]
Z4=(A[0]*A[1]*A[2]*A[3])[0:3,2]

# 需要导入模块: from sympy import Matrix [as 别名]
# 或者: from sympy.Matrix import cross [as 别名]

#.........这里部分代码省略.........

        # Direction vector
        self.d2_vect = self.T2_CtoF2 * self.p2h

        return self.d2_vect

    def create_direction_vectors_symbols(self, as_real=None):
        self.d1x, self.d1y, self.d1z = symbols("v_1_x, v_1_y, v_1_z", real=as_real)
        self.d1_vect_as_symbol = Matrix([self.d1x, self.d1y, self.d1z])
        self.d2x, self.d2y, self.d2z = symbols("v_2_x, v_2_y, v_2_z", real=as_real)
        self.d2_vect_as_symbol = Matrix([self.d2x, self.d2y, self.d2z])
        self.direction_vectors_as_symb = Matrix([self.d1_vect_as_symbol, self.d2_vect_as_symbol])

    def get_triangulated_point_expanded(self):
        # Substitute expanded symbols
        reps_for_lambda_solns = [(self.d1x, self.d1_vect[0]), (self.d1y, self.d1_vect[1]), (self.d1z, self.d1_vect[2]), (self.d2x, self.d2_vect[0]), (self.d2y, self.d2_vect[1]), (self.d2z, self.d2_vect[2])]
        self.lambdaG1_expanded = self.lambdaG1.subs(reps_for_lambda_solns)
        self.lambda_perp_expanded = self.lambda_perp.subs(reps_for_lambda_solns)
        self.perp_vect_unit_expanded = self.perp_vect_unit_as_symbol.subs(reps_for_lambda_solns)
        self.G1_expanded = self.f1 + self.lambdaG1_expanded * self.d1_vect

        self.mid_Pw_expanded = self.G1_expanded + self.lambda_perp_expanded / 2.0 * self.perp_vect_unit_expanded
        return self.mid_Pw_expanded

    def compute_triangulated_point(self):
        # Solving using the common perpendicular method

        # Step 1. Create symbolic variables.
        self.create_direction_vectors_symbols(as_real=True)
        self.lambdaG1, self.lambdaG2, self.lambda_perp = symbols("lambda_G_1, lambda_G_2, lambda_{perp}", nonnegative=True, real=True)
        # To simplify computation, treat the direction vectors as independent symbolic variables
        # self.nx, self.ny, self.nz = symbols("n_x, n_y, n_z", real=True)
        # self.perp_vect_unit = Matrix([self.nx, self.ny, self.nz])
        d1_cross_d2 = self.d1_vect_as_symbol.cross(self.d2_vect_as_symbol)
        d1d2_norm = Abs(d1_cross_d2.norm())
        self.perp_vect_unit_as_symbol = d1_cross_d2 / d1d2_norm

        # Solving the linear system
        # self.M = Matrix([self.d1_vect_as_symbol.T, -self.d2_vect_as_symbol.T, self.perp_vect_unit_as_symbol.T]).T
        self.b_vect = self.f2 - self.f1
        # Step 1. Create a list of equations using the symbolic variables
        self.eqn_G2_is_G2 = Eq(self.lambdaG1 * self.d1_vect_as_symbol - self.lambdaG2 * self.d2_vect_as_symbol + self.lambda_perp * self.perp_vect_unit_as_symbol, self.b_vect)

        self.solve_eqn_G2_is_G2(use_solver=True)

        self.G1 = self.f1 + self.lambdaG1 * self.d1_vect_as_symbol
        self.mid_Pw = self.G1 + self.lambda_perp / 2.0 * self.perp_vect_unit_as_symbol
        return self.mid_Pw


    def solve_eqn_G2_is_G2(self, use_solver=True):
        # Step 2. Solve linear system of equations for the common perpendicular between two 3D rays
        if use_solver:
            soln = solve(self.eqn_G2_is_G2, [self.lambdaG1, self.lambdaG2, self.lambda_perp])
            self.lambdaG1 = soln[self.lambdaG1]
            self.lambdaG2 = soln[self.lambdaG2]
            self.lambda_perp = soln[self.lambda_perp]
        else:
            # This would be another way to solve this system
            # Given as M * t = b. Then, t = M^-1 * b
            M1_vect = Matrix([self.d1_vect_as_symbol, -self.d2_vect_as_symbol, self.perp_vect_unit_as_symbol])
            M1_3x3_T = Matrix(3, 3, M1_vect)
            M = M1_3x3_T.T
            lambda_vect = M.inv() * self.b_vect
            self.lambdaG1 = lambda_vect[0]
            self.lambdaG2 = lambda_vect[1]