def _dgm_one(robo, symo, i, j, fast_form=True,
             forced=False, trig_subs=True):
    k = robo.common_root(i, j)
    is_loop = i > robo.NL and j > robo.NL
    chain1 = robo.chain(j, k)
    chain2 = robo.chain(i, k)
    chain2.reverse()
    complete_chain = (chain1 + chain2 + [None])
    T_res = eye(4)
    T = eye(4)
    for indx, x in enumerate(complete_chain[:-1]):
        inverted = indx >= len(chain1)
        T = _transform(robo, x, inverted) * T
        if trig_subs:
            for ang, name in robo.get_angles(x):
                symo.trig_replace(T, ang, name)
        T = T.expand()
        T = T.applyfunc(symo.CS12_simp)
        if is_loop:
            T = T.applyfunc(symo.C2S2_simp)
        x_next = complete_chain[indx + 1]
        if robo.paral(x, x_next):    # false if x_next is None
            continue
        T_res = T * T_res
        T = eye(4)
        if fast_form:
            _dgm_rename(robo, symo, T_res, x, i, j, inverted, forced)
    if not fast_form and forced:
        _dgm_rename(robo, symo, T_res, x, i, j, inverted, forced)
    return T_res

    def compute_vect1_from_pixel(self):
        # Parameters related to Mirror 1:
        self.k1, self.c1 = symbols("k_1, c_1", real=True, positive=True)
        self.u1, self.v1 = symbols("u_1, v_1", real=True, nonnegative=True)
        # TODO: add assumption for k > 2
        # Viewpoint (focus) position vector:
        self.f1x, self.f1y, self.f1z = symbols("x_f_1, y_f_1, z_f_1", real=True)
        # (Coaxial alignment assumption)
        # where
        self.f1x = 0
        self.f1y = 0
        self.f1z = self.c1
        self.f1 = Matrix([self.f1x, self.f1y, self.f1z])
        # Pixel vector
        self.m1h = Matrix([self.u1, self.v1, 1])
        # Point in normalized projection plane
        self.q1 = self.Kc_inv * self.m1h
        # Point in mirror wrt C
        self.t1 = self.c1 / (self.k1 - self.q1.norm() * sqrt(self.k1 * (self.k1 - 2)))
        self.p1 = self.t1 * self.q1
        self.p1h = self.p1.col_join(eye(1))

        # Transform matrix from C to F1 frame
        self.T1_CtoF1 = eye(3).row_join(-self.f1)

        # Direction vector
        self.d1_vect = self.T1_CtoF1 * self.p1h

        return self.d1_vect

def regular_completion(matrix):
    m,n = matrix.shape
    r = matrix.rank()
    
    #~ IPS()
    assert m!=n, "There is no regular completion of a square matrix."

    if m<n:
        assert r==m, "Matrix does not have full row rank."
        A, B, V_pi = reshape_matrix_columns(matrix)
        zeros = sp.zeros(n-m,m)
        ones = sp.eye(n-m)
        S = st.col_stack(zeros,ones)
        completion = S*V_pi.T
        
        regular_matrix = st.row_stack(matrix,completion)
        assert st.rnd_number_rank(regular_matrix)==n, "Regular completion seems to be wrong."

    elif n<m:
        assert r==n, "Matrix does not have full column rank."
        A, B, V_pi = reshape_matrix_columns(matrix.T)
        zeros = sp.zeros(m-n,n)
        ones = sp.eye(m-n)
        S = st.col_stack(zeros,ones)
        completion = V_pi*S.T
        
        regular_matrix = st.col_stack(completion,matrix)
        assert st.rnd_number_rank(regular_matrix)==m, "Regular completion seems to be wrong."

    return completion

def _dgm_right(robo, symo, i, j, trig_subs=True, sep_const=False):
    k = robo.common_root(i, j)
    chain1 = robo.chain(i, k)
    chain2 = robo.chain(j, k)
    chain2.reverse()
    complete_chain = (chain1 + chain2 + [None])
    T_out = {(i, i): eye(4)}
    T_res = eye(4)
    T = eye(4)
    for indx, x in enumerate(complete_chain[:-1]):
        inverted = indx < len(chain1)
        T = T * _transform(robo, x, inverted)
        if trig_subs:
            for ang, name in robo.get_angles(x):
                symo.trig_replace(T, ang, name)
        T = T.expand()
        T = T.applyfunc(symo.CS12_simp)
        x_next = complete_chain[indx + 1]
        if inverted:
            t_name = (i, robo.ant[x])
        else:
            t_name = (i, x)
        T_out[t_name] = T_res * T
        if robo.paral(x, x_next):
            continue
        T_res = T_out[t_name]
        T = eye(4)
    return T_out

