from sympy import symbols, simplify, lambdify, solve
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, inertia, RigidBody, KanesMethod
from pydy.codegen.code import generate_ode_function
from numpy import array, linspace, deg2rad, rad2deg, ones, concatenate, sin, cos, pi, zeros, dot, eye
from numpy.linalg import inv
from scipy.integrate import odeint
from scipy.linalg import solve_continuous_are
import matplotlib.animation as animation
import matplotlib.pyplot as plt
from matplotlib.pyplot import plot, xlabel, ylabel, legend, rcParams
import numpy as np
from body_model_setup import theta1, theta2, theta3,theta4, omega1, omega2, omega3,omega4, l_ankle_torque, l_hip_torque, waist_torque, r_hip_torque, coordinates, speeds, kane, mass_matrix, forcing_vector, specified, parameter_dict, constants, numerical_constants, l_leg, r_leg, crotch, body
from sympy.physics.vector import init_vprinting, vlatex
from math import fabs
init_vprinting()
import pickle
# Control
# =======
inputK = open('bm_LQR_K_useful.pkl','rb')
inputa1 = open('bm_angle_one_useful.pkl','rb')
inputa2 = open('bm_angle_two_useful.pkl','rb')
inputa3 = open('bm_angle_three_useful.pkl','rb')
inputa4 = open('bm_angle_four_useful.pkl','rb')
K = pickle.load(inputK)
a1 = pickle.load(inputa1)
a1 = np.asarray(a1, dtype = float)
a2 = pickle.load(inputa2)
import sympy as sp
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)
from IPython.display import Image
from sympy.physics.mechanics import dynamicsymbols
import math
from math import pi
import numpy as np
import scipy
import scipy.linalg
from mat_mul import mat_mul1
import random
da_coeff = 0.0000001
#import scipy.linalg as spla
alpha, d, theta, a, gamma, l_ck, l_pk, l_mk, l_dk, theta_01, theta_1, theta_2, theta_3, theta_4, c, d = dynamicsymbols(
'alpha d theta a gamma l_ck l_pk,l_mk,l_dk,theta_01,theta_1,theta_2,theta_3,theta_4,c,d'
)
theta_1 = dynamicsymbols('theta_1')
#print(alpha,d,theta,a)
rot = sp.Matrix([[
sp.cos(alpha), -sp.sin(alpha) * sp.cos(theta),
sp.sin(alpha) * sp.sin(theta)
],
[
sp.sin(alpha),
sp.cos(alpha) * sp.cos(theta),
-sp.cos(alpha) * sp.sin(theta)
], [0, sp.sin(theta), sp.cos(theta)]])
trans = sp.Matrix([a * sp.cos(alpha), a * sp.sin(alpha), 0])
'''
## Search for check, TODO for further work
from sympy.physics.mechanics import *
from sympy import symbols
from sympy import simplify
from sympy import Matrix
import numpy as np
from sympy.solvers import solve
from sympy.physics.vector import init_vprinting
from sympy.physics.vector import vlatex, vpprint, vprint
from sympy.physics.vector import kinematic_equations
# from sympy import subs
from sympy import init_printing
init_vprinting(use_latex='mathjax', pretty_print=True, use_unicode=True)
init_printing(use_latex='mathjax', pretty_print=True, use_unicode=True)
print 'Calculation of kinematic model for state estimation with states X, Y, Z, Roll, Pitch, Yaw, Vx, Vy, Vz \n'
'''
Reference frames
---------------
N : Newtonian frame
T : tanget frame or NED frame
B : Body frame
IMU : IMU frame
DVL : DVL frame
'''
N = ReferenceFrame('N')
T = ReferenceFrame('T')
import pydy import sympy as sym import sympy.physics.mechanics as me from scipy.linalg import solve, lstsq from scipy.integrate import odeint import time import functools me.Vector.simp = False # to increase computation speed from sympy.physics.vector import init_vprinting init_vprinting() from matplotlib import animation from matplotlib.patches import Ellipse, Rectangle from IPython.html import widgets from IPython.html.widgets import interact, interactive # my own imports import nchain n = 3 dynamic, symbol, setup, fbd = nchain.formulate_nchain_parameters(n) eom = nchain.make_kane_eom(dynamic, setup, fbd)
############ Humanoid Robot ########
from numpy import array, zeros, eye, asarray, dot, rad2deg
from numpy.linalg import inv
from matplotlib.pyplot import plot, xlabel, ylabel, legend, rcParams
rcParams['figure.figsize'] = (14, 8)
from sympy import simplify, Matrix, matrix2numpy
from sympy.physics.vector import init_vprinting, vlatex
init_vprinting(use_latex='mathjax')
from Utilities.Controller import controllable
from scipy.linalg import solve_continuous_are
from scipy.integrate import odeint
A = np.array([[ 0. , 0. , 0. , 1. , 0. , 0. ],
[ 0. , 0. , 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 1. ],
[ 18.337, -75.864, 6.395, 0. , 0. , 0. ],
[ -22.175, 230.549, -49.01 , 0. , 0. , 0. ],
[ 4.353, -175.393, 95.29 , 0. , 0. , 0. ]])
B = np.array([[ 0. , 0. , 0. ],
[ 0. , 0. , 0. ],
[ 0. , 0. , 0. ],
[ 0.292, -0.785, 0.558],
#%%
from __future__ import print_function, division
from sympy import symbols, simplify, Matrix
from sympy import trigsimp
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, Particle, inertia, RigidBody, KanesMethod
from numpy import deg2rad, rad2deg, array, zeros, linspace
from sympy.physics.vector import init_vprinting, vlatex
import numpy as np
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)
#%%
# define all reference frames of all individually moving links of the robot
print("Defining reference frames")
inertial_frame = ReferenceFrame('I')
lower_leg_left_frame = ReferenceFrame('R_1')
upper_leg_left_frame = ReferenceFrame('R_2')
hip_frame = ReferenceFrame('R_3')
upper_leg_right_frame = ReferenceFrame('R_4')
lower_leg_right_frame = ReferenceFrame('R_5')
#%% Angles
# everything is symbolic, so create all angles of your robot
# NOTE: the angle phi is the angle between your robot and the inertial frame
theta0, theta1, theta2, theta3, phi = dynamicsymbols('theta0, theta1, theta2, theta3, phi')
lower_leg_left_frame.orient(inertial_frame, 'Axis', (phi, inertial_frame.z))
simplify(lower_leg_left_frame.dcm(inertial_frame))
upper_leg_left_frame.orient(lower_leg_left_frame, 'Axis', (theta0, -lower_leg_left_frame.z))
simplify(upper_leg_left_frame.dcm(inertial_frame))
Caso esteja no Windows e o Python tenha sido instalado via Anaconda, SymPy ja vem
instalado, mas talvez possa ser necessario atualiza-lo fazendo
WINDOWS:
conda update sympy numpy
Mais informaçoes podem ser obtidas em>
<https://docs.sympy.org/latest/>
"""
import numpy as np
from numpy import eye
import sympy as sp # Pacote para manipulaçoes simbolicas
from sympy import pi, cos, sin
from sympy.physics.vector import init_vprinting # Usada para printar em latex
from sympy.physics.mechanics import dynamicsymbols
init_vprinting(use_latex='mathjax', pretty_print=False) # Configurando o vprinting
def DH (rows,TDH):
#rows = len(TDH) #Linhas de TDH
theta, alpha, a, d = dynamicsymbols('theta alpha a d')
# Rotação de theta/ em z com Rotação de alpha em x
Rot = sp.Matrix([[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha)],
[ sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha)],
[ 0, sin(alpha), cos(alpha)]])
# Translação dos links
Tran = sp.Matrix([a*cos(theta),a*sin(theta),d])
# Distorção e Escala
