-
Notifications
You must be signed in to change notification settings - Fork 1
/
rotation.py
38 lines (31 loc) · 1.24 KB
/
rotation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
# -*- coding: utf-8 -*-
"""
Created on Fri Jun 12 11:42:32 2020
@author: usth2
"""
############ Rotation_Template.py
#!/usr/bin/env python
from sympy import symbols, cos, sin, pi, sqrt
from sympy.matrices import Matrix
import numpy as np
### Create symbols for joint variables
q1, q2 = symbols('q1:3')
# Create a symbolic matrix representing an extrinsic sequence of rotations
# about the Z and then Y axes. Let the rotation about the Y axis be described
# by q1 and the rotation about Z by q2.
####### TO DO ########
# Replace R_y and R_z with the appropriate (symbolic) elementary rotation matrices
# and then compute ZY_extrinsic.
R_y = Matrix([[ cos(q1), 0, sin(q1)],
[ 0, 1, 0],
[-sin(q1), 0, cos(q1)]])
R_z = Matrix([[ cos(q2), -sin(q2), 0],
[ sin(q2), cos(q2), 0],
[ 0, 0, 1]])
ZY_extrinsic_sym = R_y * R_z
####### TO DO ########
# Numerically evaluate ZY_extrinsic assuming:
# q1 = 45 degrees and q2 = 60 degrees.
# NOTE: Trigonometric functions in Python assume the input is in radians!
dtr = np.pi / 180.0
ZY_extrinsic_num = ZY_extrinsic_sym.evalf(subs={q1: 45*dtr, q2: 60*dtr})