def rne_park_backward(rbtdef, geom, fw_results, ifunc=None):
    '''RNE backward pass.'''

    V, dV = fw_results

    if not ifunc:
        ifunc = identity

    # extend Tdh_inv so that Tdh_inv[dof] return identity
    Tdh_inv = geom.Tdh_inv + [eye(4)]

    F = list(range(rbtdef.dof + 1))
    F[rbtdef.dof] = zeros((6, 1))

    tau = zeros((rbtdef.dof, 1))

    fric = frictionforce(rbtdef)
    Idrive = driveinertiaterm(rbtdef)

    # Backward
    for i in range(rbtdef.dof - 1, -1, -1):

        Llm = (rbtdef.L[i].row_join(skew(rbtdef.l[i]))).col_join(
            (-skew(rbtdef.l[i])).row_join(eye(3) * rbtdef.m[i]))

        F[i] = Adjdual(Tdh_inv[i + 1], F[i + 1]) + \
            Llm * dV[i] - adjdual(V[i],  Llm * V[i])
        F[i] = ifunc(F[i])

        tau[i] = ifunc((geom.S[i].transpose() * F[i])[0] + fric[i] + Idrive[i])

    return tau

    def _form_coefficient_matrices(self):
        """Form the coefficient matrices C_0, C_1, and C_2."""

        # Extract dimension variables
        l, m, n, o, s, k = self._dims
        # Build up the coefficient matrices C_0, C_1, and C_2
        # If there are configuration constraints (l > 0), form C_0 as normal.
        # If not, C_0 is I_(nxn). Note that this works even if n=0
        if l > 0:
            f_c_jac_q = self.f_c.jacobian(self.q)
            self._C_0 = (eye(n) - self._Pqd * (f_c_jac_q *
                    self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
        else:
            self._C_0 = eye(n)
        # If there are motion constraints (m > 0), form C_1 and C_2 as normal.
        # If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
        # o = 0.
        if m > 0:
            f_v_jac_u = self.f_v.jacobian(self.u)
            temp = f_v_jac_u * self._Pud
            if n != 0:
                f_v_jac_q = self.f_v.jacobian(self.q)
                self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
            else:
                self._C_1 = 0
            self._C_2 = (eye(o) - self._Pud *
                    temp.LUsolve(f_v_jac_u)) * self._Pui
        else:
            self._C_1 = 0
            self._C_2 = eye(o)

def test_submatrix_assignment():
    m = zeros(4)
    m[2:4, 2:4] = eye(2)
    assert m == Matrix(((0,0,0,0),
                        (0,0,0,0),
                        (0,0,1,0),
                        (0,0,0,1)))
    m[0:2, 0:2] = eye(2)
    assert m == eye(4)
    m[:,0] = Matrix(4,1,(1,2,3,4))
    assert m == Matrix(((1,0,0,0),
                        (2,1,0,0),
                        (3,0,1,0),
                        (4,0,0,1)))
    m[:,:] = zeros(4)
    assert m == zeros(4)
    m[:,:] = ((1,2,3,4),(5,6,7,8),(9, 10, 11, 12),(13,14,15,16))
    assert m == Matrix(((1,2,3,4),
                        (5,6,7,8),
                        (9, 10, 11, 12),
                        (13,14,15,16)))
    m[0:2, 0] = [0,0]
    assert m == Matrix(((0,2,3,4),
                        (0,6,7,8),
                        (9, 10, 11, 12),
                        (13,14,15,16)))

def rotation_from_vectors(v1, v2):
    """
        Find the rotation Matrix R that fullfils:
            R*v2 = v1

        Jur van den Berg,
        Calculate Rotation Matrix to align Vector A to Vector B in 3d?,
        URL (version: 2016-09-01): https://math.stackexchange.com/q/476311
    """
    v1 = sp.Matrix(v1).normalized()
    v2 = sp.Matrix(v2).normalized()

    ax = v1.cross(v2)
    s = ax.norm()
    c = v1.dot(v2)

    if c==1:
        return sp.eye(3)
    if c==-1:
        return -sp.eye(3)


    u1, u2, u3 = ax
    u_ = sp.Matrix(((  0, -u3,  u2), 
                    ( u3,   0, -u1),
                    (-u2,  u1,   0)))

    R = sp.eye(3) - u_ + u_**2 * (1-c)/s**2

    return R

def DHMaker(segments):
  dh_matrices = {}
  inverse_dh_matrices = {}
  required_parameters = []
  for segment in segments:
    if segment.theta is not None:
      theta = segment.theta
    else:
      theta = sp.Symbol(str('theta_' + segment.label))
      required_parameters.append(theta)
    if segment.alpha is not None:
      alpha = segment.alpha
    else:
      alpha = sp.Symbol(str('alpha_' + segment.label))
      required_parameters.append(alpha)
    if segment.r is not None:
      r = segment.r
    else:
      r = sp.Symbol(str('r_' + segment.label))
      required_parameters.append(r)
    if segment.d is not None:
      d = segment.d
    else:
      d = sp.Symbol(str('d_' + segment.label))
      required_parameters.append(d)

    # Components of the dh matrix
    rotation_matrix = sp.Matrix([[sp.cos(theta), -sp.sin(theta)*sp.cos(alpha), sp.sin(theta)*sp.sin(alpha)],
             [sp.sin(theta), sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha)],
             [0, sp.sin(alpha), sp.cos(alpha)]])
    inverse_rotation = rotation_matrix.T

    translation_matrix = sp.Matrix([r*sp.cos(theta),r*sp.sin(theta),d])
    inverse_translation = -inverse_rotation*translation_matrix

    last_row = sp.Matrix([[0, 0, 0, 1]])

    # Compose the forwards and inverse DH Matrices
    dh_matrix = sp.Matrix.vstack(sp.Matrix.hstack(rotation_matrix, translation_matrix), last_row)
    inverse_dh_matrix = sp.Matrix.vstack(sp.Matrix.hstack(inverse_rotation, inverse_translation), last_row)

    inverse_dh_matrices[segment.label] = inverse_dh_matrix
    dh_matrices[segment.label] = dh_matrix

  # Finally, flatten all the matrices into end-to-end transformation matrices
  compound_dh_matrix = sp.eye(4)
  compound_inverse_dh_matrix = sp.eye(4)
  for segment in segments:
    compound_dh_matrix *= dh_matrices[segment.label]
  for segment in reversed(segments):
    compound_inverse_dh_matrix *= inverse_dh_matrices[segment.label]

  # Chop off terms with small coefficients
  compound_dh_matrix = compound_dh_matrix.applyfunc(coeff_chop)
  compound_inverse_dh_matrix = compound_inverse_dh_matrix.applyfunc(coeff_chop)

  actual_reqs = compound_dh_matrix.atoms(sp.Symbol).union(compound_inverse_dh_matrix.atoms(sp.Symbol))
  required_parameters = [str(req) for req in required_parameters if req in actual_reqs]  # This preserves the order in segments
  return compound_dh_matrix, compound_inverse_dh_matrix, required_parameters

 def __init__(self, symo=None, trig_subs=False, simplify=True):
     self.rot = CompTransf(0, 0, 0)
     self.rot_mat = eye(3)
     self.trans = zeros(3, 1)
     self.symo = symo
     self.trig_subs = trig_subs and symo is not None
     self.T_tmp = eye(4)
     self.simplify = simplify

def lm(f_str, sym_str):
	[f, symbols] = pr(f_str, sym_str)
	# # of variables
	D = len(symbols)
	hess = sp.hessian(f, symbols)
	mexpr = sp.Matrix([f])
	grad = mexpr.jacobian(symbols)
	grad = grad.T
	# initial value
	xk = sp.zeros(D, 1)
	fk = 0
	uk = 0.000000001
	epsilon = 0.00001
	for k in range(100):
		Xk_map = {} 
		for i in range(D):
			Xk_map[symbols[i]] = xk[i, 0]
		fk = f.evalf(subs=Xk_map)
		gk = grad.evalf(subs=Xk_map)
		Gk = hess.evalf(subs=Xk_map)
		if gk.norm() < epsilon:
			break
		while  True:
			eigvalue = sp.Matrix.eigenvals(Gk + uk * sp.eye(D))
			if allzero(eigvalue):
				break
			else:
				uk = uk * 4
# 		sk = sp.(A=Gk + uk * sp.ones(D), b=-gk)
		sk = (Gk + uk * sp.eye(D)).LUsolve(-gk)
		Xsk_map = {}
		for i in range(D):
			Xsk_map[symbols[i]] = xk[i, 0] + sk[i, 0]
		fxsk = f.evalf(subs=Xsk_map)
		delta_fk = fk - fxsk
		
		t1 = (gk.T * sk)[0, 0]
		t2 = (0.5 * sk.T * Gk * sk)[0, 0]
		qk = fk + t1 + t2
		delta_qk = fk - qk
		rk = delta_fk / delta_qk
		if rk < 0.25:
			uk = uk * 4
		elif rk > 0.75:
			uk = uk / 2
		else:
			uk = uk
		
		if rk <= 0:
			xk = xk
		else:
			xk = xk + sk
		
	print f, symbols
	for i in range(D):
		print symbols[i], ' = ', xk[i]
	print 'min f =', fk
	return [xk, fk, k]

def test_power():
    A = Matrix([[2,3],[4,5]])
    assert (A**5)[:] == [6140, 8097, 10796, 14237]
    A = Matrix([[2, 1, 3],[4,2, 4], [6,12, 1]])
    assert (A**3)[:] == [290, 262, 251, 448, 440, 368, 702, 954, 433]
    assert A**0 == eye(3)
    assert A**1 == A
    assert (Matrix([[2]]) ** 100)[0,0] == 2**100
    assert eye(2)**10000000 == eye(2)

 def __init__(self, simplify=True):
     self.simplify   = simplify
     self.constants  = []
     self.params     = []
     self.joints_sym = [SymbolicRevoluteJoint(0, 0, 0, 0)]
     self.joints     = [SymbolicRevoluteJoint(0, 0, 0, 0)]
     self.hms_sym    = [sympy.eye(4)]
     self.hms        = [sympy.eye(4)]
     self.functional = False

    def rotation_matrix(self, other):
        """
        Returns the direction cosine matrix(DCM), also known as the
        'rotation matrix' of this coordinate system with respect to
        another system.

        If v_a is a vector defined in system 'A' (in matrix format)
        and v_b is the same vector defined in system 'B', then
        v_a = A.rotation_matrix(B) * v_b.

        A SymPy Matrix is returned.

        Parameters
        ==========

        other : CoordSysCartesian
            The system which the DCM is generated to.

        Examples
        ========

        >>> from sympy.vector import CoordSysCartesian
        >>> from sympy import symbols
        >>> q1 = symbols('q1')
        >>> N = CoordSysCartesian('N')
        >>> A = N.orient_new_axis('A', q1, N.i)
        >>> N.rotation_matrix(A)
        Matrix([
        [1,       0,        0],
        [0, cos(q1), -sin(q1)],
        [0, sin(q1),  cos(q1)]])

        """

        from sympy.vector.functions import _path
        if not isinstance(other, CoordSysCartesian):
            raise TypeError(str(other) +
                            " is not a CoordSysCartesian")
        #Handle special cases
        if other == self:
            return eye(3)
        elif other == self._parent:
            return self._parent_rotation_matrix
        elif other._parent == self:
            return other._parent_rotation_matrix.T
        #Else, use tree to calculate position
        rootindex, path = _path(self, other)
        result = eye(3)
        i = -1
        for i in range(rootindex):
            result *= path[i]._parent_rotation_matrix
        i += 2
        while i < len(path):
            result *= path[i]._parent_rotation_matrix.T
            i += 1
        return result

def test_can_transf_matrix():
    dimsys = DimensionSystem((length, mass, time))

    assert dimsys._can_transf_matrix is None
    assert dimsys.can_transf_matrix == eye(3)
    assert dimsys._can_transf_matrix == eye(3)

    dimsys = DimensionSystem((length, velocity, action))
    assert dimsys.can_transf_matrix == Matrix(((0, 1, 0), (1, 0, 1),
                                               (0, -2, -1)))

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

def test_QR_non_square():
    A = Matrix([[9,0,26],[12,0,-7],[0,4,4],[0,-3,-3]])
    Q, R = A.QRdecomposition()
    assert Q.T * Q == eye(Q.cols)
    assert R.is_upper()
    assert A == Q*R

    A = Matrix([[1,-1,4],[1,4,-2],[1,4,2],[1,-1,0]])
    Q, R = A.QRdecomposition()
    assert Q.T * Q == eye(Q.cols)
    assert R.is_upper()
    assert A == Q*R

 def kPj(self, robo, antPj, antRj, k, chainj):
     T = eye(4)
     for i in chainj:
         if i > k:
             kTj = eye(4)
             kTj[0:3, 0:3] = antRj[i]
             kTj[0:3, 3:] = antPj[i]
             T = kTj * T
     Px = T[0, 3]
     Py = T[1, 3]
     Pz = T[2, 3]
     return Px, Py, Pz

 def _generate_matrix(self):
     '''Generate Matrixs for form homotopy equations'''
     
     self.Lamda_matrix = sym.zeros(self.num_states)
     self.Lamda_matrix[self.num_states/2:, self.num_states/2:] = self.parameters['homotopy_control']*sym.eye(self.num_states/2)
     
     self.Inv_Lamda_matrix = sym.eye(self.num_states)
     self.Inv_Lamda_matrix[self.num_states/2:, self.num_states/2:] = (1-self.parameters['homotopy_control'])*sym.eye(self.num_states/2)
     
     self.k_matrix = self.parameters['tracking_dynamic_control'] * sym.eye(self.num_states)
     
     return self

def test_issue_15601():
    if not np:
        skip("Numpy not installed")

    M = MatrixSymbol("M", 3, 3)
    N = MatrixSymbol("N", 3, 3)
    expr = M*N
    f = lambdify((M, N), expr, "numpy")

    with warns_deprecated_sympy():
        ans = f(eye(3), eye(3))
        assert np.array_equal(ans, np.array([1, 0, 0, 0, 1, 0, 0, 0, 1]))