def _check_orthogonality(equations): """ Helper method for _connect_to_cartesian. It checks if set of transformation equations create orthogonal curvilinear coordinate system Parameters ========== equations : Lambda Lambda of transformation equations """ x1, x2, x3 = symbols("x1, x2, x3", cls=Dummy) equations = equations(x1, x2, x3) v1 = Matrix([diff(equations[0], x1), diff(equations[1], x1), diff(equations[2], x1)]) v2 = Matrix([diff(equations[0], x2), diff(equations[1], x2), diff(equations[2], x2)]) v3 = Matrix([diff(equations[0], x3), diff(equations[1], x3), diff(equations[2], x3)]) if any(simplify(i[0] + i[1] + i[2]) == 0 for i in (v1, v2, v3)): return False else: if simplify(v1.dot(v2)) == 0 and simplify(v2.dot(v3)) == 0 \ and simplify(v3.dot(v1)) == 0: return True else: return False
def test_matrix_to_vector(): m = Matrix([[1], [2], [3]]) assert matrix_to_vector(m, C) == C.i + 2 * C.j + 3 * C.k m = Matrix([[0], [0], [0]]) assert matrix_to_vector(m, N) == matrix_to_vector(m, C) == \ Vector.zero m = Matrix([[q1], [q2], [q3]]) assert matrix_to_vector(m, N) == q1 * N.i + q2 * N.j + q3 * N.k
def _rot(axis, angle): """DCM for simple axis 1, 2 or 3 rotations. """ if axis == 1: return Matrix(rot_axis1(angle).T) elif axis == 2: return Matrix(rot_axis2(angle).T) elif axis == 3: return Matrix(rot_axis3(angle).T)
def test_dyadic(): d1 = A.x | A.x d2 = A.y | A.y d3 = A.x | A.y assert d1 * 0 == 0 assert d1 != 0 assert d1 * 2 == 2 * A.x | A.x assert d1 / 2. == 0.5 * d1 assert d1 & (0 * d1) == 0 assert d1 & d2 == 0 assert d1 & A.x == A.x assert d1 ^ A.x == 0 assert d1 ^ A.y == A.x | A.z assert d1 ^ A.z == - A.x | A.y assert d2 ^ A.x == - A.y | A.z assert A.x ^ d1 == 0 assert A.y ^ d1 == - A.z | A.x assert A.z ^ d1 == A.y | A.x assert A.x & d1 == A.x assert A.y & d1 == 0 assert A.y & d2 == A.y assert d1 & d3 == A.x | A.y assert d3 & d1 == 0 assert d1.dt(A) == 0 q = dynamicsymbols('q') qd = dynamicsymbols('q', 1) B = A.orientnew('B', 'Axis', [q, A.z]) assert d1.express(B) == d1.express(B, B) assert d1.express(B) == ((cos(q)**2) * (B.x | B.x) + (-sin(q) * cos(q)) * (B.x | B.y) + (-sin(q) * cos(q)) * (B.y | B.x) + (sin(q)**2) * (B.y | B.y)) assert d1.express(B, A) == (cos(q)) * (B.x | A.x) + (-sin(q)) * (B.y | A.x) assert d1.express(A, B) == (cos(q)) * (A.x | B.x) + (-sin(q)) * (A.x | B.y) assert d1.dt(B) == (-qd) * (A.y | A.x) + (-qd) * (A.x | A.y) assert d1.to_matrix(A) == Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]]) assert d1.to_matrix(A, B) == Matrix([[cos(q), -sin(q), 0], [0, 0, 0], [0, 0, 0]]) assert d3.to_matrix(A) == Matrix([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) a, b, c, d, e, f = symbols('a, b, c, d, e, f') v1 = a * A.x + b * A.y + c * A.z v2 = d * A.x + e * A.y + f * A.z d4 = v1.outer(v2) assert d4.to_matrix(A) == Matrix([[a * d, a * e, a * f], [b * d, b * e, b * f], [c * d, c * e, c * f]]) d5 = v1.outer(v1) C = A.orientnew('C', 'Axis', [q, A.x]) for expected, actual in zip(C.dcm(A) * d5.to_matrix(A) * C.dcm(A).T, d5.to_matrix(C)): assert (expected - actual).simplify() == 0 raises(TypeError, lambda: d1.applyfunc(0))
def test_dcm(): q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4') N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = A.orientnew('B', 'Axis', [q2, A.x]) C = B.orientnew('C', 'Axis', [q3, B.y]) D = N.orientnew('D', 'Axis', [q4, N.y]) E = N.orientnew('E', 'Space', [q1, q2, q3], '123') assert N.dcm(C) == Matrix( [[ -sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), -sin(q1) * cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1) ], [ sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3) ], [-sin(q3) * cos(q2), sin(q2), cos(q2) * cos(q3)]]) # This is a little touchy. Is it ok to use simplify in assert? test_mat = D.dcm(C) - Matrix( [[ cos(q1) * cos(q3) * cos(q4) - sin(q3) * (-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)), -sin(q2) * sin(q4) - sin(q1) * cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * (-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)) ], [ sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3) ], [ sin(q4) * cos(q1) * cos(q3) - sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)), sin(q2) * cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * sin(q4) * cos(q1) + cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)) ]]) assert test_mat.expand() == zeros(3, 3) assert E.dcm(N) == Matrix( [[cos(q2) * cos(q3), sin(q3) * cos(q2), -sin(q2)], [ sin(q1) * sin(q2) * cos(q3) - sin(q3) * cos(q1), sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), sin(q1) * cos(q2) ], [ sin(q1) * sin(q3) + sin(q2) * cos(q1) * cos(q3), -sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2) ]])
def test_Vector(): assert A.x != A.y assert A.y != A.z assert A.z != A.x assert A.x + 0 == A.x v1 = x*A.x + y*A.y + z*A.z v2 = x**2*A.x + y**2*A.y + z**2*A.z v3 = v1 + v2 v4 = v1 - v2 assert isinstance(v1, Vector) assert dot(v1, A.x) == x assert dot(v1, A.y) == y assert dot(v1, A.z) == z assert isinstance(v2, Vector) assert dot(v2, A.x) == x**2 assert dot(v2, A.y) == y**2 assert dot(v2, A.z) == z**2 assert isinstance(v3, Vector) # We probably shouldn't be using simplify in dot... assert dot(v3, A.x) == x**2 + x assert dot(v3, A.y) == y**2 + y assert dot(v3, A.z) == z**2 + z assert isinstance(v4, Vector) # We probably shouldn't be using simplify in dot... assert dot(v4, A.x) == x - x**2 assert dot(v4, A.y) == y - y**2 assert dot(v4, A.z) == z - z**2 assert v1.to_matrix(A) == Matrix([[x], [y], [z]]) q = symbols('q') B = A.orientnew('B', 'Axis', (q, A.x)) assert v1.to_matrix(B) == Matrix([[x], [ y * cos(q) + z * sin(q)], [-y * sin(q) + z * cos(q)]]) #Test the separate method B = ReferenceFrame('B') v5 = x*A.x + y*A.y + z*B.z assert Vector(0).separate() == {} assert v1.separate() == {A: v1} assert v5.separate() == {A: x*A.x + y*A.y, B: z*B.z} #Test the free_symbols property v6 = x*A.x + y*A.y + z*A.z assert v6.free_symbols(A) == {x,y,z} raises(TypeError, lambda: v3.applyfunc(v1))
def test_w_diff_dcm1(): # Ref: # Dynamics Theory and Applications, Kane 1985 # Sec. 2.1 ANGULAR VELOCITY A = ReferenceFrame('A') B = ReferenceFrame('B') c11, c12, c13 = dynamicsymbols('C11 C12 C13') c21, c22, c23 = dynamicsymbols('C21 C22 C23') c31, c32, c33 = dynamicsymbols('C31 C32 C33') c11d, c12d, c13d = dynamicsymbols('C11 C12 C13', level=1) c21d, c22d, c23d = dynamicsymbols('C21 C22 C23', level=1) c31d, c32d, c33d = dynamicsymbols('C31 C32 C33', level=1) DCM = Matrix([[c11, c12, c13], [c21, c22, c23], [c31, c32, c33]]) B.orient(A, 'DCM', DCM) b1a = (B.x).express(A) b2a = (B.y).express(A) b3a = (B.z).express(A) # Equation (2.1.1) B.set_ang_vel( A, B.x * (dot((b3a).dt(A), B.y)) + B.y * (dot( (b1a).dt(A), B.z)) + B.z * (dot((b2a).dt(A), B.x))) # Equation (2.1.21) expr = ((c12 * c13d + c22 * c23d + c32 * c33d) * B.x + (c13 * c11d + c23 * c21d + c33 * c31d) * B.y + (c11 * c12d + c21 * c22d + c31 * c32d) * B.z) assert B.ang_vel_in(A) - expr == 0
def test_w_diff_dcm2(): q1, q2, q3 = dynamicsymbols('q1:4') N = ReferenceFrame('N') A = N.orientnew('A', 'axis', [q1, N.x]) B = A.orientnew('B', 'axis', [q2, A.y]) C = B.orientnew('C', 'axis', [q3, B.z]) DCM = C.dcm(N).T D = N.orientnew('D', 'DCM', DCM) # Frames D and C are the same ReferenceFrame, # since they have equal DCM respect to frame N. # Therefore, D and C should have same angle velocity in N. assert D.dcm(N) == C.dcm(N) == Matrix( [[ cos(q2) * cos(q3), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3) ], [ -sin(q3) * cos(q2), -sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1) ], [sin(q2), -sin(q1) * cos(q2), cos(q1) * cos(q2)]]) assert (D.ang_vel_in(N) - C.ang_vel_in(N)).express(N).simplify() == 0
def test_issue_11498(): A = ReferenceFrame('A') B = ReferenceFrame('B') # Identity transformation A.orient(B, 'DCM', eye(3)) assert A.dcm(B) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) assert B.dcm(A) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # x -> y # y -> -z # z -> -x A.orient(B, 'DCM', Matrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])) assert B.dcm(A) == Matrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]) assert A.dcm(B) == Matrix([[0, 0, -1], [1, 0, 0], [0, -1, 0]]) assert B.dcm(A).T == A.dcm(B)
def test_dcm_diff_16824(): # NOTE : This is a regression test for the bug introduced in PR 14758, # identified in 16824, and solved by PR 16828. # This is the solution to Problem 2.2 on page 264 in Kane & Lenvinson's # 1985 book. q1, q2, q3 = dynamicsymbols('q1:4') s1 = sin(q1) c1 = cos(q1) s2 = sin(q2) c2 = cos(q2) s3 = sin(q3) c3 = cos(q3) dcm = Matrix([[c2 * c3, s1 * s2 * c3 - s3 * c1, c1 * s2 * c3 + s3 * s1], [c2 * s3, s1 * s2 * s3 + c3 * c1, c1 * s2 * s3 - c3 * s1], [-s2, s1 * c2, c1 * c2]]) A = ReferenceFrame('A') B = ReferenceFrame('B') B.orient(A, 'DCM', dcm) AwB = B.ang_vel_in(A) alpha2 = s3 * c2 * q1.diff() + c3 * q2.diff() beta2 = s1 * c2 * q3.diff() + c1 * q2.diff() assert simplify(AwB.dot(A.y) - alpha2) == 0 assert simplify(AwB.dot(B.y) - beta2) == 0
def __new__(cls, q0, q1, q2, q3): q0 = sympify(q0) q1 = sympify(q1) q2 = sympify(q2) q3 = sympify(q3) parent_orient = (Matrix([[ q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 - q0 * q3), 2 * (q0 * q2 + q1 * q3) ], [ 2 * (q1 * q2 + q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 - q0 * q1) ], [ 2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3), q0**2 - q1**2 - q2**2 + q3**2 ]])) parent_orient = parent_orient.T obj = super().__new__(cls, q0, q1, q2, q3) obj._q0 = q0 obj._q1 = q1 obj._q2 = q2 obj._q3 = q3 obj._parent_orient = parent_orient return obj
def to_matrix(self, system): """ Returns the matrix form of this vector with respect to the specified coordinate system. Parameters ========== system : CoordSys3D The system wrt which the matrix form is to be computed Examples ======== >>> from sympy.vector import CoordSys3D >>> C = CoordSys3D('C') >>> from sympy.abc import a, b, c >>> v = a*C.i + b*C.j + c*C.k >>> v.to_matrix(C) Matrix([ [a], [b], [c]]) """ return Matrix([self.dot(unit_vec) for unit_vec in system.base_vectors()])
def test_orienters(): A = CoordSys3D('A') axis_orienter = AxisOrienter(a, A.k) body_orienter = BodyOrienter(a, b, c, '123') space_orienter = SpaceOrienter(a, b, c, '123') q_orienter = QuaternionOrienter(q1, q2, q3, q4) assert axis_orienter.rotation_matrix(A) == Matrix([[cos(a), sin(a), 0], [-sin(a), cos(a), 0], [0, 0, 1]]) assert body_orienter.rotation_matrix() == Matrix( [[ cos(b) * cos(c), sin(a) * sin(b) * cos(c) + sin(c) * cos(a), sin(a) * sin(c) - sin(b) * cos(a) * cos(c) ], [ -sin(c) * cos(b), -sin(a) * sin(b) * sin(c) + cos(a) * cos(c), sin(a) * cos(c) + sin(b) * sin(c) * cos(a) ], [sin(b), -sin(a) * cos(b), cos(a) * cos(b)]]) assert space_orienter.rotation_matrix() == Matrix( [[cos(b) * cos(c), sin(c) * cos(b), -sin(b)], [ sin(a) * sin(b) * cos(c) - sin(c) * cos(a), sin(a) * sin(b) * sin(c) + cos(a) * cos(c), sin(a) * cos(b) ], [ sin(a) * sin(c) + sin(b) * cos(a) * cos(c), -sin(a) * cos(c) + sin(b) * sin(c) * cos(a), cos(a) * cos(b) ]]) assert q_orienter.rotation_matrix() == Matrix( [[ q1**2 + q2**2 - q3**2 - q4**2, 2 * q1 * q4 + 2 * q2 * q3, -2 * q1 * q3 + 2 * q2 * q4 ], [ -2 * q1 * q4 + 2 * q2 * q3, q1**2 - q2**2 + q3**2 - q4**2, 2 * q1 * q2 + 2 * q3 * q4 ], [ 2 * q1 * q3 + 2 * q2 * q4, -2 * q1 * q2 + 2 * q3 * q4, q1**2 - q2**2 - q3**2 + q4**2 ]])
def test_orient_body(): A = ReferenceFrame('A') B = ReferenceFrame('B') B.orient_body_fixed(A, (1, 1, 0), 'XYX') assert B.dcm(A) == Matrix([[cos(1), sin(1)**2, -sin(1) * cos(1)], [0, cos(1), sin(1)], [sin(1), -sin(1) * cos(1), cos(1)**2]])
def test_vector(): assert isinstance(i, BaseVector) assert i != j assert j != k assert k != i assert i - i == Vector.zero assert i + Vector.zero == i assert i - Vector.zero == i assert Vector.zero != 0 assert -Vector.zero == Vector.zero v1 = a * i + b * j + c * k v2 = a**2 * i + b**2 * j + c**2 * k v3 = v1 + v2 v4 = 2 * v1 v5 = a * i assert isinstance(v1, VectorAdd) assert v1 - v1 == Vector.zero assert v1 + Vector.zero == v1 assert v1.dot(i) == a assert v1.dot(j) == b assert v1.dot(k) == c assert i.dot(v2) == a**2 assert j.dot(v2) == b**2 assert k.dot(v2) == c**2 assert v3.dot(i) == a**2 + a assert v3.dot(j) == b**2 + b assert v3.dot(k) == c**2 + c assert v1 + v2 == v2 + v1 assert v1 - v2 == -1 * (v2 - v1) assert a * v1 == v1 * a assert isinstance(v5, VectorMul) assert v5.base_vector == i assert v5.measure_number == a assert isinstance(v4, Vector) assert isinstance(v4, VectorAdd) assert isinstance(v4, Vector) assert isinstance(Vector.zero, VectorZero) assert isinstance(Vector.zero, Vector) assert isinstance(v1 * 0, VectorZero) assert v1.to_matrix(C) == Matrix([[a], [b], [c]]) assert i.components == {i: 1} assert v5.components == {i: a} assert v1.components == {i: a, j: b, k: c} assert VectorAdd(v1, Vector.zero) == v1 assert VectorMul(a, v1) == v1 * a assert VectorMul(1, i) == i assert VectorAdd(v1, Vector.zero) == v1 assert VectorMul(0, Vector.zero) == Vector.zero raises(TypeError, lambda: v1.outer(1)) raises(TypeError, lambda: v1.dot(1))
def test_frame_dict(): A = ReferenceFrame('A') B = ReferenceFrame('B') C = ReferenceFrame('C') a, b, c = symbols('a b c') B.orient_axis(A, A.x, a) assert A._dcm_dict == { B: Matrix([[1, 0, 0], [0, cos(a), -sin(a)], [0, sin(a), cos(a)]]) } assert B._dcm_dict == { A: Matrix([[1, 0, 0], [0, cos(a), sin(a)], [0, -sin(a), cos(a)]]) } assert C._dcm_dict == {} B.orient_axis(C, C.x, b) # Previous relation is not wiped assert A._dcm_dict == { B: Matrix([[1, 0, 0], [0, cos(a), -sin(a)], [0, sin(a), cos(a)]]) } assert B._dcm_dict == {A: Matrix([[1, 0, 0],[0, cos(a), sin(a)],[0, -sin(a), cos(a)]]), \ C: Matrix([[1, 0, 0],[0, cos(b), sin(b)],[0, -sin(b), cos(b)]])} assert C._dcm_dict == { B: Matrix([[1, 0, 0], [0, cos(b), -sin(b)], [0, sin(b), cos(b)]]) } A.orient_axis(B, B.x, c) # Previous relation is updated assert B._dcm_dict == {C: Matrix([[1, 0, 0],[0, cos(b), sin(b)],[0, -sin(b), cos(b)]]),\ A: Matrix([[1, 0, 0],[0, cos(c), -sin(c)],[0, sin(c), cos(c)]])} assert A._dcm_dict == { B: Matrix([[1, 0, 0], [0, cos(c), sin(c)], [0, -sin(c), cos(c)]]) } assert C._dcm_dict == { B: Matrix([[1, 0, 0], [0, cos(b), -sin(b)], [0, sin(b), cos(b)]]) }
def test_dcm_cache_dict(): A = ReferenceFrame('A') B = ReferenceFrame('B') C = ReferenceFrame('C') D = ReferenceFrame('D') a, b, c = symbols('a b c') B.orient_axis(A, A.x, a) C.orient_axis(B, B.x, b) D.orient_axis(C, C.x, c) assert D._dcm_dict == { C: Matrix([[1, 0, 0], [0, cos(c), sin(c)], [0, -sin(c), cos(c)]]) } assert C._dcm_dict == {B: Matrix([[1, 0, 0],[0, cos(b), sin(b)],[0, -sin(b), cos(b)]]), \ D: Matrix([[1, 0, 0],[0, cos(c), -sin(c)],[0, sin(c), cos(c)]])} assert B._dcm_dict == {A: Matrix([[1, 0, 0],[0, cos(a), sin(a)],[0, -sin(a), cos(a)]]), \ C: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(b)]])} assert A._dcm_dict == { B: Matrix([[1, 0, 0], [0, cos(a), -sin(a)], [0, sin(a), cos(a)]]) } assert D._dcm_dict == D._dcm_cache D.dcm( A ) # Check calculated dcm relation is stored in _dcm_cache and not in _dcm_dict assert list(A._dcm_cache.keys()) == [A, B, D] assert list(D._dcm_cache.keys()) == [C, A] assert list(A._dcm_dict.keys()) == [B] assert list(D._dcm_dict.keys()) == [C] assert A._dcm_dict != A._dcm_cache A.orient_axis( B, B.x, b) # _dcm_cache of A is wiped out and new relation is stored. assert A._dcm_dict == { B: Matrix([[1, 0, 0], [0, cos(b), sin(b)], [0, -sin(b), cos(b)]]) } assert A._dcm_dict == A._dcm_cache assert B._dcm_dict == {C: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(b)]]), \ A: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(b)]])}
def _rotation_trans_equations(cls, matrix, equations): """ Returns the transformation equations obtained from rotation matrix. Parameters ========== matrix : Matrix Rotation matrix equations : tuple Transformation equations """ return tuple(matrix * Matrix(equations))
def to_matrix(self, system, second_system=None): """ Returns the matrix form of the dyadic with respect to one or two coordinate systems. Parameters ========== system : CoordSys3D The coordinate system that the rows and columns of the matrix correspond to. If a second system is provided, this only corresponds to the rows of the matrix. second_system : CoordSys3D, optional, default=None The coordinate system that the columns of the matrix correspond to. Examples ======== >>> from sympy.vector import CoordSys3D >>> N = CoordSys3D('N') >>> v = N.i + 2*N.j >>> d = v.outer(N.i) >>> d.to_matrix(N) Matrix([ [1, 0, 0], [2, 0, 0], [0, 0, 0]]) >>> from sympy import Symbol >>> q = Symbol('q') >>> P = N.orient_new_axis('P', q, N.k) >>> d.to_matrix(N, P) Matrix([ [ cos(q), -sin(q), 0], [2*cos(q), -2*sin(q), 0], [ 0, 0, 0]]) """ if second_system is None: second_system = system return Matrix([ i.dot(self).dot(j) for i in system for j in second_system ]).reshape(3, 3)
def rotation_matrix(self, system): """ The rotation matrix corresponding to this orienter instance. Parameters ========== system : CoordSys3D The coordinate system wrt which the rotation matrix is to be computed """ axis = sympy.vector.express(self.axis, system).normalize() axis = axis.to_matrix(system) theta = self.angle parent_orient = ( (eye(3) - axis * axis.T) * cos(theta) + Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T) parent_orient = parent_orient.T return parent_orient
def scalar_map(self, other): """ Returns a dictionary which expresses the coordinate variables (base scalars) of this frame in terms of the variables of otherframe. Parameters ========== otherframe : CoordSys3D The other system to map the variables to. Examples ======== >>> from sympy.vector import CoordSys3D >>> from sympy import Symbol >>> A = CoordSys3D('A') >>> q = Symbol('q') >>> B = A.orient_new_axis('B', q, A.k) >>> A.scalar_map(B) {A.x: B.x*cos(q) - B.y*sin(q), A.y: B.x*sin(q) + B.y*cos(q), A.z: B.z} """ relocated_scalars = [] origin_coords = tuple(self.position_wrt(other).to_matrix(other)) for i, x in enumerate(other.base_scalars()): relocated_scalars.append(x - origin_coords[i]) vars_matrix = (self.rotation_matrix(other) * Matrix(relocated_scalars)) mapping = {} for i, x in enumerate(self.base_scalars()): mapping[x] = trigsimp(vars_matrix[i]) return mapping
def __new__(cls, name, transformation=None, parent=None, location=None, rotation_matrix=None, vector_names=None, variable_names=None): """ The orientation/location parameters are necessary if this system is being defined at a certain orientation or location wrt another. Parameters ========== name : str The name of the new CoordSys3D instance. transformation : Lambda, Tuple, str Transformation defined by transformation equations or chosen from predefined ones. location : Vector The position vector of the new system's origin wrt the parent instance. rotation_matrix : SymPy ImmutableMatrix The rotation matrix of the new coordinate system with respect to the parent. In other words, the output of new_system.rotation_matrix(parent). parent : CoordSys3D The coordinate system wrt which the orientation/location (or both) is being defined. vector_names, variable_names : iterable(optional) Iterables of 3 strings each, with custom names for base vectors and base scalars of the new system respectively. Used for simple str printing. """ name = str(name) Vector = sympy.vector.Vector Point = sympy.vector.Point if not isinstance(name, str): raise TypeError("name should be a string") if transformation is not None: if (location is not None) or (rotation_matrix is not None): raise ValueError("specify either `transformation` or " "`location`/`rotation_matrix`") if isinstance(transformation, (Tuple, tuple, list)): if isinstance(transformation[0], MatrixBase): rotation_matrix = transformation[0] location = transformation[1] else: transformation = Lambda(transformation[0], transformation[1]) elif isinstance(transformation, Callable): x1, x2, x3 = symbols('x1 x2 x3', cls=Dummy) transformation = Lambda((x1, x2, x3), transformation(x1, x2, x3)) elif isinstance(transformation, str): transformation = Str(transformation) elif isinstance(transformation, (Str, Lambda)): pass else: raise TypeError("transformation: " "wrong type {}".format(type(transformation))) # If orientation information has been provided, store # the rotation matrix accordingly if rotation_matrix is None: rotation_matrix = ImmutableDenseMatrix(eye(3)) else: if not isinstance(rotation_matrix, MatrixBase): raise TypeError("rotation_matrix should be an Immutable" + "Matrix instance") rotation_matrix = rotation_matrix.as_immutable() # If location information is not given, adjust the default # location as Vector.zero if parent is not None: if not isinstance(parent, CoordSys3D): raise TypeError("parent should be a " + "CoordSys3D/None") if location is None: location = Vector.zero else: if not isinstance(location, Vector): raise TypeError("location should be a Vector") # Check that location does not contain base # scalars for x in location.free_symbols: if isinstance(x, BaseScalar): raise ValueError("location should not contain" + " BaseScalars") origin = parent.origin.locate_new(name + '.origin', location) else: location = Vector.zero origin = Point(name + '.origin') if transformation is None: transformation = Tuple(rotation_matrix, location) if isinstance(transformation, Tuple): lambda_transformation = CoordSys3D._compose_rotation_and_translation( transformation[0], transformation[1], parent ) r, l = transformation l = l._projections lambda_lame = CoordSys3D._get_lame_coeff('cartesian') lambda_inverse = lambda x, y, z: r.inv()*Matrix( [x-l[0], y-l[1], z-l[2]]) elif isinstance(transformation, Str): trname = transformation.name lambda_transformation = CoordSys3D._get_transformation_lambdas(trname) if parent is not None: if parent.lame_coefficients() != (S.One, S.One, S.One): raise ValueError('Parent for pre-defined coordinate ' 'system should be Cartesian.') lambda_lame = CoordSys3D._get_lame_coeff(trname) lambda_inverse = CoordSys3D._set_inv_trans_equations(trname) elif isinstance(transformation, Lambda): if not CoordSys3D._check_orthogonality(transformation): raise ValueError("The transformation equation does not " "create orthogonal coordinate system") lambda_transformation = transformation lambda_lame = CoordSys3D._calculate_lame_coeff(lambda_transformation) lambda_inverse = None else: lambda_transformation = lambda x, y, z: transformation(x, y, z) lambda_lame = CoordSys3D._get_lame_coeff(transformation) lambda_inverse = None if variable_names is None: if isinstance(transformation, Lambda): variable_names = ["x1", "x2", "x3"] elif isinstance(transformation, Str): if transformation.name == 'spherical': variable_names = ["r", "theta", "phi"] elif transformation.name == 'cylindrical': variable_names = ["r", "theta", "z"] else: variable_names = ["x", "y", "z"] else: variable_names = ["x", "y", "z"] if vector_names is None: vector_names = ["i", "j", "k"] # All systems that are defined as 'roots' are unequal, unless # they have the same name. # Systems defined at same orientation/position wrt the same # 'parent' are equal, irrespective of the name. # This is true even if the same orientation is provided via # different methods like Axis/Body/Space/Quaternion. # However, coincident systems may be seen as unequal if # positioned/oriented wrt different parents, even though # they may actually be 'coincident' wrt the root system. if parent is not None: obj = super().__new__( cls, Str(name), transformation, parent) else: obj = super().__new__( cls, Str(name), transformation) obj._name = name # Initialize the base vectors _check_strings('vector_names', vector_names) vector_names = list(vector_names) latex_vects = [(r'\mathbf{\hat{%s}_{%s}}' % (x, name)) for x in vector_names] pretty_vects = ['%s_%s' % (x, name) for x in vector_names] obj._vector_names = vector_names v1 = BaseVector(0, obj, pretty_vects[0], latex_vects[0]) v2 = BaseVector(1, obj, pretty_vects[1], latex_vects[1]) v3 = BaseVector(2, obj, pretty_vects[2], latex_vects[2]) obj._base_vectors = (v1, v2, v3) # Initialize the base scalars _check_strings('variable_names', vector_names) variable_names = list(variable_names) latex_scalars = [(r"\mathbf{{%s}_{%s}}" % (x, name)) for x in variable_names] pretty_scalars = ['%s_%s' % (x, name) for x in variable_names] obj._variable_names = variable_names obj._vector_names = vector_names x1 = BaseScalar(0, obj, pretty_scalars[0], latex_scalars[0]) x2 = BaseScalar(1, obj, pretty_scalars[1], latex_scalars[1]) x3 = BaseScalar(2, obj, pretty_scalars[2], latex_scalars[2]) obj._base_scalars = (x1, x2, x3) obj._transformation = transformation obj._transformation_lambda = lambda_transformation obj._lame_coefficients = lambda_lame(x1, x2, x3) obj._transformation_from_parent_lambda = lambda_inverse setattr(obj, variable_names[0], x1) setattr(obj, variable_names[1], x2) setattr(obj, variable_names[2], x3) setattr(obj, vector_names[0], v1) setattr(obj, vector_names[1], v2) setattr(obj, vector_names[2], v3) # Assign params obj._parent = parent if obj._parent is not None: obj._root = obj._parent._root else: obj._root = obj obj._parent_rotation_matrix = rotation_matrix obj._origin = origin # Return the instance return obj
def test_orient_quaternion(): A = ReferenceFrame('A') B = ReferenceFrame('B') B.orient_quaternion(A, (0, 0, 0, 0)) assert B.dcm(A) == Matrix([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
def test_orient_space(): A = ReferenceFrame('A') B = ReferenceFrame('B') B.orient_space_fixed(A, (0, 0, 0), '123') assert B.dcm(A) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
def orient_new(self, name, orienters, location=None, vector_names=None, variable_names=None): """ Creates a new CoordSys3D oriented in the user-specified way with respect to this system. Please refer to the documentation of the orienter classes for more information about the orientation procedure. Parameters ========== name : str The name of the new CoordSys3D instance. orienters : iterable/Orienter An Orienter or an iterable of Orienters for orienting the new coordinate system. If an Orienter is provided, it is applied to get the new system. If an iterable is provided, the orienters will be applied in the order in which they appear in the iterable. location : Vector(optional) The location of the new coordinate system's origin wrt this system's origin. If not specified, the origins are taken to be coincident. vector_names, variable_names : iterable(optional) Iterables of 3 strings each, with custom names for base vectors and base scalars of the new system respectively. Used for simple str printing. Examples ======== >>> from sympy.vector import CoordSys3D >>> from sympy import symbols >>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3') >>> N = CoordSys3D('N') Using an AxisOrienter >>> from sympy.vector import AxisOrienter >>> axis_orienter = AxisOrienter(q1, N.i + 2 * N.j) >>> A = N.orient_new('A', (axis_orienter, )) Using a BodyOrienter >>> from sympy.vector import BodyOrienter >>> body_orienter = BodyOrienter(q1, q2, q3, '123') >>> B = N.orient_new('B', (body_orienter, )) Using a SpaceOrienter >>> from sympy.vector import SpaceOrienter >>> space_orienter = SpaceOrienter(q1, q2, q3, '312') >>> C = N.orient_new('C', (space_orienter, )) Using a QuaternionOrienter >>> from sympy.vector import QuaternionOrienter >>> q_orienter = QuaternionOrienter(q0, q1, q2, q3) >>> D = N.orient_new('D', (q_orienter, )) """ if variable_names is None: variable_names = self._variable_names if vector_names is None: vector_names = self._vector_names if isinstance(orienters, Orienter): if isinstance(orienters, AxisOrienter): final_matrix = orienters.rotation_matrix(self) else: final_matrix = orienters.rotation_matrix() # TODO: trigsimp is needed here so that the matrix becomes # canonical (scalar_map also calls trigsimp; without this, you can # end up with the same CoordinateSystem that compares differently # due to a differently formatted matrix). However, this is # probably not so good for performance. final_matrix = trigsimp(final_matrix) else: final_matrix = Matrix(eye(3)) for orienter in orienters: if isinstance(orienter, AxisOrienter): final_matrix *= orienter.rotation_matrix(self) else: final_matrix *= orienter.rotation_matrix() return CoordSys3D(name, rotation_matrix=final_matrix, vector_names=vector_names, variable_names=variable_names, location=location, parent=self)
def test_orient_explicit(): A = ReferenceFrame('A') B = ReferenceFrame('B') A.orient_explicit(B, eye(3)) assert A.dcm(B) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
def test_rotation_matrix(): N = CoordSys3D('N') A = N.orient_new_axis('A', q1, N.k) B = A.orient_new_axis('B', q2, A.i) C = B.orient_new_axis('C', q3, B.j) D = N.orient_new_axis('D', q4, N.j) E = N.orient_new_space('E', q1, q2, q3, '123') F = N.orient_new_quaternion('F', q1, q2, q3, q4) G = N.orient_new_body('G', q1, q2, q3, '123') assert N.rotation_matrix(C) == Matrix([ [- sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), - sin(q1) * cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1)], \ [sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), \ cos(q1) * cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * \ cos(q3)], [- sin(q3) * cos(q2), sin(q2), cos(q2) * cos(q3)]]) test_mat = D.rotation_matrix(C) - Matrix( [[cos(q1) * cos(q3) * cos(q4) - sin(q3) * (- sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)), - sin(q2) * sin(q4) - sin(q1) * cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * \ (- sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4))], \ [sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * \ cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)], \ [sin(q4) * cos(q1) * cos(q3) - sin(q3) * (cos(q2) * cos(q4) + \ sin(q1) * sin(q2) * \ sin(q4)), sin(q2) * cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * \ sin(q4) * cos(q1) + cos(q3) * (cos(q2) * cos(q4) + \ sin(q1) * sin(q2) * sin(q4))]]) assert test_mat.expand() == zeros(3, 3) assert E.rotation_matrix(N) == Matrix( [[cos(q2)*cos(q3), sin(q3)*cos(q2), -sin(q2)], [sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), \ sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2)], \ [sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), - \ sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2)]]) assert F.rotation_matrix(N) == Matrix([[ q1**2 + q2**2 - q3**2 - q4**2, 2 * q1 * q4 + 2 * q2 * q3, -2 * q1 * q3 + 2 * q2 * q4 ], [ -2 * q1 * q4 + 2 * q2 * q3, q1**2 - q2**2 + q3**2 - q4**2, 2 * q1 * q2 + 2 * q3 * q4 ], [ 2 * q1 * q3 + 2 * q2 * q4, -2 * q1 * q2 + 2 * q3 * q4, q1**2 - q2**2 - q3**2 + q4**2 ]]) assert G.rotation_matrix(N) == Matrix( [[ cos(q2) * cos(q3), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3) ], [ -sin(q3) * cos(q2), -sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1) ], [sin(q2), -sin(q1) * cos(q2), cos(q1) * cos(q2)]])
def test_vector_with_orientation(): """ Tests the effects of orientation of coordinate systems on basic vector operations. """ N = CoordSys3D('N') A = N.orient_new_axis('A', q1, N.k) B = A.orient_new_axis('B', q2, A.i) C = B.orient_new_axis('C', q3, B.j) # Test to_matrix v1 = a * N.i + b * N.j + c * N.k assert v1.to_matrix(A) == Matrix([[a * cos(q1) + b * sin(q1)], [-a * sin(q1) + b * cos(q1)], [c]]) # Test dot assert N.i.dot(A.i) == cos(q1) assert N.i.dot(A.j) == -sin(q1) assert N.i.dot(A.k) == 0 assert N.j.dot(A.i) == sin(q1) assert N.j.dot(A.j) == cos(q1) assert N.j.dot(A.k) == 0 assert N.k.dot(A.i) == 0 assert N.k.dot(A.j) == 0 assert N.k.dot(A.k) == 1 assert N.i.dot(A.i + A.j) == -sin(q1) + cos(q1) == \ (A.i + A.j).dot(N.i) assert A.i.dot(C.i) == cos(q3) assert A.i.dot(C.j) == 0 assert A.i.dot(C.k) == sin(q3) assert A.j.dot(C.i) == sin(q2) * sin(q3) assert A.j.dot(C.j) == cos(q2) assert A.j.dot(C.k) == -sin(q2) * cos(q3) assert A.k.dot(C.i) == -cos(q2) * sin(q3) assert A.k.dot(C.j) == sin(q2) assert A.k.dot(C.k) == cos(q2) * cos(q3) # Test cross assert N.i.cross(A.i) == sin(q1) * A.k assert N.i.cross(A.j) == cos(q1) * A.k assert N.i.cross(A.k) == -sin(q1) * A.i - cos(q1) * A.j assert N.j.cross(A.i) == -cos(q1) * A.k assert N.j.cross(A.j) == sin(q1) * A.k assert N.j.cross(A.k) == cos(q1) * A.i - sin(q1) * A.j assert N.k.cross(A.i) == A.j assert N.k.cross(A.j) == -A.i assert N.k.cross(A.k) == Vector.zero assert N.i.cross(A.i) == sin(q1) * A.k assert N.i.cross(A.j) == cos(q1) * A.k assert N.i.cross(A.i + A.j) == sin(q1) * A.k + cos(q1) * A.k assert (A.i + A.j).cross(N.i) == (-sin(q1) - cos(q1)) * N.k assert A.i.cross(C.i) == sin(q3) * C.j assert A.i.cross(C.j) == -sin(q3) * C.i + cos(q3) * C.k assert A.i.cross(C.k) == -cos(q3) * C.j assert C.i.cross(A.i) == (-sin(q3)*cos(q2))*A.j + \ (-sin(q2)*sin(q3))*A.k assert C.j.cross(A.i) == (sin(q2)) * A.j + (-cos(q2)) * A.k assert express(C.k.cross(A.i), C).trigsimp() == cos(q3) * C.j
def test_dyadic(): a, b = symbols('a, b') assert Dyadic.zero != 0 assert isinstance(Dyadic.zero, DyadicZero) assert BaseDyadic(A.i, A.j) != BaseDyadic(A.j, A.i) assert (BaseDyadic(Vector.zero, A.i) == BaseDyadic(A.i, Vector.zero) == Dyadic.zero) d1 = A.i | A.i d2 = A.j | A.j d3 = A.i | A.j assert isinstance(d1, BaseDyadic) d_mul = a * d1 assert isinstance(d_mul, DyadicMul) assert d_mul.base_dyadic == d1 assert d_mul.measure_number == a assert isinstance(a * d1 + b * d3, DyadicAdd) assert d1 == A.i.outer(A.i) assert d3 == A.i.outer(A.j) v1 = a * A.i - A.k v2 = A.i + b * A.j assert v1 | v2 == v1.outer(v2) == a * (A.i|A.i) + (a*b) * (A.i|A.j) +\ - (A.k|A.i) - b * (A.k|A.j) assert d1 * 0 == Dyadic.zero assert d1 != Dyadic.zero assert d1 * 2 == 2 * (A.i | A.i) assert d1 / 2. == 0.5 * d1 assert d1.dot(0 * d1) == Vector.zero assert d1 & d2 == Dyadic.zero assert d1.dot(A.i) == A.i == d1 & A.i assert d1.cross(Vector.zero) == Dyadic.zero assert d1.cross(A.i) == Dyadic.zero assert d1 ^ A.j == d1.cross(A.j) assert d1.cross(A.k) == -A.i | A.j assert d2.cross(A.i) == -A.j | A.k == d2 ^ A.i assert A.i ^ d1 == Dyadic.zero assert A.j.cross(d1) == -A.k | A.i == A.j ^ d1 assert Vector.zero.cross(d1) == Dyadic.zero assert A.k ^ d1 == A.j | A.i assert A.i.dot(d1) == A.i & d1 == A.i assert A.j.dot(d1) == Vector.zero assert Vector.zero.dot(d1) == Vector.zero assert A.j & d2 == A.j assert d1.dot(d3) == d1 & d3 == A.i | A.j == d3 assert d3 & d1 == Dyadic.zero q = symbols('q') B = A.orient_new_axis('B', q, A.k) assert express(d1, B) == express(d1, B, B) expr1 = ((cos(q)**2) * (B.i | B.i) + (-sin(q) * cos(q)) * (B.i | B.j) + (-sin(q) * cos(q)) * (B.j | B.i) + (sin(q)**2) * (B.j | B.j)) assert (express(d1, B) - expr1).simplify() == Dyadic.zero expr2 = (cos(q)) * (B.i | A.i) + (-sin(q)) * (B.j | A.i) assert (express(d1, B, A) - expr2).simplify() == Dyadic.zero expr3 = (cos(q)) * (A.i | B.i) + (-sin(q)) * (A.i | B.j) assert (express(d1, A, B) - expr3).simplify() == Dyadic.zero assert d1.to_matrix(A) == Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]]) assert d1.to_matrix(A, B) == Matrix([[cos(q), -sin(q), 0], [0, 0, 0], [0, 0, 0]]) assert d3.to_matrix(A) == Matrix([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) a, b, c, d, e, f = symbols('a, b, c, d, e, f') v1 = a * A.i + b * A.j + c * A.k v2 = d * A.i + e * A.j + f * A.k d4 = v1.outer(v2) assert d4.to_matrix(A) == Matrix([[a * d, a * e, a * f], [b * d, b * e, b * f], [c * d, c * e, c * f]]) d5 = v1.outer(v1) C = A.orient_new_axis('C', q, A.i) for expected, actual in zip(C.rotation_matrix(A) * d5.to_matrix(A) * \ C.rotation_matrix(A).T, d5.to_matrix(C)): assert (expected - actual).simplify() == 0
def test_Vector_diffs(): q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4') q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1) q1dd, q2dd, q3dd, q4dd = dynamicsymbols('q1 q2 q3 q4', 2) N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q3, N.z]) B = A.orientnew('B', 'Axis', [q2, A.x]) v1 = q2 * A.x + q3 * N.y v2 = q3 * B.x + v1 v3 = v1.dt(B) v4 = v2.dt(B) v5 = q1*A.x + q2*A.y + q3*A.z assert v1.dt(N) == q2d * A.x + q2 * q3d * A.y + q3d * N.y assert v1.dt(A) == q2d * A.x + q3 * q3d * N.x + q3d * N.y assert v1.dt(B) == (q2d * A.x + q3 * q3d * N.x + q3d * N.y - q3 * cos(q3) * q2d * N.z) assert v2.dt(N) == (q2d * A.x + (q2 + q3) * q3d * A.y + q3d * B.x + q3d * N.y) assert v2.dt(A) == q2d * A.x + q3d * B.x + q3 * q3d * N.x + q3d * N.y assert v2.dt(B) == (q2d * A.x + q3d * B.x + q3 * q3d * N.x + q3d * N.y - q3 * cos(q3) * q2d * N.z) assert v3.dt(N) == (q2dd * A.x + q2d * q3d * A.y + (q3d**2 + q3 * q3dd) * N.x + q3dd * N.y + (q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v3.dt(A) == (q2dd * A.x + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) * N.y + (q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v3.dt(B) == (q2dd * A.x - q3 * cos(q3) * q2d**2 * A.y + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) * N.y + (2 * q3 * sin(q3) * q2d * q3d - 2 * cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v4.dt(N) == (q2dd * A.x + q3d * (q2d + q3d) * A.y + q3dd * B.x + (q3d**2 + q3 * q3dd) * N.x + q3dd * N.y + (q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v4.dt(A) == (q2dd * A.x + q3dd * B.x + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) * N.y + (q3 * sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v4.dt(B) == (q2dd * A.x - q3 * cos(q3) * q2d**2 * A.y + q3dd * B.x + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) * N.y + (2 * q3 * sin(q3) * q2d * q3d - 2 * cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z) assert v5.dt(B) == q1d*A.x + (q3*q2d + q2d)*A.y + (-q2*q2d + q3d)*A.z assert v5.dt(A) == q1d*A.x + q2d*A.y + q3d*A.z assert v5.dt(N) == (-q2*q3d + q1d)*A.x + (q1*q3d + q2d)*A.y + q3d*A.z assert v3.diff(q1d, N) == 0 assert v3.diff(q2d, N) == A.x - q3 * cos(q3) * N.z assert v3.diff(q3d, N) == q3 * N.x + N.y assert v3.diff(q1d, A) == 0 assert v3.diff(q2d, A) == A.x - q3 * cos(q3) * N.z assert v3.diff(q3d, A) == q3 * N.x + N.y assert v3.diff(q1d, B) == 0 assert v3.diff(q2d, B) == A.x - q3 * cos(q3) * N.z assert v3.diff(q3d, B) == q3 * N.x + N.y assert v4.diff(q1d, N) == 0 assert v4.diff(q2d, N) == A.x - q3 * cos(q3) * N.z assert v4.diff(q3d, N) == B.x + q3 * N.x + N.y assert v4.diff(q1d, A) == 0 assert v4.diff(q2d, A) == A.x - q3 * cos(q3) * N.z assert v4.diff(q3d, A) == B.x + q3 * N.x + N.y assert v4.diff(q1d, B) == 0 assert v4.diff(q2d, B) == A.x - q3 * cos(q3) * N.z assert v4.diff(q3d, B) == B.x + q3 * N.x + N.y # diff() should only express vector components in the derivative frame if # the orientation of the component's frame depends on the variable v6 = q2**2*N.y + q2**2*A.y + q2**2*B.y # already expressed in N n_measy = 2*q2 # A_C_N does not depend on q2, so don't express in N a_measy = 2*q2 # B_C_N depends on q2, so express in N b_measx = (q2**2*B.y).dot(N.x).diff(q2) b_measy = (q2**2*B.y).dot(N.y).diff(q2) b_measz = (q2**2*B.y).dot(N.z).diff(q2) n_comp, a_comp = v6.diff(q2, N).args assert len(v6.diff(q2, N).args) == 2 # only N and A parts assert n_comp[1] == N assert a_comp[1] == A assert n_comp[0] == Matrix([b_measx, b_measy + n_measy, b_measz]) assert a_comp[0] == Matrix([0, a_measy, 0])