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