├── models ├── __init__.py ├── human_exoskeleton_model_complie_commands.sh ├── README.rst ├── human_exoskeleton_model_generate_c_code.py ├── bicycle_model.py ├── virtual_cycling_simulator_system.py └── human_exoskeleton_modeling_sympy.py ├── test ├── __init__.py ├── README.md ├── test_virtual_cycling_simulator.py ├── annimation_human_model.py ├── test_human_exoskeleton_model.py └── test_bicycle_model.py ├── examples ├── __init__.py ├── README.md ├── pedal_force.py ├── inverse_kinematics.py ├── virtual_cycling_simulator_forward_simulation.py ├── virtual_cycling_simulator_optimal_torque_optimzation.py └── annimation_human_cycling.py ├── pics ├── Principles.png ├── Bicycle Model.png ├── Human Dynamics.png ├── cycling_pic.jpeg ├── full_body_exok.png ├── Bicycle Model_puc.png ├── Optimization-Powers.png ├── Simulation_Torque.png ├── model connections.png ├── Bicycle Contact Model.png ├── Optimization-Torques.png ├── seated_leg_extension.png ├── Virtual Cycling Simulator.png ├── optimize3_result_annimation.gif ├── README.md └── README.rst ├── README.md └── LICENSE /models/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | -------------------------------------------------------------------------------- /test/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | -------------------------------------------------------------------------------- /examples/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | -------------------------------------------------------------------------------- /pics/Principles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Principles.png -------------------------------------------------------------------------------- /pics/Bicycle Model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Bicycle Model.png -------------------------------------------------------------------------------- /pics/Human Dynamics.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Human Dynamics.png -------------------------------------------------------------------------------- /pics/cycling_pic.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/cycling_pic.jpeg -------------------------------------------------------------------------------- /pics/full_body_exok.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/full_body_exok.png -------------------------------------------------------------------------------- /pics/Bicycle Model_puc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Bicycle Model_puc.png -------------------------------------------------------------------------------- /pics/Optimization-Powers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Optimization-Powers.png -------------------------------------------------------------------------------- /pics/Simulation_Torque.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Simulation_Torque.png -------------------------------------------------------------------------------- /pics/model connections.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/model connections.png -------------------------------------------------------------------------------- /pics/Bicycle Contact Model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Bicycle Contact Model.png -------------------------------------------------------------------------------- /pics/Optimization-Torques.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Optimization-Torques.png -------------------------------------------------------------------------------- /pics/seated_leg_extension.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/seated_leg_extension.png -------------------------------------------------------------------------------- /pics/Virtual Cycling Simulator.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/Virtual Cycling Simulator.png -------------------------------------------------------------------------------- /pics/optimize3_result_annimation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Huawei-ClevelandStateUniv/Virtual-Cycling-Simulator/HEAD/pics/optimize3_result_annimation.gif -------------------------------------------------------------------------------- /models/human_exoskeleton_model_complie_commands.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Author: Huawei Wang 4 | # Complie c code model 5 | # Date: 11-20-2018 6 | 7 | 8 | python human_exoskeleton_model_generate_c_code.py 9 | python Q_pydy_codegen_setup.py build_ext --inplace 10 | python dQdq_pydy_codegen_setup.py build_ext --inplace 11 | python dQdqd_pydy_codegen_setup.py build_ext --inplace 12 | python dQdqdd_pydy_codegen_setup.py build_ext --inplace 13 | python dQdf_pydy_codegen_setup.py build_ext --inplace 14 | -------------------------------------------------------------------------------- /models/README.rst: -------------------------------------------------------------------------------- 1 | Introduction 2 | ============= 3 | This folder includes models of human/exoskeleton as well as bicycle. Human/exoskeleton model is created using SymPy and Pydy packages, and in c language verison. Bicycle model is wrote in python. All the models include dynamic euqations and their derivatives. 4 | 5 | Usage 6 | ========== 7 | Excute ``Excute human_exoskeleton_model_complie_commands.sh`` in terminal:: 8 | 9 | $ cd your storage path/Virtual-Cycling-Simulator/models 10 | $ ./human_exoskeleton_model_complie_commands.sh 11 | 12 | C code verison human/exoskeleton model will be generated and compiled. 13 | 14 | Bicycle model is in python language, so there is no need to compile it. 15 | 16 | If there is no issues above, at this line, all the models will be ready to use. 17 | 18 | 19 | -------------------------------------------------------------------------------- /test/README.md: -------------------------------------------------------------------------------- 1 | ## Introduction 2 | 3 | This folder includes test code of all models and their connection system. 4 | 5 | 6 | ### test_bicycle_model.py 7 | 8 | It checked the bicycle model, two items included. 9 | 10 | 1. forward simulation with the rotation of foot points. 11 | 2. derivative check 12 | 13 | 14 | ### test_human_exoskeleton_model.py 15 | 16 | It tested the generated and compiled ``human/exoskeleton model`` (c code) 17 | 18 | Two tests are contained here: leg falling and derivative check 19 | 20 | 1. leg falling: 21 | both left and right leg are lifted vertically up initially. Then 22 | simulation the falling process 23 | 24 | 2. derivative check: 25 | check the derivative of ``q, u, a, and F`` 26 | 27 | 28 | ### test_virtual_cycling_simulator.py 29 | 30 | It checks derivative of the connected models, which is preparing for generating Jacobian matrix in optimization 31 | 32 | 33 | -------------------------------------------------------------------------------- /examples/README.md: -------------------------------------------------------------------------------- 1 | ## Introduction 2 | 3 | 4 | This folder includes examples of how to use the Virtual-Cycling-Simulator 5 | 6 | ### Froward Simulation 7 | 8 | Forward simulation is to let human track reference joint trajectories and see if the generated torques can drive the bicycle model cycling at the constant speed or not. 9 | 10 | - Joints’ trajectories are got from inverse kinematics 11 | - Local PD controller is used to Let human model track the reference joint angles 12 | - Assume exoskeleton is light weight and can generate target torque with good controller 13 | - No external control of exoskeleton and bicycle model 14 | 15 | ### Optimization 16 | 17 | Optimization is to find the minimum torques at all joints that could generate the constant speed cycling. 18 | 19 | - Tracking crank trajectory, as well as minimizing joint torques 20 | - No tracking of joints’ trajectories 21 | - Optimization is in direct collocation format 22 | -------------------------------------------------------------------------------- /examples/pedal_force.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sat Nov 24 20:59:17 2018 5 | 6 | This code is to calculate joint torques of exoskeletons, forces at pedals, and cycling power. 7 | 8 | @author: huawei 9 | """ 10 | 11 | import sys 12 | sys.path.append('../') 13 | 14 | import numpy as np 15 | from models.bicycle_model import VirtualBicycleModel 16 | 17 | 18 | def get_pedal_force(Nnode, interval, nDoF, problem, cy_theta, traj, tor, 19 | Exo_const, Bike_param): 20 | 21 | F_pedal = np.zeros((Nnode, 4)) 22 | 23 | for k in range(Nnode-1): 24 | 25 | q = traj[k+1, :int(nDoF/2-1)] 26 | qd = traj[k+1, int(nDoF/2):-1] 27 | 28 | states_cy = np.hstack((traj[k+1, int(nDoF/2-1)], traj[k+1, int(nDoF-1)], 29 | (traj[k+1, int(nDoF-1)] -traj[k, int(nDoF-1)])/interval)) 30 | 31 | P, Pdot, _, _, _ = problem.FootPoint(q, qd, Exo_const) 32 | 33 | PM = VirtualBicycleModel(Bike_param) 34 | 35 | (f_cy, F_cy, dfdx_cy, dfdP_cy, dfdPdot_cy, dFdx_cy, dFdP_cy, dFdPdot_cy) =\ 36 | PM.rotor_dynamics(states_cy, P, Pdot) 37 | 38 | F_pedal[k+1, :] = -F_cy 39 | 40 | return F_pedal -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Introduction 2 | 3 | ''Virtual-Cycling-Simulator'' is a project to simulate virtual cycling force field. 4 | With the simulator, cycling can be done using an exoskeleton instead of a cycling machine. 5 | The goal of the virtual cycling simulator is to calcualte the torques which hip/knee joints need to overcome at different cycling speed and loads. In other words, this simulator could calculate the joint level force (torque) field for cycling motion. 6 | The method of this research can be used in other motions, for instance, running, walking, and even jumpping. 7 | 8 | # Features 9 | 10 | - A human dynamics model, exoskeleton dynamics model, as well as a bicycle dynamcis model are created. 11 | - Human model and exoskeleton model are created and write into C code using ''SymPy'' and ''Pydy'' packages 12 | - Cycling model is created using python language 13 | - Direct collocaiton format optimization structure code is provided 14 | 15 | For more detail of how these model be created, please look at the [Wiki](https://github.com/HuaweiWang/Virtual-Cycling-Simulator/wiki) of this project. 16 | 17 | # Demo 18 | 19 | [![Demo Doccou alpha](https://github.com/HuaweiWang/Virtual-Cycling-Simulator/blob/master/pics/optimize3_result_annimation.gif)](https://youtu.be/zvIn8QWE_BI) 20 | 21 | # Dependencies 22 | =========== 23 | - numpy >= 1.14.3 24 | - pydy >= 0.4.0 25 | - scipy >= 1.1.0 26 | - sympy >= 1.1.1 27 | - cython >= 0.28.2 28 | - matplotlib >= 2.2.2 29 | - ipopt 30 | - [cyipopt](https://github.com/matthias-k/cyipopt) 31 | 32 | # Usage 33 | 34 | There are two ways of using this simulator: forward simulation and trajectory optimization. 35 | 36 | Both of these two ways are included in example. Please check the README in the examples folder. 37 | 38 | # Conditions of use 39 | 40 | cyipopt is open-source code released under the [EPL](https://www.eclipse.org/legal/epl-v10.html) license. 41 | -------------------------------------------------------------------------------- /test/test_virtual_cycling_simulator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Nov 23 03:09:54 2018 5 | 6 | This code is to check derivative of cycling model for optimization 7 | 8 | @author: huawei 9 | """ 10 | 11 | import sys 12 | sys.path.append('../') 13 | 14 | import numpy as np 15 | from models.virtual_cycling_simulator_system import VirtualCyclingSimulator 16 | 17 | Nnode = 11 18 | nDoF = 10 19 | interval = 0.01 20 | cy_theta = np.random.random(Nnode) 21 | x = np.random.random(Nnode*nDoF + (Nnode-1)*4) 22 | lamda = 0.9 23 | Human_const = np.array([0.8, 0.4, 0.4, 0.23, 0.18, 6.3, 4.1, 0.122, 0.041, 9.8]) # [l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g] 24 | Exo_const = np.array([0.8, 0.4, 0.4, 0.23, 0.18, 4.3, 2.1, 0.082, 0.021, 9.8]) # [l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g] 25 | 26 | Bike_param = [0.8, np.array([0.15, 0.20]), 0.17, 10000, 10, 0.05, 0.05, 1, 1] 27 | 28 | #seat_height=1.0, pedal_center = np.array([0.25, 0.25]), 29 | #pedal_radius=0.17, K=10000, C=10, Ip=0.05, In=0.05, 30 | #Cp=1, Cn=1 31 | 32 | model = VirtualCyclingSimulator(cy_theta, Nnode, nDoF, interval, lamda, Human_const, Exo_const, Bike_param) 33 | 34 | obj0 = model.objective(x) 35 | grad0 = model.gradient(x) 36 | cons0 = model.constraints(x) 37 | 38 | jac0_ele = model.jacobian(x) 39 | (row, col) = model.jacobianstructure() 40 | jac0 = np.zeros((nDoF*(Nnode-1), len(x))) 41 | jac0[row, col] = jac0_ele 42 | 43 | delta = 1e-4 44 | unit = np.eye(len(x)) 45 | 46 | grad_pd = np.zeros_like(x) 47 | jac_pd = np.zeros((nDoF*(Nnode-1), len(x))) 48 | 49 | for m in range(len(x)): 50 | 51 | obj_u = model.objective(x+delta*unit[m, :]) 52 | obj_d = model.objective(x-delta*unit[m, :]) 53 | grad_pd[m] = (obj_u - obj_d)/(2*delta) 54 | 55 | cons_u = model.constraints(x+delta*unit[m, :]) 56 | cons_d = model.constraints(x-delta*unit[m, :]) 57 | jac_pd[:, m] = (cons_u - cons_d)/(2*delta) 58 | 59 | 60 | diff_grad = grad0 - grad_pd 61 | diff_jac = jac0 - jac_pd 62 | 63 | max_diff_grad = np.max(np.abs(diff_grad)) 64 | max_diff_jac = np.max(np.abs(diff_jac)) 65 | 66 | print('The maximum number of diff_grad is: ' + str(max_diff_grad)) 67 | print('The maximum number of diff_jac is: ' + str(max_diff_jac)) -------------------------------------------------------------------------------- /models/human_exoskeleton_model_generate_c_code.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Mon Nov 19 16:06:29 2018 5 | 6 | This code is to generate c code of symbolic dynamic equations and derivatives. 7 | 8 | @author: huawei 9 | """ 10 | 11 | from pydy.codegen.cython_code import CythonMatrixGenerator 12 | from human_exoskeleton_modeling_sympy import PlanarCyclingModel4Segments 13 | import sympy as sm 14 | import sympy.physics.mechanics as me 15 | 16 | sym_kwargs = {'positive': True, 'real': True} 17 | me.dynamicsymbols._t = sm.symbols('t', **sym_kwargs) 18 | 19 | # import code to generate symbolic dynamic equations 20 | sys = PlanarCyclingModel4Segments() 21 | sys._setup_problem() 22 | eoms_equ, kane= sys._generate_eoms() 23 | 24 | # load symbolic state parameters, specific parameters, and constants 25 | q = kane.q[:] 26 | u = kane.u[:] 27 | udot = sys.time_varying_a 28 | T = sys.time_varying_T 29 | F = sys.time_varying_F 30 | constants = sys.parameter_strings 31 | 32 | # write symbolic dynamic euqations into c code as well as the python interface 33 | g = CythonMatrixGenerator([q, u, udot, T, F, constants], [eoms_equ], prefix='Q_pydy_codegen') 34 | setup_py, cython_src, c_header, c_src = g.doprint() 35 | g.write() 36 | 37 | # write the derivative of symbolic dynamic euqations with respect to joint angles into c code as well as the python interface 38 | dfdq = eoms_equ.jacobian(q) 39 | g1 = CythonMatrixGenerator([q, u, udot, T, F, constants], [dfdq], prefix='dQdq_pydy_codegen') 40 | g1.write() 41 | 42 | # write derivative of symbolic dynamic euqations with respect to joint velocities into c code as well as the python interface 43 | dfdu = eoms_equ.jacobian(u) 44 | g2 = CythonMatrixGenerator([q, u, udot, T, F, constants], [dfdu], prefix='dQdqd_pydy_codegen') 45 | g2.write() 46 | 47 | # write derivative of symbolic dynamic euqations with respect to joint accelerations into c code as well as the python interface 48 | dfda = eoms_equ.jacobian(udot) 49 | g3 = CythonMatrixGenerator([q, u, udot, T, F, constants], [dfda], prefix='dQdqdd_pydy_codegen') 50 | g3.write() 51 | 52 | # write derivative of symbolic dynamic euqations with respect to external force into c code as well as the python interface 53 | dfdf = eoms_equ.jacobian(list(F)) 54 | g5 = CythonMatrixGenerator([q, u, udot, T, F, constants], [dfdf], prefix='dQdf_pydy_codegen') 55 | g5.write() 56 | -------------------------------------------------------------------------------- /examples/inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sun Dec 2 01:01:30 2018 5 | 6 | @author: huawei 7 | """ 8 | import numpy as np 9 | from numpy import sin, cos, pi 10 | import matplotlib.pyplot as plt 11 | from scipy.optimize import fsolve 12 | 13 | def generate_joints(problem, cy_theta, Bike_param, Exo_const, Nnode, interval): 14 | 15 | duration = (Nnode-1)*interval 16 | time = np.linspace(0, duration, Nnode) 17 | 18 | Pedal_A = np.array([Bike_param[2]*cos(cy_theta) + Bike_param[1][0], Bike_param[2]*sin(cy_theta) + Bike_param[1][1]]) 19 | Pedal_B = np.array([Bike_param[2]*cos(cy_theta-pi) + Bike_param[1][0], Bike_param[2]*sin(cy_theta-pi) + Bike_param[1][1]]) 20 | 21 | Pedal_Av = np.diff(Pedal_A) 22 | Pedal_Bv = np.diff(Pedal_B) 23 | 24 | joints = np.zeros((Nnode, 8)) 25 | joints[0, :] = np.array([pi/2, -pi/2, pi/6, -pi/6, -pi, pi, pi, -pi]) 26 | 27 | for k in range(Nnode-1): 28 | 29 | def func(x): 30 | P, Pdot, _, _, _ = problem.FootPoint(x[:4], x[4:], Exo_const) 31 | 32 | res = np.zeros(8) 33 | res[:2] = P[:2] - Pedal_A[:, k] 34 | res[2:4] = P[2:4] - Pedal_B[:, k] 35 | res[4:6] = Pdot[:2] - Pedal_Av[:, k] 36 | res[6:8] = Pdot[2:4] - Pedal_Bv[:, k] 37 | 38 | return res 39 | 40 | if k == 0: 41 | x0 = joints[k, :] 42 | else: 43 | x0 = joints[k-1, :] 44 | 45 | xres = fsolve(func, x0) 46 | 47 | joints[k, :] = xres 48 | 49 | sticks = np.zeros((Nnode, 6)) 50 | 51 | sticks[:, 0] = 0 52 | sticks[:, 1] = Exo_const[0] 53 | 54 | sticks[:, 2] = Exo_const[1]*sin(joints[:, 0]) 55 | sticks[:, 3] = Exo_const[0] - Exo_const[1]*cos(joints[:, 0]) 56 | 57 | sticks[:, 4] = Exo_const[1]*sin(joints[:, 0]) + Exo_const[2]*sin(joints[:, 0] + joints[:, 1]) 58 | sticks[:, 5] = Exo_const[0] - Exo_const[1]*cos(joints[:, 0]) - Exo_const[2]*cos(joints[:, 0] + joints[:, 1]) 59 | 60 | 61 | Snode = Nnode-1 62 | plt.figure() 63 | plt.plot(time[:Snode], joints[:Snode, 0], 'r-', label = 'left hip') 64 | plt.plot(time[:Snode], joints[:Snode, 1], 'g-', label = 'left knee') 65 | plt.plot(time[:Snode], joints[:Snode, 2], 'r--', label = 'right hip') 66 | plt.plot(time[:Snode], joints[:Snode, 3], 'g--', label = 'right knee') 67 | plt.legend() 68 | plt.xlabel('time') 69 | plt.xlabel('Angles (rad)') 70 | plt.show(block=True) 71 | 72 | plt.figure(figsize=(6,12)) 73 | for k in range(Snode): 74 | plt.plot(sticks[k, [0, 2, 4]], sticks[k, [1, 3, 5]], 'b-') 75 | plt.plot(Pedal_A[0, :Snode], Pedal_A[1, :Snode], 'r.') 76 | plt.plot(Pedal_B[0, :Snode], Pedal_B[1, :Snode], 'g.') 77 | plt.title('Stick plot of inverse kinematics') 78 | plt.show(block=True) 79 | 80 | return joints -------------------------------------------------------------------------------- /pics/README.md: -------------------------------------------------------------------------------- 1 | ## Manually compile IPOPT and Cyipopt 2 | 3 | Before start, make sure the pip is installed. If not, please use '' sudo apt install python-pip''. 4 | 5 | Then use ''pip'' install the following packages: 6 | 7 | ''pip2 install pkgconfig numpy scipy cython six future wget'' 8 | 9 | 10 | ### First Compile Ipopt 11 | 12 | 1. Download ipopt verison that you would like to have from this page https://www.coin-or.org/download/source/Ipopt/. Here I choiced Ipopt-12.11 13 | 14 | Extract ipopt and put it at home dictionary (recommended) . 15 | 16 | 2. Download the external code using 'wget', example code is as follow: 17 | 18 | Create the Ipopt directionary using the following code: 19 | ''export IPOPTDIR=Path to ipopt/Ipopt-3.12.4'' 20 | 21 | Get the third party solvers for ipopt using wget: 22 | 23 | ''cd $IPOPTDIR/ThirdParty/Blas 24 | ./get.Blas 25 | cd ../Lapack && ./get.Lapack 26 | cd ../ASL && ./get.ASL 27 | cd ../Mumps && ./get.Mumps 28 | cd ../Metis && ./get.Metis'' 29 | 30 | In order to use ma27, ma57, ma86 solvers, HSL source are needed. HSL can only be get from office website (http://www.hsl.rl.ac.uk/ipopt/), academic license is free but needs about 1 day to process. 31 | 32 | Extract HSL source code after you get it. Rename the extracted file to 'coinhsl' and copy it in the HSL folder inside Ipopt- 33 | 3.**.**/ThirdParty/HSL. 34 | 35 | 3. Install Ipopt using ./configure, make, and install commands. 36 | 37 | I used ipopt documents as reference, if you got an error in this process, please check here https://www.coin-or.org/Ipopt/documentation/node14.html 38 | 39 | Type in the following command in your terminal: 40 | cd $IPOPTDIR/build 41 | $IPOPTDIR/configure --prefix=/opt/ipopt/ 42 | make 43 | make test 44 | make install 45 | 46 | Set environment path: 47 | 48 | ''export IPOPTPATH="/home/huawei/Ipopt-3.12.11/build" '' make sure this one starts with root path 49 | 50 | ''export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:$IPOPTPATH/lib/pkgconfig 51 | export PATH=$PATH:$IPOPTPATH/bin'' 52 | 53 | Get help from this web-page if you got error at this step. https://stackoverflow.com/questions/13428910/how-to-set-the-environmental-variable-ld-library-path-in-linux 54 | 55 | ### Connect ipopt with python using cyipopt 56 | 57 | Download cyipopt-0.1.7 from this github: https://github.com/matthias-k/cyipopt 58 | 59 | Extract cyipopt-0.1.7 and put it to home dictionary. 60 | 61 | Write following commands in terminal: 62 | 63 | cd cyipopt-0.1.7 64 | 65 | python setup.py install 66 | if everthing works well you can run the test code now 67 | python -c "import ipopt" 68 | 69 | python examplehs071.py 70 | cd test 71 | 72 | 73 | 74 | Caution 1: Python may cannot find Ipopt while loading it. This can be solved by define LD_LIBRARY_PATH 75 | 76 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/huawei/Ipopt-3.12.10/build/lib 77 | 78 | -------------------------------------------------------------------------------- /examples/virtual_cycling_simulator_forward_simulation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Nov 23 18:55:16 2018 5 | 6 | This code does forward simulation of cycling model 7 | 8 | @author: huawei 9 | """ 10 | 11 | import sys 12 | sys.path.append('../') 13 | 14 | import numpy as np 15 | from models.virtual_cycling_simulator_system import VirtualCyclingSimulator 16 | from examples.inverse_kinematics import generate_joints 17 | from examples.pedal_force import get_pedal_force 18 | from examples.annimation_human_cycling import animate_cycling 19 | from scipy.optimize import fsolve 20 | from numpy import pi 21 | import os 22 | 23 | Nnode = 1001 24 | interval = 0.001 25 | duration = (Nnode-1)*interval 26 | nDoF = 10 27 | 28 | lamda = 0.999999 29 | time = np.linspace(0, 1, Nnode) 30 | cy_theta = np.linspace(0.5*pi, -1.5*pi, Nnode) 31 | 32 | Human_const = np.array([0.8, 0.4, 0.4, 0.23, 0.18, 6.3, 4.1, 0.122, 0.041, 9.8]) 33 | # [l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g] 34 | Exo_const = np.array([0.8, 0.4, 0.4, 0.23, 0.18, 0, 0, 0, 0, 9.8]) 35 | # [l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g] 36 | 37 | bike_loads = 3 38 | 39 | Ip = 0.15*bike_loads 40 | In = 0.15*bike_loads 41 | Cp = 1*bike_loads 42 | Cn = 1*bike_loads 43 | 44 | Bike_param = [0.8, np.array([0.15, 0.20]), 0.17, 200000, 10, Ip, In, Cp, Cn] 45 | #seat_height=1.0, pedal_center = np.array([0.25, 0.25]), 46 | #pedal_radius=0.17, K=10000, C=10, Ip=0.05, In=0.05, 47 | #Cp=1, Cn=1 48 | 49 | K = 10000 50 | D = 50 51 | 52 | problem = VirtualCyclingSimulator(cy_theta, Nnode, nDoF, interval, lamda, Human_const, 53 | Exo_const, Bike_param) 54 | 55 | # the joint angles reference from inverse kinematics 56 | joints_ref = generate_joints(problem, cy_theta, Bike_param, Exo_const, Nnode, interval) 57 | 58 | cy_thetad = np.zeros_like(cy_theta) + (cy_theta[1]-cy_theta[0])/interval 59 | 60 | states = np.zeros((Nnode, nDoF)) 61 | states[0, :4] = joints_ref[0, :4] 62 | states[0, 4] = cy_theta[0] 63 | states[0, 5:9] = joints_ref[0, 4:8] 64 | states[0, 9] = cy_thetad[0] 65 | 66 | tor = np.zeros((Nnode, 4)) 67 | 68 | Snode = 1000 69 | 70 | for k in range(Snode): 71 | 72 | def func(x): 73 | q = x 74 | qd = (x - states[k, :5])/interval 75 | qdd = (qd - states[k, 5:])/interval 76 | 77 | tor[k+1, :] = K*(joints_ref[k+1, :4] - q[:4]) + D*(joints_ref[k+1, 4:] - qd[:4]) 78 | 79 | f, _, _, _, _ = problem.dynamic_equations(q, qd, qdd, tor[k+1, :]) 80 | 81 | return f 82 | 83 | x0 = np.hstack((joints_ref[k+1, :4], cy_theta[k+1])) 84 | 85 | xres = fsolve(func, x0) 86 | 87 | states[k+1, :5] = xres 88 | states[k+1, 5:] = (xres - states[k, :5])/interval 89 | 90 | 91 | Pforce = get_pedal_force(Nnode, interval, nDoF, problem, cy_theta, states, tor, 92 | Exo_const, Bike_param) 93 | 94 | newpath = 'example_result' 95 | if not os.path.exists(newpath): 96 | os.makedirs(newpath) 97 | 98 | animate_cycling(time, states, Pforce, tor, Exo_const, Bike_param, fps = 50, filename = newpath + '/forward_simualtion.mp4') 99 | 100 | -------------------------------------------------------------------------------- /test/annimation_human_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sun Apr 30 13:34:36 2017 5 | 6 | @author: huawei 7 | """ 8 | import numpy as np 9 | from matplotlib import pyplot as plt 10 | from matplotlib import animation 11 | 12 | def animate_human_model(t, states, constants, filename=None): 13 | """Animates the 4-link cycling model and optionally saves it to file. 14 | 15 | Parameters 16 | ---------- 17 | t : ndarray, shape(m) 18 | Time array. 19 | states: ndarray, shape(m,p) 20 | State time history. 21 | filename: string or None, optional 22 | If true a movie file will be saved of the animation. This may take some time. 23 | 24 | Returns 25 | ------- 26 | fig : matplotlib.Figure 27 | The figure. 28 | anim : matplotlib.FuncAnimation 29 | The animation. 30 | 31 | """ 32 | 33 | # first set up the figure, the axis, and the plot elements we want to animate 34 | fig = plt.figure(figsize=(12, 12)) 35 | 36 | # create the axes 37 | ax = plt.axes(xlim=(-1.0, 1.0), ylim=(0.0, 2.0), aspect='equal') 38 | 39 | # display the current time 40 | time_text = ax.text(0.04, 0.9, '', transform=ax.transAxes) 41 | 42 | # blank line for the pendulum 43 | line_link_r, = ax.plot([], [], 'r', lw=3) 44 | line_link_l, = ax.plot([], [], 'b', lw=3) 45 | 46 | # initialization function: plot the background of each frame 47 | def init(): 48 | time_text.set_text('') 49 | 50 | line_link_r.set_data([], []) 51 | line_link_l.set_data([], []) 52 | return (time_text, line_link_r, line_link_l) 53 | 54 | # calcuate sticks based states 55 | def generate_sticks(): 56 | 57 | sticks = np.zeros((len(states[:, 0]), 10)) 58 | sticks[:, 0] = 0 59 | sticks[:, 1] = constants[0] 60 | sticks[:, 2] = np.sin(states[:, 0])*constants[1] 61 | sticks[:, 3] = constants[0] - np.cos(states[:, 0])*constants[1] 62 | sticks[:, 4] = np.sin(states[:, 0])*constants[1] + np.sin(states[:, 0]+states[:, 1])*constants[2] 63 | sticks[:, 5] = constants[0] - np.cos(states[:, 0])*constants[1] - np.cos(states[:, 0]+states[:, 1])*constants[2] 64 | sticks[:, 6] = np.sin(states[:, 2])*constants[1] 65 | sticks[:, 7] = constants[0] - np.cos(states[:, 2])*constants[1] 66 | sticks[:, 8] = np.sin(states[:, 2])*constants[1] + np.sin(states[:, 2]+states[:, 3])*constants[2] 67 | sticks[:, 9] = constants[0] - np.cos(states[:, 2])*constants[1] - np.cos(states[:, 2]+states[:, 3])*constants[2] 68 | 69 | return sticks 70 | 71 | # animation function: update the objects 72 | def animate(i): 73 | time_text.set_text('time = {:2.2f}'.format(t[i])) 74 | 75 | x_l = sticks[:, 2*np.array([0,1,2])] 76 | y_l = sticks[:, 2*np.array([0,1,2])+1] 77 | x_r = sticks[:, 2*np.array([0,3,4])] 78 | y_r = sticks[:, 2*np.array([0,3,4])+1] 79 | 80 | line_link_l.set_data(x_l[i, :], y_l[i, :]) 81 | line_link_r.set_data(x_r[i, :], y_r[i, :]) 82 | 83 | return (time_text, line_link_l, line_link_r) 84 | 85 | # call the animator function 86 | sticks = generate_sticks() 87 | anim = animation.FuncAnimation(fig, animate, frames=len(t), init_func=init, 88 | interval=t[-1] / len(t), blit=True, repeat=False) 89 | 90 | # save the animation if a filename is given 91 | if filename is not None: 92 | anim.save(filename, fps=25, codec='libx264', bitrate=-1) 93 | 94 | -------------------------------------------------------------------------------- /pics/README.rst: -------------------------------------------------------------------------------- 1 | Manually compile IPOPT and Cyipopt (On Ubuntu 18.04, python 2.7.15) 2 | ------------------------------------- 3 | 4 | Before start, make sure ``pip`` is installed. If not, please install it. 5 | 6 | ``sudo apt install python-pip`` 7 | 8 | Then use ``pip`` install the following packages: 9 | 10 | $ pip2 install pkgconfig numpy scipy cython six future wget 11 | 12 | 13 | * Compile Ipopt 14 | 15 | 1. Download ipopt source code, choose the verison that you would like to have from this page . 16 | 17 | Here I used ``Ipopt-3.12.11`` 18 | 19 | Extract ipopt and put it in the floder you want (I put it at ``Home`` folder). 20 | 21 | Create the Ipopt directory: ``export IPOPTDIR=/home/huawei/Ipopt-3.12.11`` 22 | 23 | 2. Download external packages using ``wget``:: 24 | 25 | cd $IPOPTDIR/ThirdParty/Blas 26 | ./get.Blas 27 | cd ../Lapack && ./get.Lapack 28 | cd ../ASL && ./get.ASL 29 | cd ../Mumps && ./get.Mumps 30 | cd ../Metis && ./get.Metis 31 | 32 | To use ``ma27, ma57, ma86`` solvers, ``HSL`` package are needed. ``HSL`` can be get from its official website , academic license is free. 33 | 34 | Extract ''HSL'' source code after you get it. Rename the extracted folder to ``coinhsl`` and copy it in the HSL folder: ``Ipopt-3.12.11/ThirdParty/HSL`` 35 | 36 | 3. Install Ipopt using ``./configure, make, make install`` commands:: 37 | 38 | $ mkdir $IPOPTDIR/build 39 | $ cd $IPOPTDIR/build 40 | $ IPOPTDIR/configure 41 | $ make 42 | $ make test 43 | $ make install 44 | 45 | I used the ipopt document as reference, and it descrips the compile process in a very detail way. 46 | If you get errors in this process, please check it 47 | 48 | 4. Set environment path:: 49 | 50 | $ export IPOPTPATH="/home/huawei/Ipopt-3.12.11/build" # make sure this one starts with the Home path 51 | $ export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:$IPOPTPATH/lib/pkgconfig 52 | $ export PATH=$PATH:$IPOPTPATH/bin 53 | 54 | Get help from this web-page if you get errors in setting environments. 55 | 56 | 57 | * Compile cyipopt 58 | 59 | 1. Download ``cyipopt`` source code from this github: 60 | 61 | Extract cyipopt and put it at the place you want (I put it at Home directory). 62 | 63 | Change the path of terminal to cyipopt: ``cd ~/cyipopt`` 64 | 65 | 2. check that everything linked correctly with ``ldd`` :: 66 | 67 | $ ldd build/lib.linux-x86_64-2.7/cyipopt.so 68 | linux-vdso.so.1 (0x00007ffe895e1000) 69 | libipopt.so.1 => /home/huawei/Ipopt-3.12.11/build/lib/libipopt.so.1 (0x00007f74efc2a000) 70 | libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x00007f74ef839000) 71 | libcoinmumps.so.1 => /home/huawei/Ipopt-3.12.11/build/lib/libcoinmumps.so.1 (0x00007f74ef4ae000) 72 | libcoinhsl.so.1 => /home/huawei/Ipopt-3.12.11/build/lib/libcoinhsl.so.1 (0x00007f74ef169000) 73 | liblapack.so.3 => /usr/lib/x86_64-linux-gnu/liblapack.so.3 (0x00007f74ee8cb000) 74 | libblas.so.3 => /usr/lib/x86_64-linux-gnu/libblas.so.3 (0x00007f74ee65e000) 75 | libdl.so.2 => /lib/x86_64-linux-gnu/libdl.so.2 (0x00007f74ee45a000) 76 | libstdc++.so.6 => /usr/lib/x86_64-linux-gnu/libstdc++.so.6 (0x00007f74ee0d1000) 77 | libm.so.6 => /lib/x86_64-linux-gnu/libm.so.6 (0x00007f74edd33000) 78 | /lib64/ld-linux-x86-64.so.2 (0x00007f74f02c0000) 79 | libgcc_s.so.1 => /lib/x86_64-linux-gnu/libgcc_s.so.1 (0x00007f74edb1b000) 80 | libcoinmetis.so.1 => /home/huawei/Ipopt-3.12.11/build/lib/libcoinmetis.so.1 (0x00007f74ed8ca000) 81 | libgfortran.so.4 => /usr/lib/x86_64-linux-gnu/libgfortran.so.4 (0x00007f74ed4eb000) 82 | 83 | 84 | 3. Compile cyipopt using the command: ``python setup.py install`` 85 | 86 | If there is no error, then you have compiled ``cyipopt`` successfully 87 | 88 | Before try the test code, add Ipopt ``lib`` path to ``LD_LIBRARY_PATH``:: 89 | 90 | $ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/huawei/Ipopt-3.12.11/build/lib 91 | 92 | To make this path works for all terminal, it can be added to ``.bashrc`` :: 93 | 94 | $ echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/huawei/Ipopt-3.12.11/build/lib' >> ~/.bashrc 95 | 96 | 4. Now, you can run the test code:: 97 | 98 | $ cd test 99 | $ python -c "import ipopt" 100 | $ python examplehs071.py 101 | 102 | If it could be run successfully, the optimization will start with the following descriptions:: 103 | 104 | ****************************************************************************** 105 | This program contains Ipopt, a library for large-scale nonlinear optimization. 106 | Ipopt is released as open source code under the Eclipse Public License (EPL). 107 | For more information visit http://projects.coin-or.org/Ipopt 108 | ****************************************************************************** 109 | 110 | This is Ipopt version 3.12.11, running with linear solver ma27. 111 | 112 | 113 | 114 | -------------------------------------------------------------------------------- /examples/virtual_cycling_simulator_optimal_torque_optimzation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Nov 23 04:53:46 2018 5 | 6 | This code optimize the motion tractory as well as joint torques 7 | 8 | @author: huawei 9 | """ 10 | 11 | import sys 12 | sys.path.append('../') 13 | 14 | import ipopt 15 | import numpy as np 16 | from models.virtual_cycling_simulator_system import VirtualCyclingSimulator 17 | from examples.inverse_kinematics import generate_joints 18 | from examples.pedal_force import get_pedal_force 19 | from examples.annimation_human_cycling import animate_cycling 20 | from numpy import pi 21 | import os 22 | 23 | 24 | Nnode = 201 25 | interval = 0.005 26 | duration = (Nnode-1)*interval 27 | time = np.linspace(0, duration, Nnode) 28 | nDoF = 10 29 | 30 | lamda = 0.999999 31 | Human_const = np.array([0.8, 0.4, 0.4, 0.23, 0.18, 6.3, 4.1, 0.122, 0.041, 9.8]) # [l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g] 32 | Exo_const = np.array([0.8, 0.4, 0.4, 0.23, 0.18, 0, 0, 0, 0, 9.8]) # [l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g] 33 | 34 | #for b in range(1, 5): 35 | 36 | bike_loads = 3.2 37 | 38 | Ip = 0.15*bike_loads 39 | In = 0.15*bike_loads 40 | Cp = 1*bike_loads 41 | Cn = 1*bike_loads 42 | 43 | Bike_param = [0.8, np.array([0.15, 0.20]), 0.17, 200000, 10, Ip, In, Cp, Cn] 44 | #seat_height=1.0, pedal_center = np.array([0.15, 0.20]), 45 | #pedal_radius=0.17, K=10000, C=10, Ip=0.05, In=0.05, 46 | #Cp=1, Cn=1 47 | 48 | lhip = np.linspace(0, Nnode*nDoF, Nnode, endpoint = False, dtype = np.int32) 49 | lknee = lhip + 1 50 | rhip = lhip + 2 51 | rknee = lhip + 3 52 | cy_x = lhip + 4 53 | lhipd = lhip + 5 54 | lkneed = lhip + 6 55 | rhipd = lhip + 7 56 | rkneed = lhip + 8 57 | cy_xd = lhip + 9 58 | 59 | lb_x = np.zeros(Nnode*nDoF) 60 | lb_x[lhip] = -pi/4 61 | lb_x[lknee] = -pi 62 | lb_x[rhip] = -pi/4 63 | lb_x[rknee] = -pi 64 | lb_x[cy_x] = -2*pi 65 | 66 | lb_x[lhipd] = -pi/4*10 67 | lb_x[lkneed] = -pi*5 68 | lb_x[rhipd] = -pi/4*10 69 | lb_x[rkneed] = -pi*5 70 | lb_x[cy_xd] = -2*pi*5 71 | 72 | ub_x = np.zeros(Nnode*nDoF) 73 | ub_x[lhip] = pi 74 | ub_x[lknee] = 0.1 75 | ub_x[rhip] = pi 76 | ub_x[rknee] = 0.1 77 | ub_x[cy_x] = pi 78 | 79 | ub_x[lhipd] = pi/4*10 80 | ub_x[lkneed] = pi*5 81 | ub_x[rhipd] = pi/4*10 82 | ub_x[rkneed] = pi*5 83 | ub_x[cy_xd] = 2*pi*5 84 | 85 | lb_u = np.zeros((Nnode-1)*int(nDoF/2-1)) - 500000 86 | ub_u = np.zeros((Nnode-1)*int(nDoF/2-1)) + 500000 87 | 88 | lb = np.hstack((lb_x, lb_u)) 89 | ub = np.hstack((ub_x, ub_u)) 90 | 91 | cl = np.zeros(nDoF*(Nnode -1)) 92 | cu = np.zeros(nDoF*(Nnode -1)) 93 | 94 | cy_theta = np.linspace(0.5*pi, -1.5*pi, Nnode) 95 | cy_thetad = np.zeros_like(cy_theta) + (cy_theta[1]-cy_theta[0])/interval 96 | 97 | model = VirtualCyclingSimulator(cy_theta, Nnode, nDoF, interval, lamda, Human_const, 98 | Exo_const, Bike_param) 99 | 100 | # the joint angles reference from inverse kinematics 101 | joints_ref = generate_joints(model, cy_theta, Bike_param, Exo_const, Nnode, interval) 102 | 103 | nlp = ipopt.problem( 104 | n=Nnode*nDoF + (Nnode-1)*int(nDoF/2-1), 105 | m=nDoF*(Nnode -1), 106 | problem_obj=VirtualCyclingSimulator(cy_theta, Nnode, nDoF, interval, lamda, Human_const, 107 | Exo_const, Bike_param), 108 | lb=lb, 109 | ub=ub, 110 | cl=cl, 111 | cu=cu 112 | ) 113 | 114 | nlp.addOption(b'linear_solver', b'MA86') 115 | nlp.addOption(b'max_iter', 10000) 116 | nlp.addOption(b'hessian_approximation', b'limited-memory') 117 | nlp.addOption(b'tol', 1e-4) 118 | nlp.addOption(b'acceptable_tol', 1e-3) 119 | nlp.addOption(b'max_cpu_time', 1e+5) 120 | 121 | x_init = np.zeros(Nnode*nDoF + (Nnode-1)*int(nDoF/2-1)) 122 | 123 | x_init[lhip] = joints_ref[:, 0]+ 0.001*np.random.random(Nnode) 124 | x_init[lknee] = joints_ref[:, 1]+ 0.001*np.random.random(Nnode) 125 | x_init[rhip] = joints_ref[:, 2]+ 0.001*np.random.random(Nnode) 126 | x_init[rhip] = joints_ref[:, 3]+ 0.001*np.random.random(Nnode) 127 | x_init[cy_x] = cy_theta + 0.001*np.random.random(Nnode) 128 | 129 | x_init[lhipd] = joints_ref[:, 4]+ 0.001*np.random.random(Nnode) 130 | x_init[lkneed] = joints_ref[:, 5]+ 0.001*np.random.random(Nnode) 131 | x_init[rhipd] = joints_ref[:, 6]+ 0.001*np.random.random(Nnode) 132 | x_init[rhipd] = joints_ref[:, 7]+ 0.001*np.random.random(Nnode) 133 | x_init[cy_xd] = cy_thetad + 0.001*np.random.random(Nnode) 134 | 135 | x_init[Nnode*nDoF:] = -2 + 4*np.random.random((Nnode-1)*int(nDoF/2-1)) 136 | x, info = nlp.solve(x_init) 137 | 138 | states = np.zeros((Nnode, nDoF)) 139 | tor = np.zeros((Nnode, nDoF)) 140 | 141 | for m in range(0,Nnode): 142 | for n in range(0,nDoF): 143 | states[m, n] = x[m*nDoF + n] 144 | 145 | for m in range(0,Nnode-1): 146 | for n in range(0, 4): 147 | tor[m, n] = x[Nnode*nDoF + m*4 + n] 148 | 149 | 150 | Pforce = get_pedal_force(Nnode, interval, nDoF, model, cy_theta, states, tor, 151 | Exo_const, Bike_param) 152 | 153 | newpath = 'example_result' 154 | if not os.path.exists(newpath): 155 | os.makedirs(newpath) 156 | 157 | animate_cycling(time, states, Pforce, tor, Exo_const, Bike_param, fps = 50, filename = newpath + '/optimization.mp4') -------------------------------------------------------------------------------- /test/test_human_exoskeleton_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Tue Nov 20 14:31:52 2018 5 | 6 | @author: huawei 7 | """ 8 | 9 | import sys 10 | sys.path.append('../') 11 | 12 | import numpy as np 13 | import os 14 | from models.Q_pydy_codegen import eval as func 15 | from models.dQdq_pydy_codegen import eval as dfdq 16 | from models.dQdqd_pydy_codegen import eval as dfdqd 17 | from models.dQdqdd_pydy_codegen import eval as dfdqdd 18 | from models.dQdf_pydy_codegen import eval as dfdf 19 | from scipy.optimize import fsolve 20 | from annimation_human_model import animate_human_model 21 | 22 | 23 | """ 24 | This code is to test the generated and complied planar cycling model (c code) 25 | 26 | Two tests are contained here: leg falling and derivative check 27 | 28 | -- leg falling: 29 | both left and right leg are lifted vertically up initially. Then 30 | simulation the falling process 31 | 32 | -- derivative check: 33 | check the derivative of q, u, a, and F 34 | 35 | """ 36 | 37 | nDoF = 4 38 | 39 | def leg_fall(ltime= 2): 40 | 41 | Nnode = ltime*100 42 | interval = ltime/Nnode 43 | time = np.linspace(0, ltime, Nnode) 44 | 45 | q = np.zeros((Nnode, nDoF)) # [theta_lh(t), theta_lk(t), theta_rh(t), theta_rk(t)] 46 | qd = np.zeros((Nnode, nDoF)) # [omega_lh(t), omega_lk(t), omega_rh(t), omega_rk(t)] 47 | qdd = np.zeros((Nnode, nDoF)) # [beta_lh(t), beta_lk(t), beta_rh(t), beta_rk(t)] 48 | T = np.zeros((Nnode, nDoF)) # [T_lh(t), T_lk(t), T_rh(t), T_rk(t)] 49 | F = np.zeros((Nnode, nDoF)) # [F_lx(t), F_ly(t), F_rx(t), F_ry(t)] 50 | constants = np.array([1.0, 0.5, 0.4, 0.23, 0.18, 6.3, 4.1, 0.122, 0.041, 9.8]) # [l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g] 51 | res0 = np.zeros(nDoF) #[residuleT_lh, residuleT_lk, residuleF_rh, residuleF_rk] 52 | 53 | q[0, 0] = np.pi + 0.001 54 | q[0, 2] = np.pi - 0.001 55 | 56 | for k in range(Nnode-1): 57 | def func_eval(x): 58 | residule = func(q[k, :], qd[k, :], x, T[k, :], F[k, :], constants, res0) 59 | return residule 60 | x0 = qdd[k, :] 61 | xres = fsolve(func_eval, x0) 62 | 63 | qdd[k+1, :] = xres 64 | qd[k+1, :] = qd[k, :] + interval*qdd[k+1, :] 65 | q[k+1, :] = q[k, :] + interval*qd[k+1, :] 66 | 67 | newpath = 'test_result' 68 | if not os.path.exists(newpath): 69 | os.makedirs(newpath) 70 | 71 | animate_human_model(time, q, constants, filename = newpath + '/leg_fall_test.mp4') 72 | 73 | 74 | def derivative_check(): 75 | 76 | q = np.random.random(4) 77 | qd = np.random.random(4) 78 | qdd = np.random.random(4) 79 | T = np.random.random(4) 80 | F = np.random.random(4) 81 | 82 | constants = np.array([1.0, 0.5, 0.4, 0.23, 0.18, 6.3, 4.1, 0.122, 0.041, 9.8]) # [l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g] 83 | #res0 = np.zeros(self.nDoF) #[residuleT_lh, residuleT_lk, residuleF_rh, residuleF_rk] 84 | jac_dfdq_ini = np.zeros((nDoF*nDoF)) 85 | jac_dfdqd_ini = np.zeros((nDoF*nDoF)) 86 | jac_dfdqdd_ini = np.zeros((nDoF*nDoF)) 87 | jac_dfdf_ini = np.zeros((nDoF*nDoF)) 88 | 89 | 90 | delta = 0.0001 91 | unit_mx = np.eye(4) 92 | 93 | jac_dfdq0 = dfdq(q, qd, qdd, T, F, constants, jac_dfdq_ini) 94 | jac_dfdqd0 = dfdqd(q, qd, qdd, T, F, constants, jac_dfdqd_ini) 95 | jac_dfdqdd0 = dfdqdd(q, qd, qdd, T, F, constants, jac_dfdqdd_ini) 96 | jac_dfdf0 = dfdf(q, qd, qdd, T, F, constants, jac_dfdf_ini) 97 | 98 | jac_dfdq_pd = np.zeros((nDoF, nDoF)) 99 | jac_dfdqd_pd = np.zeros((nDoF, nDoF)) 100 | jac_dfdqdd_pd = np.zeros((nDoF, nDoF)) 101 | jac_dfdf_pd = np.zeros((nDoF, nDoF)) 102 | 103 | res_up = np.zeros(nDoF) 104 | res_dw = np.zeros(nDoF) 105 | 106 | for m in range(nDoF): 107 | 108 | res_up = func(q+delta*unit_mx[m, :], qd, qdd, T, F, constants, res_up) 109 | res_dw = func(q-delta*unit_mx[m, :], qd, qdd, T, F, constants, res_dw) 110 | jac_dfdq_pd[:, m] = (res_up - res_dw)/(2*delta) 111 | 112 | res_up = func(q, qd+delta*unit_mx[m, :], qdd, T, F, constants, res_up) 113 | res_dw = func(q, qd-delta*unit_mx[m, :], qdd, T, F, constants, res_dw) 114 | jac_dfdqd_pd[:, m] = (res_up - res_dw)/(2*delta) 115 | 116 | res_up = func(q, qd, qdd+delta*unit_mx[m, :], T, F, constants, res_up) 117 | res_dw = func(q, qd, qdd-delta*unit_mx[m, :], T, F, constants, res_dw) 118 | jac_dfdqdd_pd[:, m] = (res_up - res_dw)/(2*delta) 119 | 120 | res_up = func(q, qd, qdd, T, F+delta*unit_mx[m, :], constants, res_up) 121 | res_dw = func(q, qd, qdd, T, F-delta*unit_mx[m, :], constants, res_dw) 122 | jac_dfdf_pd[:, m] = (res_up - res_dw)/(2*delta) 123 | 124 | diff_jac_dfdq = jac_dfdq0 - jac_dfdq_pd 125 | diff_jac_dfdqd = jac_dfdqd0 - jac_dfdqd_pd 126 | diff_jac_dfdqdd = jac_dfdqdd0 - jac_dfdqdd_pd 127 | diff_jac_dfdf = jac_dfdf0 - jac_dfdf_pd 128 | 129 | print('The maximum number in dfdq is ' + str(np.max(np.abs(diff_jac_dfdq)))) 130 | print('The maximum number in dfdqd is ' + str(np.max(np.abs(diff_jac_dfdqd)))) 131 | print('The maximum number in dfdqdd is ' + str(np.max(np.abs(diff_jac_dfdqdd)))) 132 | print('The maximum number in dfdf is ' + str(np.max(np.abs(diff_jac_dfdf)))) 133 | 134 | leg_fall() 135 | derivative_check() 136 | 137 | -------------------------------------------------------------------------------- /test/test_bicycle_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Wed Nov 21 16:38:07 2018 5 | 6 | This code is to test pedal_model 7 | 8 | @author: huawei 9 | """ 10 | 11 | import sys 12 | sys.path.append('../') 13 | 14 | import numpy as np 15 | from models.bicycle_model import VirtualBicycleModel 16 | from numpy import pi, sin, cos 17 | from scipy.optimize import fsolve 18 | import matplotlib.pyplot as plt 19 | 20 | 21 | 22 | """ 23 | This code is to check the pedal model. Here are two items checking. 24 | 25 | 1. forward simulation with the rotation of foot points. 26 | 2. derivative check 27 | """ 28 | 29 | # seat_height, crank_center, crank_radius, contact model stiffness, 30 | # contact model damping, inertial of postive cycling, inertial of negative cycling, 31 | # airdynamics of positve cycling, airdyanmcis of negative cycling. 32 | bike_param = [1.0, np.array([0.25, 0.25]), 0.17, 10000, 10, 0.05, 0.05, 1, 1] 33 | 34 | seat_height = bike_param[0] 35 | pedal_center = bike_param[1] 36 | pedal_radius = bike_param[2] 37 | PM = VirtualBicycleModel(bike_param) 38 | 39 | def forward_simulation(): 40 | # forward simulation of crank rotation to see if it can following the foot trajectories. 41 | 42 | Nnode = 240 43 | time = np.linspace(0, 1.2, Nnode) 44 | theta = np.linspace(0, -2*np.pi, Nnode) 45 | 46 | P = np.zeros((Nnode, 4)) 47 | Pdot = np.zeros((Nnode, 4)) 48 | 49 | states = np.zeros((Nnode, 3)) 50 | 51 | 52 | P[:, 0] = pedal_center[0] + pedal_radius*cos(theta) 53 | P[:, 1] = pedal_center[1] + pedal_radius*sin(theta) 54 | 55 | P[:, 2] = pedal_center[0] + pedal_radius*cos(theta+pi) 56 | P[:, 3] = pedal_center[1] + pedal_radius*sin(theta+pi) 57 | 58 | Pdot[:, 0] = pedal_radius*sin(theta) 59 | Pdot[:, 1] = -pedal_radius*cos(theta) 60 | 61 | Pdot[:, 2] = pedal_radius*sin(theta+pi) 62 | Pdot[:, 3] = -pedal_radius*cos(theta+pi) 63 | 64 | states[0, 0] = theta[0] 65 | states[0, 1] = (theta[1] - theta[0])/time[1] 66 | 67 | F = np.zeros((Nnode, 4)) 68 | 69 | for i in range(Nnode-1): 70 | 71 | def func(x): 72 | states_i = np.hstack((states[i, :2], x)) 73 | 74 | (f, F[i, :], dfdx, dfdP, dfdPdot, dFdx, dFdP, dFdPdot) =\ 75 | PM.rotor_dynamics(states_i, P[i, :], Pdot[i, :]) 76 | 77 | return f 78 | 79 | x0 = states[i, 2] 80 | xres = fsolve(func, x0) 81 | 82 | states[i, 2] = xres 83 | states[i+1, 1] = states[i, 1] + states[i, 2]*(time[1]) 84 | states[i+1, 0] = states[i, 0] + states[i+1, 1]*(time[1]) 85 | 86 | plt.figure() 87 | plt.plot(time, theta, 'r-', label='foot rotation') 88 | plt.plot(time, states[:, 0], 'g--', label='pedal rotation') 89 | plt.legend() 90 | plt.xlabel('Time (s)') 91 | plt.ylabel('Rotation angles (rad)') 92 | plt.show(block = True) 93 | 94 | plt.figure() 95 | plt.plot(time, F[:, 0], 'r--', label='left foot force x direction') 96 | plt.plot(time, F[:, 1], 'g--', label='left foot force y direction') 97 | plt.legend() 98 | plt.xlabel('Time (s)') 99 | plt.ylabel('Force (N)') 100 | plt.show(block = True) 101 | 102 | plt.figure() 103 | plt.plot(time, F[:, 2], 'r--', label='right foot force x direction') 104 | plt.plot(time, F[:, 3], 'g--', label='right foot force y direction') 105 | plt.legend() 106 | plt.xlabel('Time (s)') 107 | plt.ylabel('Force (N)') 108 | plt.show(block = True) 109 | 110 | def derivative_check(): 111 | 112 | # derivative check of bicylce model. Check the difference between the 113 | # output of derivative functions and partial derivative methods. 114 | 115 | states = np.random.random(3) 116 | P = np.random.random(4) 117 | Pdot = np.random.random(4) 118 | 119 | delta = 0.00001 120 | Unit3 = np.eye(3) 121 | Unit4 = np.eye(4) 122 | 123 | dfdx_pd = np.zeros(3) 124 | dfdP_pd = np.zeros(4) 125 | dfdPdot_pd = np.zeros(4) 126 | dFdx_pd = np.zeros((4, 3)) 127 | dFdP_pd = np.zeros((4, 4)) 128 | dFdPdot_pd = np.zeros((4, 4)) 129 | 130 | 131 | (f0, F0, dfdx0, dfdP0, dfdPdot0, dFdx0, dFdP0, dFdPdot0) =\ 132 | PM.rotor_dynamics(states, P, Pdot) 133 | 134 | for i in range(4): 135 | if i < 3: 136 | (f_u, F_u, dfdx, dfdP, dfdPdot, dFdx, dFdP, dFdPdot) =\ 137 | PM.rotor_dynamics(states+delta*Unit3[i, :], P, Pdot) 138 | 139 | (f_d, F_d, dfdx, dfdP, dfdPdot, dFdx, dFdP, dFdPdot) =\ 140 | PM.rotor_dynamics(states-delta*Unit3[i, :], P, Pdot) 141 | 142 | dfdx_pd[i] = (f_u - f_d)/(2*delta) 143 | dFdx_pd[:, i] = (F_u - F_d)/(2*delta) 144 | 145 | 146 | (f_u, F_u, dfdx, dfdP, dfdPdot, dFdx, dFdP, dFdPdot) =\ 147 | PM.rotor_dynamics(states, P+delta*Unit4[i, :], Pdot) 148 | 149 | (f_d, F_d, dfdx, dfdP, dfdPdot, dFdx, dFdP, dFdPdot) =\ 150 | PM.rotor_dynamics(states, P-delta*Unit4[i, :], Pdot) 151 | 152 | dfdP_pd[i] = (f_u - f_d)/(2*delta) 153 | dFdP_pd[:, i] = (F_u - F_d)/(2*delta) 154 | 155 | (f_u, F_u, dfdx, dfdP, dfdPdot, dFdx, dFdP, dFdPdot) =\ 156 | PM.rotor_dynamics(states, P, Pdot+delta*Unit4[i, :]) 157 | 158 | (f_d, F_d, dfdx, dfdP, dfdPdot, dFdx, dFdP, dFdPdot) =\ 159 | PM.rotor_dynamics(states, P, Pdot-delta*Unit4[i, :]) 160 | 161 | dfdPdot_pd[i] = (f_u - f_d)/(2*delta) 162 | dFdPdot_pd[:, i] = (F_u - F_d)/(2*delta) 163 | 164 | diff_dfdx = dfdx_pd - dfdx0 165 | diff_dFdx = dFdx_pd - dFdx0 166 | 167 | diff_dfdP = dfdP_pd - dfdP0 168 | diff_dFdP = dFdP_pd - dFdP0 169 | 170 | diff_dFdPdot = dFdPdot_pd - dFdPdot0 171 | 172 | print('The maximum number in dfdx is: ' + str(np.max(np.abs(diff_dfdx)))) 173 | print('The maximum number in dFdx is: ' + str(np.max(np.abs(diff_dFdx)))) 174 | 175 | print('The maximum number in dfdP is: ' + str(np.max(np.abs(diff_dfdP)))) 176 | 177 | print('The maximum number in dFdP is: ' + str(np.max(np.abs(diff_dFdP)))) 178 | print('The maximum number in dFdPdot is: ' + str(np.max(np.abs(diff_dFdPdot)))) 179 | 180 | forward_simulation() 181 | derivative_check() 182 | 183 | 184 | 185 | -------------------------------------------------------------------------------- /examples/annimation_human_cycling.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sun Apr 30 13:34:36 2017 5 | 6 | @author: huawei 7 | """ 8 | import numpy as np 9 | from matplotlib import pyplot as plt 10 | from matplotlib import animation 11 | 12 | def animate_cycling(t, states, pedal_forces, torques, constants, bike_param, fps = 25, filename=None): 13 | """Animates the 4-link cycling model and optionally saves it to file. 14 | 15 | Parameters 16 | ---------- 17 | t : ndarray, shape(m) 18 | Time array. 19 | states: ndarray, shape(m,p) 20 | State time history. 21 | filename: string or None, optional 22 | If true a movie file will be saved of the animation. This may take some time. 23 | 24 | Returns 25 | ------- 26 | fig : matplotlib.Figure 27 | The figure. 28 | anim : matplotlib.FuncAnimation 29 | The animation. 30 | 31 | """ 32 | 33 | # first set up the figure, the axis, and the plot elements we want to animate 34 | fig = plt.figure(figsize=(12, 12)) 35 | 36 | # create the axes 37 | ax = plt.axes(xlim=(-0.2, .6), ylim=(-0.3, 1.0), aspect='equal') 38 | 39 | # display the current time 40 | time_text = ax.text(0.04, 0.9, '', transform=ax.transAxes) 41 | 42 | # blank line for the pendulum 43 | line_link_r, = ax.plot([], [], 'r', lw=3) 44 | line_link_l, = ax.plot([], [], 'b', lw=3) 45 | line_link_c, = ax.plot([], [], 'k--', lw=1.5) 46 | line_link_p, = ax.plot([], [], 'k', lw=3) 47 | 48 | line_link_flx, = ax.plot([], [], 'g', lw=1.5) 49 | line_link_fly, = ax.plot([], [], 'c', lw=1.5) 50 | line_link_frx, = ax.plot([], [], 'g', lw=1.5) 51 | line_link_fry, = ax.plot([], [], 'c', lw=1.5) 52 | 53 | line_link_tlh, = ax.plot([], [], 'm', lw=5) 54 | line_link_tlk, = ax.plot([], [], 'm', lw=5) 55 | line_link_trh, = ax.plot([], [], 'm', lw=5) 56 | line_link_trk, = ax.plot([], [], 'm', lw=5) 57 | 58 | # initialization function: plot the background of each frame 59 | def init(): 60 | time_text.set_text('') 61 | 62 | line_link_l.set_data([], []) 63 | line_link_r.set_data([], []) 64 | line_link_c.set_data([], []) 65 | line_link_p.set_data([], []) 66 | 67 | line_link_flx.set_data([], []) 68 | line_link_fly.set_data([], []) 69 | line_link_frx.set_data([], []) 70 | line_link_fry.set_data([], []) 71 | 72 | line_link_tlh.set_data([], []) 73 | line_link_tlk.set_data([], []) 74 | line_link_trh.set_data([], []) 75 | line_link_trk.set_data([], []) 76 | 77 | return (time_text, line_link_r, line_link_l, line_link_c, line_link_p, 78 | line_link_flx, line_link_fly, line_link_frx, line_link_fry, 79 | line_link_tlh, line_link_tlk, line_link_trh, line_link_trk) 80 | 81 | # calcuate sticks based states 82 | def generate_sticks(): 83 | 84 | sticks = np.zeros((len(states[:, 0]), 10)) 85 | sticks[:, 0] = 0 86 | sticks[:, 1] = constants[0] 87 | sticks[:, 2] = np.sin(states[:, 0])*constants[1] 88 | sticks[:, 3] = constants[0] - np.cos(states[:, 0])*constants[1] 89 | sticks[:, 4] = np.sin(states[:, 0])*constants[1] + np.sin(states[:, 0]+states[:, 1])*constants[2] 90 | sticks[:, 5] = constants[0] - np.cos(states[:, 0])*constants[1] - np.cos(states[:, 0]+states[:, 1])*constants[2] 91 | sticks[:, 6] = np.sin(states[:, 2])*constants[1] 92 | sticks[:, 7] = constants[0] - np.cos(states[:, 2])*constants[1] 93 | sticks[:, 8] = np.sin(states[:, 2])*constants[1] + np.sin(states[:, 2]+states[:, 3])*constants[2] 94 | sticks[:, 9] = constants[0] - np.cos(states[:, 2])*constants[1] - np.cos(states[:, 2]+states[:, 3])*constants[2] 95 | 96 | return sticks 97 | 98 | def generate_cycling(): 99 | 100 | cycle = np.zeros((100, 2)) 101 | theta = np.linspace(0, 2*np.pi, 100) 102 | cycle[:, 0] = bike_param[1][0] + bike_param[2]*np.sin(theta) 103 | cycle[:, 1] = bike_param[1][1] + bike_param[2]*np.cos(theta) 104 | 105 | return cycle 106 | 107 | def generate_crank(): 108 | 109 | crank = np.zeros((len(states[:, 0]), 4)) 110 | 111 | crank[:, 0] = bike_param[1][0] + bike_param[2]*np.cos(states[:, 4]) 112 | crank[:, 1] = bike_param[1][1] + bike_param[2]*np.sin(states[:, 4]) 113 | 114 | crank[:, 2] = bike_param[1][0] + bike_param[2]*np.cos(states[:, 4]+np.pi) 115 | crank[:, 3] = bike_param[1][1] + bike_param[2]*np.sin(states[:, 4]+np.pi) 116 | 117 | return crank 118 | 119 | def generate_pedal_force(): 120 | 121 | force_vec = np.zeros((len(states[:, 0]), 8)) 122 | 123 | force_vec[:, 0] = bike_param[1][0] + bike_param[2]*np.cos(states[:, 4]) 124 | force_vec[:, 1] = bike_param[1][1] + bike_param[2]*np.sin(states[:, 4]) 125 | force_vec[:, 2] = bike_param[1][0] + bike_param[2]*np.cos(states[:, 4]) + pedal_forces[:, 0]*0.001 126 | force_vec[:, 3] = bike_param[1][1] + bike_param[2]*np.sin(states[:, 4]) + pedal_forces[:, 1]*0.001 127 | 128 | force_vec[:, 4] = bike_param[1][0] + bike_param[2]*np.cos(states[:, 4]+np.pi) 129 | force_vec[:, 5] = bike_param[1][1] + bike_param[2]*np.sin(states[:, 4]+np.pi) 130 | force_vec[:, 6] = bike_param[1][0] + bike_param[2]*np.cos(states[:, 4]+np.pi) + pedal_forces[:, 2]*0.001 131 | force_vec[:, 7] = bike_param[1][1] + bike_param[2]*np.sin(states[:, 4]+np.pi) + pedal_forces[:, 3]*0.001 132 | 133 | return force_vec 134 | 135 | def generate_torque(): 136 | 137 | torque_vec_lh = np.zeros((len(states[:, 0]), 20)) 138 | torque_vec_lk = np.zeros((len(states[:, 0]), 20)) 139 | torque_vec_rh = np.zeros((len(states[:, 0]), 20)) 140 | torque_vec_rk = np.zeros((len(states[:, 0]), 20)) 141 | torque_q_lh = torques[:, 0]*0.1 142 | torque_q_lk = torques[:, 1]*0.1 143 | torque_q_rh = torques[:, 2]*0.1 144 | torque_q_rk = torques[:, 3]*0.1 145 | 146 | x_index = np.linspace(0, 18, 10, endpoint = True, dtype = np.int32) 147 | y_index = x_index + 1 148 | 149 | for n in range(10): 150 | torque_vec_lh[:, 2*n] = np.sin(states[:, 0]+torque_q_lh*0.1*n)*constants[1]*0.2 151 | torque_vec_lh[:, 2*n+1] = constants[0] - np.cos(states[:, 0]+torque_q_lh*0.1*n)*constants[1]*0.2 152 | 153 | torque_vec_lk[:, 2*n] = np.sin(states[:, 0])*constants[1] + np.sin(states[:, 0]+states[:, 1]+torque_q_lk*0.1*n)*constants[2]*0.2 154 | torque_vec_lk[:, 2*n+1] = constants[0] - np.cos(states[:, 0])*constants[1] - np.cos(states[:, 0]+states[:, 1]+torque_q_lk*0.1*n)*constants[2]*0.2 155 | 156 | torque_vec_rh[:, 2*n] = np.sin(states[:, 2]+torque_q_rh*0.1*n)*constants[1]*0.3 157 | torque_vec_rh[:, 2*n+1] = constants[0] - np.cos(states[:, 2]+torque_q_rh*0.1*n)*constants[1]*0.3 158 | 159 | torque_vec_rk[:, 2*n] = np.sin(states[:, 2])*constants[1] + np.sin(states[:, 2]+states[:, 3]+torque_q_rk*0.1*n)*constants[2]*0.2 160 | torque_vec_rk[:, 2*n+1] = constants[0] - np.cos(states[:, 2])*constants[1] - np.cos(states[:, 2]+states[:, 3]+torque_q_rk*0.1*n)*constants[2]*0.2 161 | 162 | return torque_vec_lh, torque_vec_lk, torque_vec_rh, torque_vec_rk, x_index, y_index 163 | 164 | # animation function: update the objects 165 | def animate(i): 166 | time_text.set_text('time = {:2.2f}'.format(t[i])) 167 | 168 | x_l = sticks[:, 2*np.array([0,1,2])] 169 | y_l = sticks[:, 2*np.array([0,1,2])+1] 170 | x_r = sticks[:, 2*np.array([0,3,4])] 171 | y_r = sticks[:, 2*np.array([0,3,4])+1] 172 | 173 | c_x = cycle[:, 0] 174 | c_y = cycle[:, 1] 175 | 176 | x_p = crank[:, [0, 2]] 177 | y_p = crank[:, [1, 3]] 178 | 179 | line_link_l.set_data(x_l[i, :], y_l[i, :]) 180 | line_link_r.set_data(x_r[i, :], y_r[i, :]) 181 | line_link_c.set_data(c_x, c_y) 182 | line_link_p.set_data(x_p[i, :], y_p[i, :]) 183 | 184 | line_link_flx.set_data(force_vec[i, [0, 2]], force_vec[i, [1, 1]]) 185 | line_link_fly.set_data(force_vec[i, [0, 0]], force_vec[i, [1, 3]]) 186 | line_link_frx.set_data(force_vec[i, [4, 6]], force_vec[i, [5, 5]]) 187 | line_link_fry.set_data(force_vec[i, [4, 4]], force_vec[i, [5, 7]]) 188 | 189 | line_link_tlh.set_data(torque_vec_lh[i, x_index], torque_vec_lh[i, y_index]) 190 | line_link_tlk.set_data(torque_vec_lk[i, x_index], torque_vec_lk[i, y_index]) 191 | line_link_trh.set_data(torque_vec_rh[i, x_index], torque_vec_rh[i, y_index]) 192 | line_link_trk.set_data(torque_vec_rk[i, x_index], torque_vec_rk[i, y_index]) 193 | 194 | return (time_text, line_link_l, line_link_r, line_link_c, line_link_p, 195 | line_link_flx, line_link_fly, line_link_frx, line_link_fry, 196 | line_link_tlh, line_link_tlk, line_link_trh, line_link_trk) 197 | 198 | # call the animator function 199 | cycle = generate_cycling() 200 | crank = generate_crank() 201 | sticks = generate_sticks() 202 | force_vec = generate_pedal_force() 203 | torque_vec_lh, torque_vec_lk, torque_vec_rh, torque_vec_rk, x_index, y_index = generate_torque() 204 | anim = animation.FuncAnimation(fig, animate, frames=len(t), init_func=init, 205 | interval=t[-1] / len(t), blit=True, repeat=False) 206 | 207 | # save the animation if a filename is given 208 | if filename is not None: 209 | anim.save(filename, fps=fps, codec='libx264', bitrate=-1) 210 | 211 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Eclipse Public License - v 1.0 2 | 3 | THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE PUBLIC 4 | LICENSE ("AGREEMENT"). ANY USE, REPRODUCTION OR DISTRIBUTION OF THE PROGRAM 5 | CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT. 6 | 7 | 1. DEFINITIONS 8 | 9 | "Contribution" means: 10 | a) in the case of the initial Contributor, the initial code and 11 | documentation distributed under this Agreement, and 12 | b) in the case of each subsequent Contributor: 13 | i) changes to the Program, and 14 | ii) additions to the Program; 15 | 16 | where such changes and/or additions to the Program originate from and are 17 | distributed by that particular Contributor. A Contribution 'originates' from a 18 | Contributor if it was added to the Program by such Contributor itself or 19 | anyone acting on such Contributor's behalf. Contributions do not include 20 | additions to the Program which: (i) are separate modules of software 21 | distributed in conjunction with the Program under their own license agreement, 22 | and (ii) are not derivative works of the Program. 23 | "Contributor" means any person or entity that distributes the Program. 24 | 25 | "Licensed Patents" mean patent claims licensable by a Contributor which are 26 | necessarily infringed by the use or sale of its Contribution alone or when 27 | combined with the Program. 28 | 29 | "Program" means the Contributions distributed in accordance with this 30 | Agreement. 31 | 32 | "Recipient" means anyone who receives the Program under this Agreement, 33 | including all Contributors. 34 | 35 | 2. GRANT OF RIGHTS 36 | 37 | a) Subject to the terms of this Agreement, each Contributor hereby grants 38 | Recipient a non-exclusive, worldwide, royalty-free copyright license to 39 | reproduce, prepare derivative works of, publicly display, publicly 40 | perform, distribute and sublicense the Contribution of such Contributor, 41 | if any, and such derivative works, in source code and object code form. 42 | 43 | b) Subject to the terms of this Agreement, each Contributor hereby grants 44 | Recipient a non-exclusive, worldwide, royalty-free patent license under 45 | Licensed Patents to make, use, sell, offer to sell, import and otherwise 46 | transfer the Contribution of such Contributor, if any, in source code and 47 | object code form. This patent license shall apply to the combination of 48 | the Contribution and the Program if, at the time the Contribution is 49 | added by the Contributor, such addition of the Contribution causes such 50 | combination to be covered by the Licensed Patents. The patent license 51 | shall not apply to any other combinations which include the Contribution. 52 | No hardware per se is licensed hereunder. 53 | 54 | c) Recipient understands that although each Contributor grants the 55 | licenses to its Contributions set forth herein, no assurances are 56 | provided by any Contributor that the Program does not infringe the patent 57 | or other intellectual property rights of any other entity. Each 58 | Contributor disclaims any liability to Recipient for claims brought by 59 | any other entity based on infringement of intellectual property rights or 60 | otherwise. As a condition to exercising the rights and licenses granted 61 | hereunder, each Recipient hereby assumes sole responsibility to secure 62 | any other intellectual property rights needed, if any. For example, if a 63 | third party patent license is required to allow Recipient to distribute 64 | the Program, it is Recipient's responsibility to acquire that license 65 | before distributing the Program. 66 | 67 | d) Each Contributor represents that to its knowledge it has sufficient 68 | copyright rights in its Contribution, if any, to grant the copyright 69 | license set forth in this Agreement. 70 | 71 | 3. REQUIREMENTS 72 | A Contributor may choose to distribute the Program in object code form under 73 | its own license agreement, provided that: 74 | 75 | a) it complies with the terms and conditions of this Agreement; and 76 | 77 | b) its license agreement: 78 | i) effectively disclaims on behalf of all Contributors all 79 | warranties and conditions, express and implied, including warranties 80 | or conditions of title and non-infringement, and implied warranties 81 | or conditions of merchantability and fitness for a particular 82 | purpose; 83 | ii) effectively excludes on behalf of all Contributors all liability 84 | for damages, including direct, indirect, special, incidental and 85 | consequential damages, such as lost profits; 86 | iii) states that any provisions which differ from this Agreement are 87 | offered by that Contributor alone and not by any other party; and 88 | iv) states that source code for the Program is available from such 89 | Contributor, and informs licensees how to obtain it in a reasonable 90 | manner on or through a medium customarily used for software 91 | exchange. 92 | 93 | When the Program is made available in source code form: 94 | 95 | a) it must be made available under this Agreement; and 96 | 97 | b) a copy of this Agreement must be included with each copy of the 98 | Program. 99 | Contributors may not remove or alter any copyright notices contained within 100 | the Program. 101 | 102 | Each Contributor must identify itself as the originator of its Contribution, 103 | if any, in a manner that reasonably allows subsequent Recipients to identify 104 | the originator of the Contribution. 105 | 106 | 4. COMMERCIAL DISTRIBUTION 107 | Commercial distributors of software may accept certain responsibilities with 108 | respect to end users, business partners and the like. While this license is 109 | intended to facilitate the commercial use of the Program, the Contributor who 110 | includes the Program in a commercial product offering should do so in a manner 111 | which does not create potential liability for other Contributors. Therefore, 112 | if a Contributor includes the Program in a commercial product offering, such 113 | Contributor ("Commercial Contributor") hereby agrees to defend and indemnify 114 | every other Contributor ("Indemnified Contributor") against any losses, 115 | damages and costs (collectively "Losses") arising from claims, lawsuits and 116 | other legal actions brought by a third party against the Indemnified 117 | Contributor to the extent caused by the acts or omissions of such Commercial 118 | Contributor in connection with its distribution of the Program in a commercial 119 | product offering. The obligations in this section do not apply to any claims 120 | or Losses relating to any actual or alleged intellectual property 121 | infringement. In order to qualify, an Indemnified Contributor must: a) 122 | promptly notify the Commercial Contributor in writing of such claim, and b) 123 | allow the Commercial Contributor to control, and cooperate with the Commercial 124 | Contributor in, the defense and any related settlement negotiations. The 125 | Indemnified Contributor may participate in any such claim at its own expense. 126 | 127 | For example, a Contributor might include the Program in a commercial product 128 | offering, Product X. That Contributor is then a Commercial Contributor. If 129 | that Commercial Contributor then makes performance claims, or offers 130 | warranties related to Product X, those performance claims and warranties are 131 | such Commercial Contributor's responsibility alone. Under this section, the 132 | Commercial Contributor would have to defend claims against the other 133 | Contributors related to those performance claims and warranties, and if a 134 | court requires any other Contributor to pay any damages as a result, the 135 | Commercial Contributor must pay those damages. 136 | 137 | 5. NO WARRANTY 138 | EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, THE PROGRAM IS PROVIDED ON AN 139 | "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR 140 | IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR CONDITIONS OF TITLE, 141 | NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. Each 142 | Recipient is solely responsible for determining the appropriateness of using 143 | and distributing the Program and assumes all risks associated with its 144 | exercise of rights under this Agreement , including but not limited to the 145 | risks and costs of program errors, compliance with applicable laws, damage to 146 | or loss of data, programs or equipment, and unavailability or interruption of 147 | operations. 148 | 149 | 6. DISCLAIMER OF LIABILITY 150 | EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, NEITHER RECIPIENT NOR ANY 151 | CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY DIRECT, INDIRECT, INCIDENTAL, 152 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION 153 | LOST PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 154 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 155 | ARISING IN ANY WAY OUT OF THE USE OR DISTRIBUTION OF THE PROGRAM OR THE 156 | EXERCISE OF ANY RIGHTS GRANTED HEREUNDER, EVEN IF ADVISED OF THE POSSIBILITY 157 | OF SUCH DAMAGES. 158 | 159 | 7. GENERAL 160 | 161 | If any provision of this Agreement is invalid or unenforceable under 162 | applicable law, it shall not affect the validity or enforceability of the 163 | remainder of the terms of this Agreement, and without further action by the 164 | parties hereto, such provision shall be reformed to the minimum extent 165 | necessary to make such provision valid and enforceable. 166 | 167 | If Recipient institutes patent litigation against any entity (including a 168 | cross-claim or counterclaim in a lawsuit) alleging that the Program itself 169 | (excluding combinations of the Program with other software or hardware) 170 | infringes such Recipient's patent(s), then such Recipient's rights granted 171 | under Section 2(b) shall terminate as of the date such litigation is filed. 172 | 173 | All Recipient's rights under this Agreement shall terminate if it fails to 174 | comply with any of the material terms or conditions of this Agreement and does 175 | not cure such failure in a reasonable period of time after becoming aware of 176 | such noncompliance. If all Recipient's rights under this Agreement terminate, 177 | Recipient agrees to cease use and distribution of the Program as soon as 178 | reasonably practicable. However, Recipient's obligations under this Agreement 179 | and any licenses granted by Recipient relating to the Program shall continue 180 | and survive. 181 | 182 | Everyone is permitted to copy and distribute copies of this Agreement, but in 183 | order to avoid inconsistency the Agreement is copyrighted and may only be 184 | modified in the following manner. The Agreement Steward reserves the right to 185 | publish new versions (including revisions) of this Agreement from time to 186 | time. No one other than the Agreement Steward has the right to modify this 187 | Agreement. The Eclipse Foundation is the initial Agreement Steward. The 188 | Eclipse Foundation may assign the responsibility to serve as the Agreement 189 | Steward to a suitable separate entity. Each new version of the Agreement will 190 | be given a distinguishing version number. The Program (including 191 | Contributions) may always be distributed subject to the version of the 192 | Agreement under which it was received. In addition, after a new version of the 193 | Agreement is published, Contributor may elect to distribute the Program 194 | (including its Contributions) under the new version. Except as expressly 195 | stated in Sections 2(a) and 2(b) above, Recipient receives no rights or 196 | licenses to the intellectual property of any Contributor under this Agreement, 197 | whether expressly, by implication, estoppel or otherwise. All rights in the 198 | Program not expressly granted under this Agreement are reserved. 199 | 200 | This Agreement is governed by the laws of the State of New York and the 201 | intellectual property laws of the United States of America. No party to this 202 | Agreement will bring a legal action under this Agreement more than one year 203 | after the cause of action arose. Each party waives its rights to a jury trial 204 | in any resulting litigation. 205 | -------------------------------------------------------------------------------- /models/bicycle_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Tue Nov 20 21:42:19 2018 5 | 6 | Pedal model of the cycling machine 7 | 8 | @author: huawei 9 | """ 10 | 11 | import numpy as np 12 | from numpy import cos, sin, sqrt 13 | 14 | 15 | 16 | class VirtualBicycleModel(object): 17 | """ 18 | This code model the cycling constraints using contact model. 19 | 20 | The origion of the cycling cycle is at the a certain point ' o' ' at the 21 | global cooridiante, and the radius of the cycling cycle is 'r'. 22 | 23 | Clockwise rotation is positive!!! 24 | 25 | """ 26 | 27 | def __init__(self, bike_param): 28 | 29 | """ 30 | Set up foundamental parameters 31 | 32 | seat_height: seat height, in global coordiante 33 | pedal_center: pedalling cycle center, in global coordinate 34 | pedal_radius: radius of the rotor (m) 35 | k: stiffness of contact model 36 | c: damping parameter of contact model 37 | Inp: inertia of the positve direction, clockwise (N*m/s**2) 38 | Inn: inertia of the negative direction, anticlockwise (N*m/s**2) 39 | Cp: dampping parameter of the rotor when doing positive cycling (Nm*s/rad) 40 | Cn: dampping parameter of the rotor when doing negative cycling (Nm*s/rad) 41 | """ 42 | 43 | self.seat_height = bike_param[0] 44 | self.pedal_center = bike_param[1] 45 | self.pedal_radius = bike_param[2] 46 | 47 | self.K = bike_param[3] 48 | self.C = bike_param[4] 49 | self.Ip = bike_param[5] 50 | self.In = bike_param[6] 51 | self.Cp = bike_param[7] 52 | self.Cn = bike_param[8] 53 | 54 | def coordinate_transfer_global_to_local_position(self, P, theta): 55 | """ 56 | Transfer P from global coordinate to local coordinate with 'O' as original 57 | and rotate theta rad. Positive of theta is clockwise. 58 | 59 | Input: 60 | P: 1 dim array (2), value in global coordinate 61 | O: 1 dim array (2), origin of local coordinate, here is the pedal center 62 | theta: float, rotation rad of the coordinate 63 | 64 | Output: 65 | Po: colum array (2x1), value of P in local coordinate 66 | 67 | Method used: 68 | Tranformation matrix 69 | """ 70 | O = self.pedal_center 71 | Pe = np.array([P[0], P[1], 1]) 72 | 73 | T = np.array([[cos(theta), sin(theta), -O[0]*cos(theta)-O[1]*sin(theta)], 74 | [-sin(theta), cos(theta), O[0]*sin(theta)-O[1]*cos(theta)], 75 | [0, 0, 1]]) 76 | 77 | Po = np.dot(T, Pe) 78 | 79 | dPo_dP = T[:2, :2] 80 | dPo_dtheta = np.zeros(2) 81 | dPo_dtheta[0] = -P[0]*sin(theta) + P[1]*cos(theta) + O[0]*sin(theta) - O[1]*cos(theta) 82 | dPo_dtheta[1] = -P[0]*cos(theta) - P[1]*sin(theta) + O[0]*cos(theta) + O[1]*sin(theta) 83 | 84 | return Po[:2], dPo_dP, dPo_dtheta 85 | 86 | def coordinate_transfer_global_to_local_speed(self, P, theta): 87 | """ 88 | Transfer P from global coordinate to local coordinate with 'O' as original 89 | and rotate theta rad. Positive of theta is clockwise. 90 | 91 | Input: 92 | P: 1 dim array (2), value in global coordinate 93 | O: 1 dim array (2), origin of local coordinate, here is the pedal center 94 | theta: float, rotation rad of the coordinate 95 | 96 | Output: 97 | Po: colum array (3), value of P in local coordinate 98 | 99 | Method used: 100 | Tranformation matrix 101 | """ 102 | 103 | Pe = np.array([P[0], P[1]]) 104 | 105 | T = np.array([[cos(theta), sin(theta)], 106 | [-sin(theta), cos(theta)]]) 107 | 108 | Po = np.dot(T, Pe) 109 | 110 | dPo_dP = T 111 | dPo_dtheta = np.zeros(2) 112 | dPo_dtheta[0] = -P[0]*sin(theta) + P[1]*cos(theta) 113 | dPo_dtheta[1] = -P[0]*cos(theta) - P[1]*sin(theta) 114 | 115 | return Po, dPo_dP, dPo_dtheta 116 | 117 | 118 | def coordinate_transfer_local_to_global_force(self, F, theta): 119 | """ 120 | Transfer P from local coordinate to global coordinate with 'O' as original of global system 121 | and rotate theta rad. Positive of theta is clockwise. 122 | 123 | Input: 124 | F: 1 dim array (2), force vector in local coordinate 125 | theta: float, rotation rad of the coordinate 126 | 127 | Output: 128 | Fg: 1 dim array (2), force vector in global coordinate 129 | 130 | Method used: 131 | Tranformation matrix 132 | """ 133 | 134 | T = np.array([[cos(theta), -sin(theta)], [sin(theta), cos(theta)]]) 135 | Fg = np.dot(T, F) 136 | 137 | dFg_dF = T 138 | dFg_dtheta = np.zeros(2) 139 | dFg_dtheta[0] = -F[0]*sin(theta) - F[1]*cos(theta) 140 | dFg_dtheta[1] = F[0]*cos(theta) - F[1]*sin(theta) 141 | 142 | return Fg, dFg_dF, dFg_dtheta 143 | 144 | def pedal_contact_model(self, Tr, Trdot, Ft, Ftdot): 145 | """ 146 | Contact Force based on spring-damping model. 147 | Positive force point to A (from B) 148 | 149 | Input: 150 | Tr: colum array (2x1), target point and velocity 151 | Ft: colum array (2x1), actual foot point and velocity 152 | 153 | Output: 154 | F: colum array (2x1), contact force. positive value means direction 155 | same as positive coordinate. 156 | """ 157 | 158 | F = self.K*(Tr - Ft) + self.C*(Trdot - Ftdot) 159 | 160 | dF_dTr = np.array([[self.K, 0], [0, self.K]]) 161 | dF_dTrdot = np.array([[self.C, 0], [0, self.C]]) 162 | dF_dFt = np.array([[-self.K, 0], [0, -self.K]]) 163 | dF_dFtdot = np.array([[-self.C, 0], [0, -self.C]]) 164 | 165 | return F, dF_dTr, dF_dTrdot, dF_dFt, dF_dFtdot 166 | 167 | def smooth_function(self, P1, P2, x, xin): 168 | """ 169 | Smooth function for two values of one parameter change at point x 170 | 171 | Input: 172 | P1: float, value level when input value less than x 173 | P2: float, value level when input value larger than x 174 | x: float, value change point 175 | xin: float, input value 176 | Output: 177 | P: float, value of smoothed function at input value xin 178 | """ 179 | 180 | P = P1 + ((xin-x) + sqrt((xin-x)**2 + 1e-4))/(2*sqrt((xin-x)**2 + 1e-4))*(P2-P1) 181 | 182 | dP_dxin = (x**2 -x*xin + 1e4)/(((xin-x)**2 + 1e-4)*np.sqrt((xin-x)**2 + 1e-4))*(P2-P1) 183 | 184 | return P, dP_dxin 185 | 186 | 187 | def rotor_dynamics(self, x, P, Pdot, theta_dot_rel=0): 188 | """ 189 | Rotor dynamics of the bike, caculate the angluar acceleration based on the bippdal force. Implict equation 190 | and derivative will be given here. 191 | There are two inertias: positive and negative. Postive rotation is harder which has larger inertia; 192 | negative rotation is easier, since no cycling load is driven. 193 | 194 | Input: 195 | Pl: 1 dim array (2x1), left foot point location, in global coordinate 196 | Pr: 1 dim array (2x1), right foot point location, in global coordinate 197 | Pl: 1 dim array (2x1), left foot point velocity, in global coordinate 198 | Pr: 1 dim array (2x1), right foot point velocity, in global coordinate 199 | x: 1 dim array (1x3), rotor rotation angle/velcity/acceleration at current 200 | theta_dot_rel: float, virtual rotor rotation velocity, if theta_dot smaller than it, negative inertia will be applied; 201 | otherwise, positive interia will be applied (rad) 202 | 203 | 204 | Output: 205 | f: float, residue of the rotor dynamics model, in local coordinate 206 | Fl: column array (2x1), contact force at left foot point, in global coordinate 207 | Fr: column array (2x1), contact force at the right foot point, in global coordinate 208 | dfdx: 2 dim array (2x3), derivative of the rotor dynamics with respect to rotor states 209 | dfdPl: 2 dim array (2x2), derivative of the rotor dynamics with respect to left foot point location 210 | dfdPldot: 2 dim array (2x2), derivative of the rotor dynamics with respect to left foot point velocity 211 | dfdPr: 2 dim array (2x2), derivative of the rotor dynamics with respect to right foot point location 212 | dfdPrdot: 2 dim array (2x2), derivative of the rotor dynamics with respect to right foot point velocity 213 | 214 | dFldx: 2 dim array (2x3), derivative of the left foot contact force with respect to rotor states 215 | dFldPl: 2 dim array (2x2), derivative of the left foot contact force with respect to left foot point location 216 | dFldPldot: 2 dim array (2x2), derivative of the left foot contact force with respect to left foot point velocity 217 | dFrdx: 2 dim array (2x3), derivative of the right foot contact force with respect to rotor states 218 | dFrdPl: 2 dim array (2x2), derivative of the right foot contact force with respect to right foot point location 219 | dFldPldot: 2 dim array (2x2), derivative of the left foot contact force with respect to right foot point velocity 220 | 221 | """ 222 | 223 | Pl = P[:2] 224 | Pr = P[2:] 225 | Pldot = Pdot[:2] 226 | Prdot = Pdot[2:] 227 | 228 | Lo, dLo_dPl, dLo_dx = self.coordinate_transfer_global_to_local_position(Pl, x[0]) 229 | Ro, dRo_dPr, dRo_dx = self.coordinate_transfer_global_to_local_position(Pr, x[0]) 230 | 231 | Lodot, dLodot_dPldot, dLodot_dx = self.coordinate_transfer_global_to_local_speed(Pldot, x[0]) 232 | Rodot, dRodot_dPrdot, dRodot_dx = self.coordinate_transfer_global_to_local_speed(Prdot, x[0]) 233 | 234 | Tl = np.array([self.pedal_radius, 0]) 235 | Tldot = np.array([0, self.pedal_radius*x[1]]) 236 | Tr = np.array([-self.pedal_radius, 0]) 237 | Trdot = np.array([0, -self.pedal_radius*x[1]]) 238 | 239 | dTldot_dxd = np.array([0, self.pedal_radius]) 240 | dTrdot_dxd = np.array([0, -self.pedal_radius]) 241 | 242 | Fl, dFl_dTl, dFl_dTldot, dFl_dLo, dFl_dLodot = self.pedal_contact_model(Tl, Tldot, Lo, Lodot) 243 | Fr, dFr_dTr, dFr_dTrdot, dFr_dRo, dFr_dRodot = self.pedal_contact_model(Tr, Trdot, Ro, Rodot) 244 | 245 | ISm, dISm_dxd= self.smooth_function(self.In, self.Ip, theta_dot_rel, x[1]) 246 | CSm, dCSm_dxd = self.smooth_function(self.Cn, self.Cp, theta_dot_rel, x[1]) 247 | 248 | f = -x[2]*ISm - Fl[1]*self.pedal_radius + Fr[1]*self.pedal_radius - CSm*x[1] 249 | 250 | dfdx = np.zeros(len(x)) 251 | dfdP = np.zeros(4) 252 | dfdPdot = np.zeros(4) 253 | 254 | dfdx[0] = (-self.pedal_radius*(dFl_dLo[1, 1]*dLo_dx[1] + dFl_dLodot[1, 1]*dLodot_dx[1]) 255 | + self.pedal_radius*(dFr_dRo[1, 1]*dRo_dx[1] + dFr_dRodot[1, 1]*dRodot_dx[1])) 256 | 257 | dfdx[1] = (-x[2]*dISm_dxd - self.pedal_radius*dFl_dTldot[1, 1]*dTldot_dxd[1] 258 | +self.pedal_radius*dFr_dTrdot[1, 1]*dTrdot_dxd[1] - CSm -x[1]*dCSm_dxd) 259 | dfdx[2] = -ISm 260 | 261 | dfdP[:2] = -self.pedal_radius*dFl_dLo[1, 1]*dLo_dPl[1, :] 262 | dfdP[2:] = self.pedal_radius*dFr_dRo[1, 1]*dRo_dPr[1, :] 263 | 264 | dfdPdot[:2] = -self.pedal_radius*dFl_dLodot[1, 1]*dLodot_dPldot[1, :] 265 | dfdPdot[2:] = self.pedal_radius*dFr_dRodot[1, 1]*dRodot_dPrdot[1, :] 266 | 267 | Flg, dFlg_dFl, dFlg_dx = self.coordinate_transfer_local_to_global_force(Fl, x[0]) 268 | Frg, dFrg_dFr, dFrg_dx = self.coordinate_transfer_local_to_global_force(Fr, x[0]) 269 | 270 | F = np.hstack((Flg, Frg)) 271 | 272 | dFdx = np.zeros((4, len(x))) 273 | dFdP = np.zeros((4, 4)) 274 | dFdPdot = np.zeros((4, 4)) 275 | 276 | dFdx[:2, 0] = (dFlg_dx + np.dot(dFlg_dFl, np.dot(dFl_dLo, dLo_dx)) 277 | + np.dot(dFlg_dFl, np.dot(dFl_dLodot, dLodot_dx))) 278 | dFdx[:2, 1] = np.dot(dFlg_dFl, np.dot(dFl_dTldot, dTldot_dxd)) 279 | 280 | dFdx[2:, 0] = (dFrg_dx + np.dot(dFrg_dFr, np.dot(dFr_dRo, dRo_dx)) 281 | + np.dot(dFrg_dFr, np.dot(dFr_dRodot, dRodot_dx))) 282 | dFdx[2:, 1] = np.dot(dFrg_dFr, np.dot(dFr_dTrdot, dTrdot_dxd)) 283 | 284 | dFdP[:2, :2] = np.dot(dFlg_dFl, np.dot(dFl_dLo, dLo_dPl)) 285 | dFdP[2:, 2:] = np.dot(dFrg_dFr, np.dot(dFr_dRo, dRo_dPr)) 286 | 287 | dFdPdot[:2, :2] = np.dot(dFlg_dFl, np.dot(dFl_dLodot, dLodot_dPldot)) 288 | dFdPdot[2:, 2:] = np.dot(dFrg_dFr, np.dot(dFr_dRodot, dRodot_dPrdot)) 289 | 290 | return (f, F, dfdx, dfdP, dfdPdot, dFdx, dFdP, dFdPdot) 291 | 292 | 293 | 294 | 295 | -------------------------------------------------------------------------------- /models/virtual_cycling_simulator_system.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Thu Nov 22 12:44:38 2018 5 | 6 | This code connectted human, exoskeleton, and bicycle models together. 7 | Also contains functions that generate objective, gradient, constraints, and Jacobian 8 | for trajecotry optimization (direct collocation). 9 | 10 | @author: huawei 11 | """ 12 | 13 | import sys 14 | sys.path.append('../') 15 | 16 | import numpy as np 17 | from models.bicycle_model import VirtualBicycleModel 18 | from models.Q_pydy_codegen import eval as func 19 | from models.dQdq_pydy_codegen import eval as dfdq 20 | from models.dQdqd_pydy_codegen import eval as dfdqd 21 | from models.dQdqdd_pydy_codegen import eval as dfdqdd 22 | from models.dQdf_pydy_codegen import eval as dfdf 23 | from numpy import cos, sin 24 | from scipy.sparse import find 25 | 26 | class VirtualCyclingSimulator(object): 27 | 28 | def __init__(self, cy_theta, Nnode, nDoF, interval, lamda, Human_const, Exo_const, Bike_param): 29 | 30 | # general informaiton of the virtual cycling simulator, including following: 31 | self.cy_theta = cy_theta # Crank angle trajectory (as reference) 32 | self.Nnode = Nnode # Number of nodes in direct collocation 33 | self.nDoF = nDoF # Degree of freedom of the virtual cycling system (4 here) 34 | self.interval = interval # Time interval between direct collocation nodes (usually 0.001~0.01 seconds) 35 | self.lamda = lamda # Weight prameter between crank angle tracking and joint torques (between 0 and 1) 36 | self.Human_const = Human_const # Human model constant information, including segment length, mass, inerial 37 | self.Exo_const = Exo_const # Exoskeleton model constant information, including segment length, mass, inerial 38 | self.Bike_param = Bike_param # Bicycle model constant information, including segment length, mass, inerial 39 | 40 | # index of crank angle index in optiming parameters, including state trajectories and joint torques 41 | self.x_cy_ind = np.linspace(0, Nnode*nDoF, Nnode, endpoint = False, dtype = np.int32) + 4 42 | 43 | def objective(self, x): 44 | 45 | # objective contains two parts: tracking of crank angle trajectories and 46 | # joint torques. 47 | 48 | obj = (self.lamda*np.sum((x[self.x_cy_ind] - self.cy_theta)**2) 49 | + (1-self.lamda)*np.sum(x[self.Nnode*self.nDoF:]**2))/self.Nnode 50 | 51 | return obj 52 | 53 | def gradient(self, x): 54 | 55 | # gradiant is the derivative of objecitve respect to optimizing parameters 56 | 57 | grad = np.zeros_like(x) 58 | grad[self.x_cy_ind] = 2*self.lamda*(x[self.x_cy_ind]-self.cy_theta)/self.Nnode 59 | grad[self.Nnode*self.nDoF:] = 2*(1-self.lamda)*x[self.Nnode*self.nDoF:]/self.Nnode 60 | 61 | return grad 62 | 63 | def constraints(self, x): 64 | 65 | # constraints are the dyanmic equations of the virtual cycling simulator system 66 | # at all direct collocation nodes. 67 | 68 | cons = np.array([]) 69 | 70 | for k in range(self.Nnode-1): 71 | 72 | x_p = x[k*self.nDoF:(k+1)*self.nDoF] 73 | x_d = x[(k+1)*self.nDoF:(k+2)*self.nDoF] 74 | 75 | T = x[self.Nnode*self.nDoF+k*int(self.nDoF/2-1):self.Nnode*self.nDoF+(k+1)*int(self.nDoF/2-1)] 76 | 77 | f, dfdq, dfdqd, dfdT = self.cycling_model(x_d, (x_d-x_p)/self.interval, T) 78 | 79 | cons = np.hstack((cons, f)) 80 | 81 | return cons 82 | 83 | def jacobianstructure(self): 84 | 85 | # To find out the non-zero elements in jacobian matrix. This is achieved 86 | # by randomly generate jacobian matrix three times using random inputs. 87 | # Then find out the row and col of the non-zero elements of the jacobian 88 | # matrix. 89 | 90 | row = np.array([]) 91 | col = np.array([]) 92 | 93 | 94 | for k in range(self.Nnode-1): 95 | 96 | jac_part = np.zeros((self.nDoF, 2*self.nDoF+int(self.nDoF/2-1))) 97 | 98 | for rep in range(3): 99 | x_p = np.random.random(self.nDoF) 100 | x_d = np.random.random(self.nDoF) 101 | T = np.random.random(int(self.nDoF/2-1)) 102 | 103 | f, dfdq, dfdqd, dfdT = self.cycling_model(x_d, (x_d-x_p)/self.interval, T) 104 | 105 | jac_part[:, :self.nDoF] += -dfdqd/self.interval 106 | jac_part[:, self.nDoF:2*self.nDoF] += dfdq + dfdqd/self.interval 107 | jac_part[:, 2*self.nDoF:] += dfdT 108 | 109 | for m in range(self.nDoF): 110 | row_x, col_x, _ = find(jac_part[m, :2*self.nDoF]) 111 | row_T, col_T, _ = find(jac_part[m, 2*self.nDoF:]) 112 | 113 | row_xf = row_x + k*self.nDoF+m 114 | row_Tf = row_T + k*self.nDoF+m 115 | 116 | col_xf = col_x + k*self.nDoF 117 | col_Tf = col_T + self.Nnode*self.nDoF + k*(self.nDoF/2-1) 118 | 119 | row = np.hstack((row, row_xf, row_Tf)) 120 | col = np.hstack((col, col_xf, col_Tf)) 121 | 122 | row = row.astype(int) 123 | col = col.astype(int) 124 | 125 | return (row, col) 126 | 127 | 128 | def jacobian(self, x): 129 | 130 | # jacobian matrix (only store non-zero elements). 131 | # this jacobian matrix include the jacobian of all nodes. 132 | 133 | jac = np.array([]) 134 | 135 | for k in range(self.Nnode-1): 136 | 137 | jac_part = np.zeros((self.nDoF, 2*self.nDoF+int(self.nDoF/2-1))) 138 | 139 | x_p = x[k*self.nDoF:(k+1)*self.nDoF] 140 | x_d = x[(k+1)*self.nDoF:(k+2)*self.nDoF] 141 | 142 | T = x[self.Nnode*self.nDoF+k*int(self.nDoF/2-1):self.Nnode*self.nDoF+(k+1)*int(self.nDoF/2-1)] 143 | 144 | f, dfdq, dfdqd, dfdT = self.cycling_model(x_d, (x_d-x_p)/self.interval, T) 145 | 146 | jac_part[:, :self.nDoF] = -dfdqd/self.interval 147 | jac_part[:, self.nDoF:2*self.nDoF] = dfdq + dfdqd/self.interval 148 | jac_part[:, 2*self.nDoF:] = dfdT 149 | 150 | _, _, nele= find(jac_part) 151 | 152 | jac = np.hstack((jac, nele)) 153 | 154 | return jac 155 | 156 | def cycling_model(self, q, qd, T): 157 | 158 | # generate constriant and jacobian of one point. 159 | 160 | q1 = q[:int(self.nDoF/2)] 161 | q1d = q[int(self.nDoF/2):] 162 | q1dd = qd[int(self.nDoF/2):] 163 | 164 | f, dfdq, dfdqd, dfdqdd, dfdT = self.dynamic_equations(q1, q1d, q1dd, T) 165 | 166 | f_full = np.zeros(self.nDoF) 167 | dfdq_full = np.zeros((self.nDoF, self.nDoF)) 168 | dfdqd_full = np.zeros((self.nDoF, self.nDoF)) 169 | dfdT_full = np.zeros((self.nDoF, int(self.nDoF/2-1))) 170 | 171 | f_full[:int(self.nDoF/2)] = q[int(self.nDoF/2):] - qd[:int(self.nDoF/2)] 172 | f_full[int(self.nDoF/2):] = f 173 | 174 | dfdq_full[:int(self.nDoF/2), int(self.nDoF/2):] = np.eye(int(self.nDoF/2)) 175 | dfdq_full[int(self.nDoF/2):, :int(self.nDoF/2)] = dfdq 176 | dfdq_full[int(self.nDoF/2):, int(self.nDoF/2):] = dfdqd 177 | 178 | dfdqd_full[:int(self.nDoF/2), :int(self.nDoF/2)] = -np.eye(int(self.nDoF/2)) 179 | dfdqd_full[int(self.nDoF/2):, int(self.nDoF/2):] = dfdqdd 180 | 181 | dfdT_full[int(self.nDoF/2):, :] = dfdT 182 | 183 | return f_full, dfdq_full, dfdqd_full, dfdT_full 184 | 185 | def dynamic_equations(self, q, qd, qdd, T_input): 186 | 187 | # connect all three models together. Find Wiki of this project to see 188 | # how do them connected. 189 | 190 | T0 = np.zeros(4) 191 | F0 = np.zeros(4) 192 | res_exo_ini = np.zeros(int(self.nDoF/2-1)) #[residuleT_lh, residuleT_lk, residuleF_rh, residuleF_rk] 193 | jac_exo_dfdq_ini = np.zeros(int(self.nDoF/2-1)*int(self.nDoF/2-1)) 194 | jac_exo_dfdqd_ini = np.zeros(int(self.nDoF/2-1)*int(self.nDoF/2-1)) 195 | jac_exo_dfdqdd_ini = np.zeros(int(self.nDoF/2-1)*int(self.nDoF/2-1)) 196 | jac_exo_dfdF_ini = np.zeros(int(self.nDoF/2-1)*int(self.nDoF/2-1)) 197 | 198 | res_human_ini = np.zeros(int(self.nDoF/2-1)) #[residuleT_lh, residuleT_lk, residuleF_rh, residuleF_rk] 199 | jac_human_dfdq_ini = np.zeros(int(self.nDoF/2-1)*int(self.nDoF/2-1)) 200 | jac_human_dfdqd_ini = np.zeros(int(self.nDoF/2-1)*int(self.nDoF/2-1)) 201 | jac_human_dfdqdd_ini = np.zeros(int(self.nDoF/2-1)*int(self.nDoF/2-1)) 202 | 203 | (P, Pdot, dPdq, dPdotdq, dPdotdqd) = self.FootPoint(q[:4], qd[:4], self.Exo_const) 204 | 205 | states_cy = np.hstack((q[4], qd[4], qdd[4])) 206 | 207 | PM = VirtualBicycleModel(self.Bike_param) 208 | 209 | (f_cy, F_cy, dfdx_cy, dfdP_cy, dfdPdot_cy, dFdx_cy, dFdP_cy, dFdPdot_cy) =\ 210 | PM.rotor_dynamics(states_cy, P, Pdot) 211 | 212 | f_exo = func(q[:4], qd[:4], qdd[:4], T0, F_cy, self.Exo_const, res_exo_ini) 213 | jac_dfdq_exo = dfdq(q[:4], qd[:4], qdd[:4], T0, F_cy, self.Exo_const, jac_exo_dfdq_ini) 214 | jac_dfdqd_exo = dfdqd(q[:4], qd[:4], qdd[:4], T0, F_cy, self.Exo_const, jac_exo_dfdqd_ini) 215 | jac_dfdqdd_exo = dfdqdd(q[:4], qd[:4], qdd[:4], T0, F_cy, self.Exo_const, jac_exo_dfdqdd_ini) 216 | jac_dfdF_exo = dfdf(q[:4], qd[:4], qdd[:4], T0, F_cy, self.Exo_const, jac_exo_dfdF_ini) 217 | 218 | delta_T = T_input + f_exo 219 | 220 | f_human = func(q[:4], qd[:4], qdd[:4], delta_T, F0, self.Human_const, res_human_ini) 221 | jac_dfdq_human = dfdq(q[:4], qd[:4], qdd[:4], delta_T, F0, self.Human_const, jac_human_dfdq_ini) 222 | jac_dfdqd_human = dfdqd(q[:4], qd[:4], qdd[:4], delta_T, F0, self.Human_const, jac_human_dfdqd_ini) 223 | jac_dfdqdd_human = dfdqdd(q[:4], qd[:4], qdd[:4], delta_T, F0, self.Human_const, jac_human_dfdqdd_ini) 224 | #jac_dfdF_human = dfdf(q, qd, qdd, delta_T, F0, constants_human, jac_dfdF_ini) 225 | 226 | f_full = np.hstack((f_human, f_cy)) 227 | 228 | dfdq_full = np.zeros((int(self.nDoF/2), int(self.nDoF/2))) 229 | dfdqd_full = np.zeros((int(self.nDoF/2), int(self.nDoF/2))) 230 | dfdqdd_full = np.zeros((int(self.nDoF/2), int(self.nDoF/2))) 231 | 232 | dfdT_full = np.zeros((int(self.nDoF/2), int(self.nDoF/2-1))) 233 | 234 | dfdq_full[:4, :4] = jac_dfdq_human + (jac_dfdq_exo + np.dot(jac_dfdF_exo, 235 | (np.dot(dFdP_cy, dPdq) + np.dot(dFdPdot_cy, dPdotdq)))) 236 | 237 | dfdq_full[:4, 4] = np.dot(np.eye(4), np.dot(jac_dfdF_exo, dFdx_cy[:, 0])) 238 | dfdq_full[4, :4] = np.dot(dfdP_cy, dPdq) + np.dot(dfdPdot_cy, dPdotdq) 239 | dfdq_full[4, 4] = dfdx_cy[0] 240 | 241 | 242 | dfdqd_full[:4, :4] = jac_dfdqd_human + np.dot(np.eye(4), jac_dfdqd_exo 243 | + np.dot(jac_dfdF_exo, np.dot(dFdPdot_cy, dPdotdqd))) 244 | dfdqd_full[:4, 4] = np.dot(np.eye(4), np.dot(jac_dfdF_exo, dFdx_cy[:, 1])) 245 | dfdqd_full[4, :4] = np.dot(dfdPdot_cy, dPdotdqd) 246 | dfdqd_full[4, 4] = dfdx_cy[1] 247 | 248 | dfdqdd_full[:4, :4] = jac_dfdqdd_human + np.dot(np.eye(4), jac_dfdqdd_exo) 249 | dfdqdd_full[:4, 4] = np.dot(np.eye(4), np.dot(jac_dfdF_exo, dFdx_cy[:, 2])) 250 | dfdqdd_full[4, 4] = dfdx_cy[2] 251 | 252 | dfdT_full[:4, :4] = np.eye(4) 253 | 254 | return f_full, dfdq_full, dfdqd_full, dfdqdd_full, dfdT_full 255 | 256 | 257 | def FootPoint(self, q, qd, constants_exosk): 258 | 259 | # kenimatics of human/exokeleton model. 260 | 261 | # from joint angle/velocity to foot locaiton and velocity 262 | 263 | P = np.zeros(4) 264 | Pdot = np.zeros(4) 265 | 266 | P[0] = constants_exosk[1]*sin(q[0]) + constants_exosk[2]*sin(q[0]+q[1]) 267 | P[1] = constants_exosk[0] - (constants_exosk[1]*cos(q[0]) + constants_exosk[2]*cos(q[0]+q[1])) 268 | 269 | P[2] = constants_exosk[1]*sin(q[2]) + constants_exosk[2]*sin(q[2]+q[3]) 270 | P[3] = constants_exosk[0] - (constants_exosk[1]*cos(q[2]) + constants_exosk[2]*cos(q[2]+q[3])) 271 | 272 | Pdot[0] = constants_exosk[1]*cos(q[0])*qd[0] + constants_exosk[2]*cos(q[0]+q[1])*(qd[0]+qd[1]) 273 | Pdot[1] = constants_exosk[1]*sin(q[0])*qd[0] + constants_exosk[2]*sin(q[0]+q[1])*(qd[0]+qd[1]) 274 | 275 | Pdot[2] = constants_exosk[1]*cos(q[2])*qd[2] + constants_exosk[2]*cos(q[2]+q[3])*(qd[2]+qd[3]) 276 | Pdot[3] = constants_exosk[1]*sin(q[2])*qd[2] + constants_exosk[2]*sin(q[2]+q[3])*(qd[2]+qd[3]) 277 | 278 | 279 | dPdq = np.zeros((4, 4)) 280 | dPdotdq = np.zeros((4, 4)) 281 | dPdotdqd = np.zeros((4, 4)) 282 | 283 | dPdq[0, 0] = constants_exosk[1]*cos(q[0]) + constants_exosk[2]*cos(q[0]+q[1]) 284 | dPdq[0, 1] = constants_exosk[2]*cos(q[0]+q[1]) 285 | dPdq[1, 0] = constants_exosk[1]*sin(q[0]) + constants_exosk[2]*sin(q[0]+q[1]) 286 | dPdq[1, 1] = constants_exosk[2]*sin(q[0]+q[1]) 287 | 288 | dPdq[2, 2] = constants_exosk[1]*cos(q[2]) + constants_exosk[2]*cos(q[2]+q[3]) 289 | dPdq[2, 3] = constants_exosk[2]*cos(q[2]+q[3]) 290 | dPdq[3, 2] = constants_exosk[1]*sin(q[2]) + constants_exosk[2]*sin(q[2]+q[3]) 291 | dPdq[3, 3] = constants_exosk[2]*sin(q[2]+q[3]) 292 | 293 | dPdotdq[0, 0] = -constants_exosk[1]*sin(q[0])*qd[0] - constants_exosk[2]*sin(q[0]+q[1])*(qd[0]+qd[1]) 294 | dPdotdq[0, 1] = -constants_exosk[2]*sin(q[0]+q[1])*(qd[0]+qd[1]) 295 | dPdotdq[1, 0] = constants_exosk[1]*cos(q[0])*qd[0] + constants_exosk[2]*cos(q[0]+q[1])*(qd[0]+qd[1]) 296 | dPdotdq[1, 1] = constants_exosk[2]*cos(q[0]+q[1])*(qd[0]+qd[1]) 297 | 298 | dPdotdq[2, 2] = -constants_exosk[1]*sin(q[2])*qd[2] - constants_exosk[2]*sin(q[2]+q[3])*(qd[2]+qd[3]) 299 | dPdotdq[2, 3] = -constants_exosk[2]*sin(q[2]+q[3])*(qd[2]+qd[3]) 300 | dPdotdq[3, 2] = constants_exosk[1]*cos(q[2])*qd[2] + constants_exosk[2]*cos(q[2]+q[3])*(qd[2]+qd[3]) 301 | dPdotdq[3, 3] = constants_exosk[2]*cos(q[2]+q[3])*(qd[2]+qd[3]) 302 | 303 | dPdotdqd[0, 0] = constants_exosk[1]*cos(q[0]) + constants_exosk[2]*cos(q[0]+q[1]) 304 | dPdotdqd[0, 1] = constants_exosk[2]*cos(q[0]+q[1]) 305 | dPdotdqd[1, 0] = constants_exosk[1]*sin(q[0]) + constants_exosk[2]*sin(q[0]+q[1]) 306 | dPdotdqd[1, 1] = constants_exosk[2]*sin(q[0]+q[1]) 307 | 308 | dPdotdqd[2, 2] = constants_exosk[1]*cos(q[2]) + constants_exosk[2]*cos(q[2]+q[3]) 309 | dPdotdqd[2, 3] = constants_exosk[2]*cos(q[2]+q[3]) 310 | dPdotdqd[3, 2] = constants_exosk[1]*sin(q[2]) + constants_exosk[2]*sin(q[2]+q[3]) 311 | dPdotdqd[3, 3] = constants_exosk[2]*sin(q[2]+q[3]) 312 | 313 | return P, Pdot, dPdq, dPdotdq, dPdotdqd -------------------------------------------------------------------------------- /models/human_exoskeleton_modeling_sympy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Tue Nov 13 23:38:14 2018 5 | 6 | This code is to generate dynamics equation of a 4 DoF multi-bodies system for cycling 7 | 8 | @author: huawei 9 | """ 10 | 11 | from collections import OrderedDict 12 | import sympy as sy 13 | import sympy.physics.mechanics as me 14 | 15 | sym_kwargs = {'positive': True, 'real': True} 16 | me.dynamicsymbols._t = sy.symbols('t', **sym_kwargs) 17 | 18 | 19 | class PlanarCyclingModel4Segments(object): 20 | 21 | """ 22 | In this class we are going to make a simple model of a human cycling model 23 | We will make several assumptions: 24 | 25 | - The human's motion is limited to a 2D plane (i.e. leaning backwards and forwards). 26 | - The model have four links: Left Thigh, Left Shank, Right Thigh, Right Shank. 27 | - The model have 4 degrees of freedom: rotation of l/r thigh, l/r shank 28 | - The forces generated by the muscles will be modeled as ideal torques between the adjacent body segments. 29 | - There is a contact point at the external point of shank, which generate contact force with environment (petals). 30 | 31 | Reference Frames 32 | ---------------- 33 | 34 | There are five reference frames and nine important points. The inertial reference frame, 35 | $I$, is rigidly attached to the ground. The left thigh reference frame, 36 | $LT$, is attached to the inertial reference by a pin joint at the hip point $H$ and rotates relative to the trunk 37 | through $\theta_1$. The left shank reference frame, $LS$, is attached to the left shank by a pin joint at 38 | left knee point $LK$ and rotates relative to the left thigh through angle $\theta_2$. The right thigh 39 | reference frame, $RT$, is attached to the inertial reference frame by a pin joint at the hip point $H$ and rotates relative 40 | to the trunk through $\theta_3$. The left shank reference frame, $RS$, is attached to the left shank by 41 | a pin joint at right knee point $RK$ and rotates relative to the left thigh through angle $\theta_4$.. 42 | Note that all rotations are about the $z$ axis and that they are relative to the orientation of the preceding body. 43 | 44 | Geometry 45 | -------- 46 | 47 | The thigh and shank's lengths are defined as $l_T$ and $l_S$. 48 | 49 | Mass Centers 50 | ------------- 51 | 52 | The four points $LT_o$, $LS_o$, $RT_o$, $RL_o$ are the mass centers of the body segments. 53 | These are each located on the line connecting the proximal and distal joints of each body segment 54 | and are located by the dimensions: $d_{T}$, and $d_{S}$. 55 | 56 | Gravity 57 | ------- 58 | 59 | Gravity is directed downwards ($-y$) and applies a force with a magnitude of $m_T, m_S$ at each mass center, respectively. 60 | 61 | Torques 62 | ------- 63 | 64 | Two forces and five torques represent the forces due to muscles contracting. 65 | Two forces are the $x$ and $y$ direction force on the trunk. Four torques are 66 | the torque on left legs $T_{LH}$, knee $T_{LK}$, and torques on right legs $T_{RH}$, $T_{RK}$, 67 | torques apply equal and opposite torques to the adjoining body segments. 68 | """ 69 | 70 | def __init__(self, implicit = True): 71 | 72 | """ 73 | Parameters 74 | ========== 75 | implicit : boolean 76 | This boolean parameter that determined whether the dyanmic equation implicit or explicit 77 | """ 78 | self.implicit = implicit 79 | 80 | 81 | def _create_states(self): 82 | 83 | self.time = me.dynamicsymbols._t 84 | 85 | syms_q = 'theta_lh, theta_lk, theta_rh, theta_rk' 86 | syms_u = 'omega_lh, omega_lk, omega_rh, omega_rk' 87 | syms_a = 'beta_lh, beta_lk, beta_rh, beta_rk' 88 | 89 | time_varying_q = [s(self.time) for s in 90 | sy.symbols(syms_q, cls=sy.Function, real=True)] 91 | 92 | time_varying_u = [s(self.time) for s in 93 | sy.symbols(syms_u, cls=sy.Function, real=True)] 94 | 95 | self.time_varying_a = [s(self.time) for s in 96 | sy.symbols(syms_a, cls=sy.Function, real=True)] 97 | 98 | self.coordinates = OrderedDict() 99 | self.coordinates['left_hip'] = time_varying_q[0] 100 | self.coordinates['left_knee'] = time_varying_q[1] 101 | self.coordinates['right_hip'] = time_varying_q[2] 102 | self.coordinates['right_knee'] = time_varying_q[3] 103 | 104 | self.speeds = OrderedDict() 105 | self.speeds['left_hip'] = time_varying_u[0] 106 | self.speeds['left_knee'] = time_varying_u[1] 107 | self.speeds['right_hip'] = time_varying_u[2] 108 | self.speeds['right_knee'] = time_varying_u[3] 109 | 110 | self.accelerations = OrderedDict() 111 | self.accelerations['left_hip'] = self.time_varying_a[0] 112 | self.accelerations['left_knee'] = self.time_varying_a[1] 113 | self.accelerations['right_hip'] = self.time_varying_a[2] 114 | self.accelerations['right_knee'] = self.time_varying_a[3] 115 | 116 | 117 | def _create_specified(self): 118 | 119 | self.time_varying_F = [s(self.time) for s in sy.symbols('F_lx, F_ly, F_rx, F_ry', 120 | cls=sy.Function, real=True)] 121 | 122 | self.time_varying_T = [s(self.time) for s in sy.symbols('T_lh, T_lk, T_rh, T_rk', 123 | cls=sy.Function, real=True)] 124 | 125 | self.specified = OrderedDict() 126 | self.specified['left_force_x'] = self.time_varying_F[0] 127 | self.specified['left_force_y'] = self.time_varying_F[1] 128 | self.specified['right_force_x'] = self.time_varying_F[2] 129 | self.specified['right_force_y'] = self.time_varying_F[3] 130 | self.specified['left_hip_torque'] = self.time_varying_T[0] 131 | self.specified['left_knee_torque'] = self.time_varying_T[1] 132 | self.specified['right_hip_torque'] = self.time_varying_T[2] 133 | self.specified['right_knee_torque'] = self.time_varying_T[3] 134 | 135 | def _create_parameters(self): 136 | 137 | self.parameter_strings = sy.symbols('l_St, l_T, l_S, d_T, d_S, m_T, m_S, i_T, i_S, g', **sym_kwargs) 138 | 139 | self.parameters = OrderedDict() 140 | self.parameters['seat_height'] = self.parameter_strings[0] 141 | self.parameters['thigh_length'] = self.parameter_strings[1] 142 | self.parameters['shank_length'] = self.parameter_strings[2] 143 | self.parameters['thigh_com_length'] = self.parameter_strings[3] 144 | self.parameters['shank_com_length'] = self.parameter_strings[4] 145 | self.parameters['thigh_mass'] = self.parameter_strings[5] 146 | self.parameters['shank_mass'] = self.parameter_strings[6] 147 | self.parameters['thigh_inertia'] = self.parameter_strings[7] 148 | self.parameters['shank_inertia'] = self.parameter_strings[8] 149 | self.parameters['g'] = self.parameter_strings[9] 150 | 151 | def _create_reference_frames(self): 152 | 153 | # Set Reference Frames 154 | 155 | self.frames = OrderedDict() 156 | 157 | self.frames['inertial'] = me.ReferenceFrame('I') 158 | 159 | self.frames['left_thigh'] = me.ReferenceFrame('LT') 160 | self.frames['left_shank'] = me.ReferenceFrame('LS') 161 | 162 | self.frames['right_thigh'] = me.ReferenceFrame('RT') 163 | self.frames['right_shank'] = me.ReferenceFrame('RS') 164 | 165 | def _orient_reference_frames(self): 166 | 167 | self.frames['left_thigh'].orient(self.frames['inertial'], 168 | 'Axis', 169 | (self.coordinates['left_hip'], 170 | self.frames['inertial'].z)) 171 | 172 | self.frames['left_shank'].orient(self.frames['left_thigh'], 173 | 'Axis', 174 | (self.coordinates['left_knee'], 175 | self.frames['left_thigh'].z)) 176 | 177 | self.frames['right_thigh'].orient(self.frames['inertial'], 178 | 'Axis', 179 | (self.coordinates['right_hip'], 180 | self.frames['inertial'].z)) 181 | 182 | self.frames['right_shank'].orient(self.frames['right_thigh'], 183 | 'Axis', 184 | (self.coordinates['right_knee'], 185 | self.frames['right_thigh'].z)) 186 | 187 | def _create_points(self): 188 | 189 | self.points = OrderedDict() 190 | self.points['origin'] = me.Point('O') 191 | self.points['hip'] = me.Point('H') 192 | self.points['lknee'] = me.Point('LK') 193 | self.points['lankle'] = me.Point('LA') 194 | self.points['rknee'] = me.Point('RK') 195 | self.points['rankle'] = me.Point('RA') 196 | self.points['lthigh_mass_center'] = me.Point('LT_o') 197 | self.points['lshank_mass_center'] = me.Point('LS_o') 198 | self.points['rthigh_mass_center'] = me.Point('RT_o') 199 | self.points['rshank_mass_center'] = me.Point('RS_o') 200 | 201 | 202 | def _set_positions(self): 203 | 204 | vec = self.parameters['seat_height'] * self.frames['inertial'].y 205 | self.points['hip'].set_pos(self.points['hip'], vec) 206 | 207 | vec = -self.parameters['thigh_length'] * self.frames['left_thigh'].y 208 | self.points['lknee'].set_pos(self.points['hip'], vec) 209 | 210 | vec = -self.parameters['shank_length'] * self.frames['left_shank'].y 211 | self.points['lankle'].set_pos(self.points['lknee'], vec) 212 | 213 | vec = -self.parameters['thigh_length'] * self.frames['right_thigh'].y 214 | self.points['rknee'].set_pos(self.points['hip'], vec) 215 | 216 | vec = -self.parameters['shank_length'] * self.frames['right_shank'].y 217 | self.points['rankle'].set_pos(self.points['rknee'], vec) 218 | 219 | vec = -self.parameters['thigh_com_length'] * self.frames['left_thigh'].y 220 | self.points['lthigh_mass_center'].set_pos(self.points['hip'], vec) 221 | 222 | vec = -self.parameters['shank_com_length'] * self.frames['left_shank'].y 223 | self.points['lshank_mass_center'].set_pos(self.points['lknee'], vec) 224 | 225 | vec = -self.parameters['thigh_com_length'] * self.frames['right_thigh'].y 226 | self.points['rthigh_mass_center'].set_pos(self.points['hip'], vec) 227 | 228 | vec = -self.parameters['shank_com_length'] * self.frames['right_shank'].y 229 | self.points['rshank_mass_center'].set_pos(self.points['rknee'], vec) 230 | 231 | def _define_kin_diff_eqs(self): 232 | 233 | self.kin_diff_eqs = (self.speeds['left_hip'] - 234 | self.coordinates['left_hip'].diff(self.time), 235 | self.speeds['left_knee'] - 236 | self.coordinates['left_knee'].diff(self.time), 237 | self.speeds['right_hip'] - 238 | self.coordinates['right_hip'].diff(self.time), 239 | self.speeds['right_knee'] - 240 | self.coordinates['right_knee'].diff(self.time)) 241 | 242 | def _set_angular_velocities(self): 243 | 244 | vec = self.speeds['left_hip'] * self.frames['inertial'].z 245 | self.frames['left_thigh'].set_ang_vel(self.frames['inertial'], vec) 246 | 247 | vec = self.speeds['left_knee'] * self.frames['left_thigh'].z 248 | self.frames['left_shank'].set_ang_vel(self.frames['left_thigh'], vec) 249 | 250 | vec = self.speeds['right_hip'] * self.frames['inertial'].z 251 | self.frames['right_thigh'].set_ang_vel(self.frames['inertial'], vec) 252 | 253 | vec = self.speeds['right_knee'] * self.frames['right_thigh'].z 254 | self.frames['right_shank'].set_ang_vel(self.frames['right_thigh'], vec) 255 | 256 | 257 | def _set_linear_velocities(self): 258 | 259 | self.points['hip'].set_vel(self.frames['inertial'], 0) 260 | 261 | self.points['lknee'].v2pt_theory(self.points['hip'], 262 | self.frames['inertial'], 263 | self.frames['left_thigh']) 264 | 265 | self.points['lankle'].v2pt_theory(self.points['lknee'], 266 | self.frames['inertial'], 267 | self.frames['left_shank']) 268 | 269 | self.points['lthigh_mass_center'].v2pt_theory(self.points['hip'], 270 | self.frames['inertial'], 271 | self.frames['left_thigh']) 272 | 273 | self.points['lshank_mass_center'].v2pt_theory(self.points['lknee'], 274 | self.frames['inertial'], 275 | self.frames['left_shank']) 276 | 277 | 278 | self.points['rknee'].v2pt_theory(self.points['hip'], 279 | self.frames['inertial'], 280 | self.frames['right_thigh']) 281 | 282 | self.points['rankle'].v2pt_theory(self.points['rknee'], 283 | self.frames['inertial'], 284 | self.frames['right_shank']) 285 | 286 | self.points['rthigh_mass_center'].v2pt_theory(self.points['hip'], 287 | self.frames['inertial'], 288 | self.frames['right_thigh']) 289 | 290 | self.points['rshank_mass_center'].v2pt_theory(self.points['rknee'], 291 | self.frames['inertial'], 292 | self.frames['right_shank']) 293 | 294 | def _create_inertia_dyadics(self): 295 | 296 | lthigh_inertia_dyadic = me.inertia(self.frames['left_thigh'], 0, 0, 297 | self.parameters['thigh_inertia']) 298 | 299 | lshank_inertia_dyadic = me.inertia(self.frames['left_shank'], 0, 0, 300 | self.parameters['shank_inertia']) 301 | 302 | rthigh_inertia_dyadic = me.inertia(self.frames['right_thigh'], 0, 0, 303 | self.parameters['thigh_inertia']) 304 | 305 | rshank_inertia_dyadic = me.inertia(self.frames['right_shank'], 0, 0, 306 | self.parameters['shank_inertia']) 307 | 308 | self.central_inertias = OrderedDict() 309 | self.central_inertias['lthigh'] = (lthigh_inertia_dyadic, 310 | self.points['lthigh_mass_center']) 311 | self.central_inertias['lshank'] = (lshank_inertia_dyadic, 312 | self.points['lshank_mass_center']) 313 | 314 | self.central_inertias['rthigh'] = (rthigh_inertia_dyadic, 315 | self.points['rthigh_mass_center']) 316 | self.central_inertias['rshank'] = (rshank_inertia_dyadic, 317 | self.points['rshank_mass_center']) 318 | 319 | def _create_rigid_bodies(self): 320 | 321 | self.rigid_bodies = OrderedDict() 322 | 323 | self.rigid_bodies['lthigh'] = \ 324 | me.RigidBody('lthigh', 325 | self.points['lthigh_mass_center'], 326 | self.frames['left_thigh'], 327 | self.parameters['thigh_mass'], 328 | self.central_inertias['lthigh']) 329 | 330 | self.rigid_bodies['lshank'] = \ 331 | me.RigidBody('lshank', 332 | self.points['lshank_mass_center'], 333 | self.frames['left_shank'], 334 | self.parameters['shank_mass'], 335 | self.central_inertias['lshank']) 336 | 337 | self.rigid_bodies['rthigh'] = \ 338 | me.RigidBody('rthigh', 339 | self.points['rthigh_mass_center'], 340 | self.frames['right_thigh'], 341 | self.parameters['thigh_mass'], 342 | self.central_inertias['rthigh']) 343 | 344 | self.rigid_bodies['rshank'] = \ 345 | me.RigidBody('rshank', 346 | self.points['rshank_mass_center'], 347 | self.frames['right_shank'], 348 | self.parameters['shank_mass'], 349 | self.central_inertias['rshank']) 350 | 351 | def _create_loads(self): 352 | 353 | self.loads = OrderedDict() 354 | 355 | g = self.parameters['g'] 356 | 357 | vec = -self.parameters['thigh_mass'] * g * self.frames['inertial'].y 358 | self.loads['lthigh_force'] = (self.points['lthigh_mass_center'], vec) 359 | 360 | vec = -self.parameters['shank_mass'] * g * self.frames['inertial'].y 361 | self.loads['lshank_force'] = (self.points['lshank_mass_center'], vec) 362 | 363 | vec = -self.parameters['thigh_mass'] * g * self.frames['inertial'].y 364 | self.loads['rthigh_force'] = (self.points['rthigh_mass_center'], vec) 365 | 366 | vec = -self.parameters['shank_mass'] * g * self.frames['inertial'].y 367 | self.loads['rshank_force'] = (self.points['rshank_mass_center'], vec) 368 | 369 | vec = (self.specified['left_force_x'] * self.frames['inertial'].x 370 | + self.specified['left_force_y'] * self.frames['inertial'].y) 371 | self.loads['lcontact_force'] = (self.points['lankle'], vec) 372 | 373 | vec = (self.specified['right_force_x'] * self.frames['inertial'].x 374 | + self.specified['right_force_y'] * self.frames['inertial'].y) 375 | self.loads['rcontact_force'] = (self.points['rankle'], vec) 376 | 377 | 378 | vec = (self.specified['left_hip_torque'] * self.frames['inertial'].z - 379 | self.specified['left_knee_torque'] * self.frames['inertial'].z) 380 | self.loads['lthigh_torque'] = (self.frames['left_thigh'], vec) 381 | 382 | 383 | self.loads['lshank_torque'] = (self.frames['left_shank'], 384 | self.specified['left_knee_torque'] 385 | *self.frames['inertial'].z) 386 | 387 | vec = (self.specified['right_hip_torque'] * self.frames['inertial'].z - 388 | self.specified['right_knee_torque'] * self.frames['inertial'].z) 389 | self.loads['rthigh_torque'] = (self.frames['right_thigh'], vec) 390 | 391 | 392 | self.loads['rshank_torque'] = (self.frames['right_shank'], 393 | self.specified['right_knee_torque'] 394 | *self.frames['inertial'].z) 395 | 396 | def _setup_problem(self): 397 | self._create_states() 398 | self._create_specified() 399 | self._create_parameters() 400 | self._create_reference_frames() 401 | self._orient_reference_frames() 402 | self._create_points() 403 | self._set_positions() 404 | self._define_kin_diff_eqs() 405 | self._set_angular_velocities() 406 | self._set_linear_velocities() 407 | self._create_inertia_dyadics() 408 | self._create_rigid_bodies() 409 | self._create_loads() 410 | 411 | def _generate_eoms(self): 412 | 413 | self.kane = me.KanesMethod(self.frames['inertial'], 414 | list(self.coordinates.values()), 415 | list(self.speeds.values()), 416 | self.kin_diff_eqs) 417 | 418 | fr, frstar = self.kane.kanes_equations(list(self.rigid_bodies.values()), 419 | list(self.loads.values())) 420 | 421 | fr_plus_frstar = sy.trigsimp(fr + frstar) 422 | 423 | sub = {self.speeds['left_hip'].diff(self.time): 424 | self.accelerations['left_hip'], 425 | self.speeds['left_knee'].diff(self.time): 426 | self.accelerations['left_knee'], 427 | self.speeds['right_hip'].diff(self.time): 428 | self.accelerations['right_hip'], 429 | self.speeds['right_knee'].diff(self.time): 430 | self.accelerations['right_knee']} 431 | 432 | self.fr_plus_frstar = fr_plus_frstar.xreplace(sub) 433 | 434 | return self.fr_plus_frstar, self.kane --------------------------------------------------------------------------------