├── config ├── cmd.py ├── linear_mpc_configs.py └── robot_configs.py ├── .gitignore ├── robot ├── a1 │ ├── meshes │ │ └── trunk_A1.png │ ├── a1_license.txt │ └── urdf │ │ └── a1.urdf └── aliengo │ ├── meshes │ ├── hip.stl │ ├── calf.stl │ ├── thigh.stl │ ├── trunk.stl │ └── thigh_mirror.stl │ ├── aliengo.xml │ └── urdf │ └── aliengo.urdf ├── doc ├── results │ └── trotting10_mujoco.gif ├── state_estimation_kf.md └── linear_mpc.md ├── utils ├── dynamics.py ├── kinematics.py ├── isaacgym_utils.py └── robot_data.py ├── LICENSE ├── README.md ├── requirements.txt ├── linear_mpc ├── leg_controller.py ├── gait.py ├── swing_foot_trajectory_generator.py └── mpc.py └── scripts ├── mujoco_aliengo.py └── isaacgym_a1.py /config/cmd.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # vscode 2 | /.vscode 3 | 4 | # python 5 | *__pycache__ 6 | 7 | # virtual environment 8 | /pympc-env 9 | -------------------------------------------------------------------------------- /robot/a1/meshes/trunk_A1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yinghansun/pympc-quadruped/HEAD/robot/a1/meshes/trunk_A1.png -------------------------------------------------------------------------------- /robot/aliengo/meshes/hip.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yinghansun/pympc-quadruped/HEAD/robot/aliengo/meshes/hip.stl -------------------------------------------------------------------------------- /robot/aliengo/meshes/calf.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yinghansun/pympc-quadruped/HEAD/robot/aliengo/meshes/calf.stl -------------------------------------------------------------------------------- /robot/aliengo/meshes/thigh.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yinghansun/pympc-quadruped/HEAD/robot/aliengo/meshes/thigh.stl -------------------------------------------------------------------------------- /robot/aliengo/meshes/trunk.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yinghansun/pympc-quadruped/HEAD/robot/aliengo/meshes/trunk.stl -------------------------------------------------------------------------------- /doc/results/trotting10_mujoco.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yinghansun/pympc-quadruped/HEAD/doc/results/trotting10_mujoco.gif -------------------------------------------------------------------------------- /robot/aliengo/meshes/thigh_mirror.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yinghansun/pympc-quadruped/HEAD/robot/aliengo/meshes/thigh_mirror.stl -------------------------------------------------------------------------------- /utils/dynamics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def make_com_inertial_matrix( 4 | ixx: float, ixy: float, ixz: float, iyy: float, iyz: float, izz: float 5 | ) -> np.ndarray: 6 | '''Compute the inertia matrix expressed in the link's CoM frame 7 | (from URDF). 8 | 9 | Args 10 | ---- 11 | six numbers (ixx, ixy, ixz, iyy, iyz, izz) in the URDF file corresponding 12 | to the inertia matrix. 13 | ''' 14 | return np.array([ 15 | [ixx, ixy, ixz], 16 | [ixy, iyy, iyz], 17 | [ixz, iyz, izz] 18 | ], dtype=np.float32) -------------------------------------------------------------------------------- /config/linear_mpc_configs.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | import numpy as np 3 | 4 | class LinearMpcConfig: 5 | 6 | dt_control: float = 0.001 7 | iteration_between_mpc: int = 20 8 | # dt_mpc: float = dt_control * iteration_between_mpc 9 | dt_mpc: float = 0.05 10 | 11 | horizon: int = 16 12 | 13 | gravity: np.float32 = 9.81 14 | 15 | friction_coef: float = 0.7 16 | 17 | # quadratic programming weights 18 | # r, p, y, x, y, z, wx, wy, wz, vx, vy, vz, g 19 | Q: np.ndarray = np.diag([5., 5., 10., 10., 10., 50., 0.01, 0.01, 0.2, 0.2, 0.2, 0.2, 0.]) 20 | R: np.ndarray = np.diag([1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5]) 21 | 22 | cmd_xvel: float = 0. 23 | cmd_yvel: float = 0. 24 | cmd_yaw_turn_rate: float = 0. -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Yinghan Sun 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /config/robot_configs.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | sys.path.append(os.path.join(os.path.dirname(__file__), '../utils')) 4 | 5 | import numpy as np 6 | 7 | from dynamics import make_com_inertial_matrix 8 | 9 | class RobotConfig: 10 | mass_base: float 11 | base_height_des: float 12 | base_inertia_base: np.ndarray 13 | 14 | fz_max: float 15 | 16 | swing_height: float 17 | Kp_swing: np.ndarray 18 | Kd_swing: np.ndarray 19 | 20 | 21 | class AliengoConfig(RobotConfig): 22 | mass_base: float = 9.042 23 | base_height_des: float = 0.38 24 | base_inertia_base = make_com_inertial_matrix( 25 | ixx=0.033260231, 26 | ixy=-0.000451628, 27 | ixz=0.000487603, 28 | iyy=0.16117211, 29 | iyz=4.8356e-05, 30 | izz=0.17460442 31 | ) 32 | 33 | fz_max = 500. 34 | 35 | swing_height = 0.1 36 | Kp_swing = np.diag([200., 200., 200.]) 37 | Kd_swing = np.diag([20., 20., 20.]) 38 | 39 | 40 | class A1Config(RobotConfig): 41 | mass_base: float = 4.713 42 | base_height_des: float = 0.42 43 | base_inertia_base = make_com_inertial_matrix( 44 | ixx=0.01683993, 45 | ixy=8.3902e-05, 46 | ixz=0.000597679, 47 | iyy=0.056579028, 48 | iyz=2.5134e-05, 49 | izz=0.064713601 50 | ) * 10 51 | 52 | fz_max = 500. 53 | 54 | swing_height = 0.1 55 | Kp_swing = np.diag([700., 700., 700.]) 56 | Kd_swing = np.diag([20., 20., 20.]) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pympc-quadruped 2 | A Python implementation about quadruped locomotion using convex model predictive control (MPC). 3 | 4 | ![image](https://github.com/yinghansun/pympc-quadruped/blob/main/doc/results/trotting10_mujoco.gif) 5 | 6 | ## Installation 7 | 8 | ### 1. Create a Virtual Environment 9 | #### a. Install virtualenv: 10 | ~~~ 11 | $ sudo apt install python3-virtualenv 12 | ~~~ 13 | or 14 | ~~~ 15 | $ pip install virtualenv 16 | ~~~ 17 | 18 | #### b. Create a virtual environment: 19 | ~~~ 20 | $ cd ${path-to-pympc-quadruped} 21 | $ virtualenv --python /usr/bin/python3.8 pympc-env 22 | ~~~ 23 | 24 | ### 2. Install Simulators 25 | #### a. Mujoco 26 | - Create a folder named `.mujoco` in your home directory: `$ mkdir ~/.mujoco`. 27 | - Download Mujoco library from https://mujoco.org/download/mujoco210-linux-x86_64.tar.gz. Extract and move it to the `.mujoco` folder. 28 | - Add the following to your `.bashrc` file: 29 | ~~~ 30 | export LD_LIBRARY_PATH=/home/${your-usr-name}/.mujoco/mujoco210/bin 31 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/nvidia 32 | export PATH="$LD_LIBRARY_PATH:$PATH" 33 | export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libGLEW.so 34 | ~~~ 35 | Do not forget to run `$ source ~/.bashrc`. 36 | - Test if the installation is successfully finished: 37 | ~~~ 38 | $ cd ~/.mujoco/mujoco210/bin 39 | $ ./simulate ../model/humanoid.xml 40 | ~~~ 41 | 42 | #### b. Isaac Gym 43 | - Download Isaac Gym Preview Release from this [website](https://developer.nvidia.com/isaac-gym). 44 | - The tutorial for installation is in the `./isaacgym/docs/install.html`. **I recommand the user to install it in the previous virtual environment**. 45 | ~~~ 46 | $ cd ${path-to-issacgym}/python 47 | $ source ${path-to-pympc-quadruped}/pympc-env/bin/activate 48 | (pympc-env)$ pip install -e . 49 | ~~~ 50 | - Then you can trying to run examples in `./isaacgym/python/examples`. Note that if you follow the instructions above, you need to run the examples in the virtual environments. 51 | ~~~ 52 | $ cd ${path-to-isaacgym}/python/examples 53 | $ source ${path-to-pympc-quadruped}/pympc-env/bin/activate 54 | (pympc-env)$ python 1080_balls_of_solitude.py 55 | ~~~ 56 | - For troubleshooting, check `./isaacgym/docs/index.html` 57 | 58 | **Note**: If you meet the following issue like 59 | `ImportError: libpython3.7m.so.1.0: cannot open shared object file: No such file or directory` when running the examples, you can try 60 | `export LD_LIBRARY_PATH=/path/to/conda/envs/your_env/lib` before executing your python script. If you are not using conda, the path shoud be `/path/to/libpython/directory`. 61 | 62 | ### 3. Install Dependences 63 | 64 | **a) Install Pinocchio** 65 | 66 | Pinocchio provides the state-of-the-art rigid body kinematics and dynamic algorithms. You could follow [this link](https://stack-of-tasks.github.io/pinocchio/download.html) to install Pinocchio. 67 | 68 | **b) Install other dependences** 69 | 70 | ~~~ 71 | $ cd ${path-to-pympc-quadruped} 72 | $ source ${path-to-pympc-quadruped}/pympc-env/bin/activate 73 | (pympc-env)$ pip install --upgrade pip 74 | (pympc-env)$ pip install -r requirements.txt 75 | ~~~ 76 | 77 | ## Sign Convention -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | absl-py==1.4.0 2 | actionlib==1.14.0 3 | angles==1.9.13 4 | base_local_planner==1.17.3 5 | bondpy==1.8.6 6 | camera-calibration==1.17.0 7 | camera-calibration-parsers==1.12.0 8 | catkin==0.8.10 9 | certifi==2022.12.7 10 | cffi==1.15.1 11 | chardet==5.1.0 12 | charset-normalizer==3.1.0 13 | contourpy==1.0.7 14 | controller-manager==0.19.6 15 | controller-manager-msgs==0.19.6 16 | cv-bridge==1.16.2 17 | cycler==0.11.0 18 | Cython==0.29.34 19 | daqp==0.5.1 20 | diagnostic-analysis==1.11.0 21 | diagnostic-common-diagnostics==1.11.0 22 | diagnostic-updater==1.11.0 23 | drake==1.15.0 24 | dynamic-reconfigure==1.7.3 25 | ecos==2.0.12 26 | fasteners==0.18 27 | fonttools==4.39.3 28 | gazebo_plugins==2.9.2 29 | gazebo_ros==2.9.2 30 | gencpp==0.7.0 31 | geneus==3.0.0 32 | genlisp==0.4.18 33 | genmsg==0.6.0 34 | gennodejs==2.0.2 35 | genpy==0.6.15 36 | glfw==2.5.9 37 | idna==3.4 38 | image-geometry==1.16.2 39 | imageio==2.28.0 40 | importlib-resources==5.12.0 41 | interactive-markers==1.12.0 42 | joint-state-publisher==1.15.1 43 | joint-state-publisher-gui==1.15.1 44 | kiwisolver==1.4.4 45 | laser_geometry==1.6.7 46 | matplotlib==3.7.1 47 | message-filters==1.16.0 48 | mujoco==2.3.4 49 | mujoco-py==2.1.2.14 50 | numpy==1.24.3 51 | osqp==0.6.2.post9 52 | packaging==23.1 53 | Pillow==9.5.0 54 | pkg_resources==0.0.0 55 | pycparser==2.21 56 | pydot==1.4.2 57 | PyOpenGL==3.1.6 58 | pyparsing==3.0.9 59 | python-dateutil==2.8.2 60 | python-qt-binding==0.4.4 61 | PyYAML==6.0 62 | qdldl==0.1.7 63 | qpsolvers==3.3.1 64 | qt-dotgraph==0.4.2 65 | qt-gui==0.4.2 66 | qt-gui-cpp==0.4.2 67 | qt-gui-py-common==0.4.2 68 | requests==2.28.2 69 | resource_retriever==1.12.7 70 | rosbag==1.16.0 71 | rosboost-cfg==1.15.8 72 | rosclean==1.15.8 73 | roscreate==1.15.8 74 | rosgraph==1.16.0 75 | roslaunch==1.16.0 76 | roslib==1.15.8 77 | roslint==0.12.0 78 | roslz4==1.16.0 79 | rosmake==1.15.8 80 | rosmaster==1.16.0 81 | rosmsg==1.16.0 82 | rosnode==1.16.0 83 | rosparam==1.16.0 84 | rospy==1.16.0 85 | rosserial_python==0.9.2 86 | rosservice==1.16.0 87 | rostest==1.16.0 88 | rostopic==1.16.0 89 | rosunit==1.15.8 90 | roswtf==1.16.0 91 | rqt-moveit==0.5.10 92 | rqt-reconfigure==0.5.5 93 | rqt-robot-dashboard==0.5.8 94 | rqt-robot-monitor==0.5.14 95 | rqt-rviz==0.7.0 96 | rqt_action==0.4.9 97 | rqt_bag==0.5.1 98 | rqt_bag_plugins==0.5.1 99 | rqt_console==0.4.11 100 | rqt_dep==0.4.12 101 | rqt_graph==0.4.14 102 | rqt_gui==0.5.3 103 | rqt_gui_py==0.5.3 104 | rqt_image_view==0.4.16 105 | rqt_launch==0.4.9 106 | rqt_logger_level==0.4.11 107 | rqt_msg==0.4.10 108 | rqt_nav_view==0.5.7 109 | rqt_plot==0.4.13 110 | rqt_pose_view==0.5.11 111 | rqt_publisher==0.4.10 112 | rqt_py_common==0.5.3 113 | rqt_py_console==0.4.10 114 | rqt_robot_steering==0.5.12 115 | rqt_runtime_monitor==0.5.9 116 | rqt_service_caller==0.4.10 117 | rqt_shell==0.4.11 118 | rqt_srv==0.4.9 119 | rqt_tf_tree==0.6.3 120 | rqt_top==0.4.10 121 | rqt_topic==0.4.13 122 | rqt_web==0.4.10 123 | rviz==1.14.19 124 | scipy==1.10.1 125 | scs==3.2.3 126 | sensor-msgs==1.13.1 127 | six==1.16.0 128 | smach==2.5.1 129 | smach-ros==2.5.1 130 | smclib==1.8.6 131 | tf==1.13.2 132 | tf-conversions==1.13.2 133 | tf2-geometry-msgs==0.7.6 134 | tf2-kdl==0.7.6 135 | tf2-py==0.7.6 136 | tf2-ros==0.7.6 137 | topic-tools==1.16.0 138 | turtlebot3_autorace_camera==1.1.1 139 | turtlebot3_autorace_core==1.1.1 140 | turtlebot3_autorace_detect==1.1.1 141 | turtlebot3_autorace_driving==1.1.1 142 | turtlebot3_example==1.2.5 143 | turtlebot3_teleop==1.2.5 144 | urllib3==1.26.15 145 | xacro==1.14.14 146 | zipp==3.15.0 147 | -------------------------------------------------------------------------------- /linear_mpc/leg_controller.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from typing import List 4 | sys.path.append(os.path.join(os.path.dirname(__file__), '../utils')) 5 | 6 | import numpy as np 7 | 8 | from robot_data import RobotData 9 | 10 | class LegController(): 11 | '''Compute torques for each joint. 12 | 13 | For stance legs, joint torques are computed with $ tau_i = Jv.T @ -f_i $, 14 | where Jv (3 x 3 matrix) is the foot Jacobian, and f_i is the contact force 15 | related to foot i, calculated by the predictive controller. All the 16 | quantities are expressed in world frame. 17 | 18 | For swing legs, joint torques are computed by a PD controller (by 19 | assuming the mass of the leg is not too large) to track the given swing 20 | foot targets. All the quantities are expressed in world frame. 21 | 22 | Params 23 | ------ 24 | Kp_swing: np.ndarray 25 | The P gain for swing leg PD control. 26 | Kd_swing: np.ndarray 27 | The D gain for swing leg PD control. 28 | ''' 29 | def __init__(self, Kp_swing: np.ndarray, Kd_swing: np.ndarray): 30 | self.__Kp_swing = Kp_swing 31 | self.__Kd_swing = Kd_swing 32 | self.__torque_cmds = np.zeros(12, dtype=np.float32) 33 | 34 | @property 35 | def torque_cmds(self) -> np.ndarray: 36 | return self.__torque_cmds 37 | 38 | def update( 39 | self, 40 | robot_data: RobotData, 41 | contact_forces: np.ndarray, 42 | swing_states: List[int], 43 | pos_targets_swingfeet: np.ndarray, 44 | vel_targets_swingfeet: np.ndarray 45 | ): 46 | '''Update joint torques using current data. 47 | 48 | Args 49 | ---- 50 | robot_data: RobotData 51 | records current robot data. You need to update the current robot 52 | data before computing the joint torques. 53 | contact_forces: np.ndarray, shape = (12, ) 54 | contact forces of each foot expressed in world frame, computed 55 | by the predictive controller. 56 | swing_states: List[int] 57 | identify whether each leg is in swing (=1) or stance (=0). 58 | pos_targets_swingfeet: np.ndarray, shape = (4, 3) 59 | target position of each swing foot relative to base, expressed in 60 | base frame. 61 | vel_targets_swingfeet: np.ndarray, shape = (4, 3) 62 | target velocity of each swing foot relative to base, expressed in 63 | base frame. 64 | 65 | Returns 66 | ------- 67 | torque_cmds: np.ndarray, shape = (12, ) 68 | torque commands of each joint. 69 | ''' 70 | Jv_feet = robot_data.Jv_feet 71 | R_base = robot_data.R_base 72 | base_vel_base_feet = robot_data.base_vel_base_feet 73 | base_pos_base_feet = robot_data.base_pos_base_feet 74 | 75 | for leg_idx in range(4): 76 | Jvi = Jv_feet[leg_idx] 77 | 78 | if swing_states[leg_idx]: 79 | base_pos_swingfoot_des = pos_targets_swingfeet[leg_idx, :] 80 | base_vel_swingfoot_des = vel_targets_swingfeet[leg_idx, :] 81 | 82 | swing_err = self.__Kp_swing @ (R_base @ base_pos_swingfoot_des - R_base @ base_pos_base_feet[leg_idx]) \ 83 | + self.__Kd_swing @ (R_base @ base_vel_swingfoot_des - R_base @ base_vel_base_feet[leg_idx]) 84 | tau_i = Jvi.T @ swing_err 85 | cmd_i = tau_i[6+3*leg_idx : 6+3*(leg_idx+1)] 86 | self.__torque_cmds[3*leg_idx:3*(leg_idx+1)] = cmd_i 87 | else: 88 | tau_i = Jvi.T @ -contact_forces[3*leg_idx:3*(leg_idx+1)] 89 | cmd_i = tau_i[6+3*leg_idx:6+3*(leg_idx+1)] 90 | self.__torque_cmds[3*leg_idx:3*(leg_idx+1)] = cmd_i 91 | 92 | return self.__torque_cmds -------------------------------------------------------------------------------- /doc/state_estimation_kf.md: -------------------------------------------------------------------------------- 1 | # State Estimation - Linear Kalman Filter Approach 2 | 3 | **Reference**: 4 | 5 | [1] Bledt G, Powell M J, Katz B, et al. Mit cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 2245-2252. [[PDF]](https://dspace.mit.edu/bitstream/handle/1721.1/126619/IROS.pdf?sequence=2&isAllowed=y) 6 | 7 | [2] Bloesch M, Hutter M, Hoepflinger M A, et al. State estimation for legged robots-consistent fusion of leg kinematics and IMU[J]. Robotics, 2013, 17: 17-24. [[PDF]](https://infoscience.epfl.ch/record/181040/files/Bloesch-Siegwart_State__IMU_2012.pdf) 8 | 9 | [3] Mahony R, Hamel T, Pflimlin J M. Nonlinear complementary filters on the special orthogonal group[J]. IEEE Transactions on automatic control, 2008, 53(5): 1203-1218. [[PDF]](https://hal.archives-ouvertes.fr/hal-00488376/file/2007_Mahony.etal_TAC-06-396_v3.pdf) 10 | 11 | ## 1. Introduction 12 | 13 | Cheetah 3 estimates its body states through a two-stage sensor fusion algorithm that *decouples estimation of body orientation from estimation of the body position and velocity* [1]. 14 | 15 | ## 2. 1st Stage: Orientation Filter 16 | 17 | The first stage of the state estimation employs an orientation filter using both the IMU gyro and accelerometer readings. The main idea of the filter is that the gyro provides an accurate reading of the high-frequency orientation dynamics, whereas the presence of a gravity bias on the accelerometer allows it to de-drift the estimate at a comparatively lower frequency [3]. 18 | 19 | The filter updates the estimation of orientation according to 20 | 21 | $$ 22 | \hat{R}_{b,k+1}^o = \hat{R}_{b,k}^o [\omega_{b,k}^b + \kappa \omega_{\text{corr},k}]^{\times} 23 | $$ 24 | 25 | where $\kappa > 0$ is a correction gain and $\omega_{\text{corr}}$ is a correction angular velocity to align the accelerometer reading $a_b$ with its gravity bias 26 | 27 | $$ 28 | \omega_{\text{corr},k} = \frac{a_{b,k}^b}{||a_{b,k}^b||}\times (\hat{R}_{b,k}^o)^\text{T}\mathbf{e}_z 29 | $$ 30 | 31 | The time constant of the de-drifting from this term can be approximated by $\kappa^{-1}$. In practice $\kappa$ is heuristically decreased during highly-dynamic portions of the gait where $||a_b|| \gg g$ with 32 | 33 | $$ 34 | \kappa = \kappa_{\text{ref}} \max \left(\min \left( 1, 1-||a_b - g|| / g \right), 0\right) 35 | $$ 36 | 37 | where $g$ is the acceleration of gravity and $\kappa_{\text{ref}}$ is chosen as $\kappa_{\text{ref}} = 0.1$. This process is effective to de-drift pitch and roll, however, error accumulation on yaw is unavoidable without fusion using exteroceptive information such as vision. 38 | 39 | ## 3. 2nd Stage: Kalman Filter 40 | 41 | The second stage of the state estimation uses the orientation estimate $\hat{R}_b^o$ along with kinematic measurements from the legs to estimate the base position and velocity. In contrast to previous state estimation techniques that posed this problem as an Extended Kalman Filter (EKF) [2], the two-stage approach allows this secondary fusion to be posed as a conventional Kalman Filter. This simplifies analysis and tuning of the filter and guarantees that the filter equations will never diverge in finite time. 42 | 43 | ### a. Process Model 44 | In this stage, the state we choose is $x_k = [p_{b,k}^o, v_{b,k}^o, p_{i,k}]^\text{T} \in \mathbb{R}^{18}$, where $p_{b,k}^o$ is the position of the body in world frame at step $k$, $v_{b,k}^o$ is the velocity of the body in world frame at step $k$, and $p_{i,k} \in \mathbb{R}^3$ is the position of foot $i$ at step $k$. 45 | 46 | In discrete time, the process equations is modeled as 47 | 48 | $$ 49 | \begin{aligned} 50 | &p_{b,k}^o = p_{b,k-1}^o + v_{b,k-1}^o \Delta t + \frac{1}{2}\left(\hat{R}_{b,k}^oa_{b,k}^b + a_g^o\right) \Delta t^2 \\ 51 | &v_{b,k}^o = v_{b,k-1}^o + \left(\hat{R}_{b,k}^oa_{b,k}^b + a_g^o\right) \Delta t \\ 52 | &p_{i,k} = p_{i,k-1}, \quad \forall i = \{1, 2, 3, 4\} 53 | \end{aligned} 54 | $$ 55 | 56 | where $a_g = [0, 0, -g]^\text{T}$ is the gravitational acceleration, $a_{b,k-1}^o = \hat{R}_{b,k}^oa_{b,k}^b + a_g^o$ is the acceleration of the body in world frame. 57 | In matrix form, we can write 58 | 59 | $$ 60 | \begin{bmatrix} 61 | p_{b,k}^o \\ v_{b,k}^o \\ p_{i,k} 62 | \end{bmatrix} = 63 | \begin{bmatrix} 64 | \mathbf{I}_3 & \mathbf{I}_3 \Delta t & \mathbf{0}_3 \\ 65 | \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\ 66 | \mathbf{0}_{12} & \mathbf{0}_{12} & \mathbf{I}_{12} 67 | \end{bmatrix} 68 | \begin{bmatrix} 69 | p_{b,k-1}^o \\ v_{b,k-1}^o \\ p_{i,k-1} 70 | \end{bmatrix} 71 | + \begin{bmatrix} 72 | \frac{1}{2}\mathbf{I}_3\Delta t^2 \\ \mathbf{I}_3 \Delta t \\ \mathbf{0}_{12\times3} 73 | \end{bmatrix} 74 | a_{b,k-1}^o 75 | $$ 76 | 77 | This equation leads to the $x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}$ in Kalman Filter, where $w_{k} \sim (0, Q_k)$ is the process noise term. In simulation, we will get the true robot state if we set $Q_k$ as zero matrix. This is different in real experiment. 78 | 79 | ### b. Measurement Model 80 | **Leg kinematics** provide measurements of the relative position vector between each foot and the body to dedrift estimates $\hat{p}_b^o$, $\hat{v}_b^o$ and $\hat{p}_i^o$ for each foot. 81 | 82 | Letting $p_{\text{rel}}^o(q_i, \hat{R}_{b}^o)$ denote the relative foot position as computed by kinematics, a measurement residual is generated 83 | 84 | $$ 85 | e_{p,i} = (\hat{p}_i^o - \hat{p}_b^o) - p_{\text{rel}}^o(q_i, \hat{R}_{b}^o) 86 | $$ 87 | 88 | Similarly, the velocity of the foot relative to the body can be computed from the leg angles, velocities, and the body orientation and angular velocity. This computation is denoted as $\dot{p}_{\text{rel}}^o(q_i, \dot{q}_i, \hat{R}_{b}^o, \omega_b^b)$. *Under the assumption that each foot is fixed (this means that $\hat{v}_i^o = 0$ for $\forall i = 1,2,3,4$)*, this provides an associated measurement residual 89 | 90 | $$ 91 | \begin{aligned} 92 | e_{v,i} &= (\hat{v}_i^o-\hat{v}_b^o) - \dot{p}_{\text{rel}}^o(q_i, \dot{q}_i, \hat{R}_{b}^o, \omega_b^b) \\ 93 | &= (-\hat{v}_b^o) - \dot{p}_{\text{rel}}^o(q_i, \dot{q}_i, \hat{R}_{b}^o, \omega_b^b) 94 | \end{aligned} 95 | $$ 96 | 97 | Finally, a contact height $h_i$ is assumed for each foot with associated measurement residual: 98 | 99 | $$ 100 | e_{h_i} = (\begin{bmatrix} 101 | 0 & 0 & 1 102 | \end{bmatrix}\hat{p}_i^o) - h_i 103 | $$ 104 | 105 | Using the measurement residual equations described above, we can build our measurement model, which is 106 | 107 | $$ 108 | \begin{aligned} 109 | &p_{\text{rel},k}^o(q_{i,k}, \hat{R}_{b,k}^o) = (\hat{p}_{i,k}^o - \hat{p}_{b,k}^o) \\ 110 | &\dot{p}_{\text{rel},k}^o(q_{i,k}, \dot{q}_{i,k}, \hat{R}_{b,k}^o, \omega_{b,k}^b) = \hat{v}_{b,k}^o \\ 111 | &h_{i,k} = (\begin{bmatrix} 112 | 0 & 0 & 1 113 | \end{bmatrix}\hat{p}_{i,k}^o) 114 | \end{aligned} 115 | $$ -------------------------------------------------------------------------------- /linear_mpc/gait.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | sys.path.append(os.path.join(os.path.dirname(__file__), '../config')) 4 | 5 | from enum import Enum 6 | import numpy as np 7 | 8 | from linear_mpc_configs import LinearMpcConfig 9 | 10 | class Gait(Enum): 11 | ''' 12 | name: name of the gait 13 | num_segment: 14 | ''' 15 | 16 | STANDING = 'standing', 16, np.array([0, 0, 0, 0]), np.array([16, 16, 16, 16]) 17 | TROTTING16 = 'trotting', 16, np.array([0, 8, 8, 0]), np.array([8, 8, 8, 8]) 18 | TROTTING10 = 'trotting', 10, np.array([0, 5, 5, 0]), np.array([5, 5, 5, 5]) 19 | JUMPING16 = 'jumping', 16, np.array([0, 0, 0, 0]), np.array([4, 4, 4, 4]) 20 | # BOUNDING8 = 'bounding', 8, np.array([4, 4, 0, 0]), np.array([4, 4, 4, 4]) 21 | PACING16 = 'pacing', 16, np.array([8, 0, 8, 0]), np.array([8, 8, 8, 8]) 22 | PACING10 = 'pacing', 10, np.array([5, 0, 5, 0]), np.array([5, 5, 5, 5]) 23 | 24 | def __init__( 25 | self, 26 | name: str, 27 | num_segment: int, 28 | stance_offsets: np.ndarray, 29 | stance_durations: np.ndarray 30 | ) -> None: 31 | 32 | self.__name = name 33 | self.__num_segment = num_segment 34 | self.__stance_offsets = stance_offsets 35 | self.__stance_durations = stance_durations 36 | 37 | self.__load_parameters() 38 | 39 | # each leg has the same swing time and stance time in one period 40 | self.total_swing_time: int = num_segment - stance_durations[0] 41 | self.total_stance_time: int = stance_durations[0] 42 | 43 | # normalization 44 | self.stance_offsets_normalized = stance_offsets / num_segment 45 | self.stance_durations_normalized = stance_durations / num_segment 46 | 47 | def __load_parameters(self) -> None: 48 | self.__dt_control: float = LinearMpcConfig.dt_control 49 | self.__iterations_between_mpc: int = LinearMpcConfig.iteration_between_mpc 50 | self.__mpc_horizon: int = LinearMpcConfig.horizon 51 | 52 | @property 53 | def name(self) -> str: 54 | return self.__name 55 | 56 | @property 57 | def num_segment(self) -> int: 58 | return self.__num_segment 59 | 60 | @property 61 | def stance_offsets(self) -> np.ndarray: 62 | return self.__stance_offsets 63 | 64 | @property 65 | def stance_durations(self) -> np.ndarray: 66 | return self.__stance_durations 67 | 68 | @property 69 | def swing_time(self) -> float: 70 | return self.get_total_swing_time(self.__dt_control * self.__iterations_between_mpc) 71 | 72 | @property 73 | def stance_time(self) -> float: 74 | return self.get_total_stance_time(self.__dt_control * self.__iterations_between_mpc) 75 | 76 | def set_iteration(self, iterations_between_mpc: int, cur_iteration: int) -> None: 77 | self.iteration = np.floor(cur_iteration / iterations_between_mpc) % self.num_segment 78 | self.phase = (cur_iteration % (iterations_between_mpc * self.num_segment)) \ 79 | / (iterations_between_mpc * self.num_segment) 80 | 81 | def get_gait_table(self) -> np.ndarray: 82 | ''' 83 | compute gait table for force constraints in mpc 84 | 85 | 1 for stance, 0 for swing 86 | ''' 87 | gait_table = np.zeros(4 * self.__mpc_horizon, dtype=np.float32) 88 | for i in range(self.__mpc_horizon): 89 | i_horizon = (i + 1 + self.iteration) % self.num_segment 90 | cur_segment = i_horizon - self.stance_offsets 91 | for j in range(4): 92 | if cur_segment[j] < 0: 93 | cur_segment[j] += self.num_segment 94 | 95 | if cur_segment[j] < self.stance_durations[j]: 96 | gait_table[i*4+j] = 1 97 | else: 98 | gait_table[i*4+j] = 0 99 | 100 | return gait_table 101 | 102 | def get_swing_state(self) -> np.ndarray: 103 | swing_offsets_normalizerd = self.stance_offsets_normalized + self.stance_durations_normalized 104 | for i in range(4): 105 | if(swing_offsets_normalizerd[i] > 1): 106 | swing_offsets_normalizerd -= 1 107 | swing_durations_normalized = 1 - self.stance_durations_normalized 108 | 109 | phase_state = np.array([self.phase, self.phase, self.phase, self.phase], dtype=np.float32) 110 | swing_state = phase_state - swing_offsets_normalizerd 111 | 112 | for i in range(4): 113 | if swing_state[i] < 0: 114 | swing_state[i] += 1 115 | 116 | if swing_state[i] > swing_durations_normalized[i]: 117 | swing_state[i] = 0 118 | else: 119 | swing_state[i] = swing_state[i] / swing_durations_normalized[i] 120 | 121 | return swing_state 122 | 123 | def get_stance_state(self) -> np.ndarray: 124 | phase_state = np.array([self.phase, self.phase, self.phase, self.phase], dtype=np.float32) 125 | stance_state = phase_state - self.stance_offsets_normalized 126 | for i in range(4): 127 | if stance_state[i] < 0: 128 | stance_state[i] += 1 129 | 130 | if stance_state[i] > self.stance_durations_normalized[i]: 131 | stance_state[i] = 0 132 | else: 133 | stance_state[i] = stance_state[i] / self.stance_durations_normalized[i] 134 | 135 | return stance_state 136 | 137 | def get_total_swing_time(self, dt_mpc: float) -> float: 138 | ''' 139 | compute total swing time 140 | 141 | dt_mpc: dt between mpc (time between mpc) 142 | i.e. dt_mpc = dt_control * iterations_between_mpc. 143 | ''' 144 | return dt_mpc * self.total_swing_time 145 | 146 | def get_total_stance_time(self, dt_mpc: float) -> float: 147 | ''' 148 | compute total stance time 149 | ''' 150 | return dt_mpc * self.total_stance_time 151 | 152 | 153 | def main(): 154 | standing = Gait.STANDING 155 | trotting = Gait.TROTTING10 156 | 157 | iteration_between_mpc = 20 158 | idx = 0 159 | 160 | print(trotting.swing_time) 161 | 162 | for i in range(1000): 163 | if i % iteration_between_mpc == 0: 164 | print('--------update MPC--------') 165 | print('idx =', idx) 166 | idx += 1 167 | trotting.set_iteration(iteration_between_mpc, i) 168 | print(trotting.get_gait_table()) 169 | 170 | # print(trotting.get_cur_swing_time(30 * 0.001)) 171 | # print(trotting.swing_time) 172 | 173 | # stance_state = trotting.get_stance_state() 174 | # swing_state = trotting.get_swing_state() 175 | # print('iteration: ', trotting.iteration) 176 | 177 | # print('phase: ', trotting.phase) 178 | # print('stance state: ', stance_state) 179 | # print('swing state: ', swing_state) 180 | 181 | 182 | 183 | if __name__ == '__main__': 184 | main() 185 | 186 | -------------------------------------------------------------------------------- /linear_mpc/swing_foot_trajectory_generator.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | sys.path.append(os.path.join(os.path.dirname(__file__), '../config')) 4 | sys.path.append(os.path.join(os.path.dirname(__file__), '../utils')) 5 | 6 | import matplotlib.pyplot as plt 7 | import numpy as np 8 | from pydrake.all import PiecewisePolynomial 9 | from gait import Gait 10 | from robot_data import RobotData 11 | 12 | from linear_mpc_configs import LinearMpcConfig 13 | from robot_configs import AliengoConfig 14 | 15 | class SwingFootTrajectoryGenerator(): 16 | 17 | def __init__(self, leg_id): 18 | self.__load_parameters() 19 | 20 | self.__is_first_swing = True 21 | self.__remaining_swing_time = 0. 22 | self.__leg_id = leg_id 23 | 24 | self.__footpos_init = np.zeros((3, 1), dtype=np.float32) 25 | self.__footpos_final = np.zeros((3, 1), dtype=np.float32) 26 | 27 | def __load_parameters(self): 28 | self.__dt_control = LinearMpcConfig.dt_control 29 | self.__swing_height = AliengoConfig.swing_height 30 | self.__gravity = LinearMpcConfig.gravity 31 | 32 | def __set_init_foot_position(self, footpos_init): 33 | self.__footpos_init = footpos_init 34 | 35 | def __set_final_foot_position(self, footpos_final): 36 | self.__footpos_final = footpos_final 37 | 38 | def generate_swing_foot_trajectory(self, total_swing_time, cur_swing_time): 39 | break_points = np.array([[0.], 40 | [total_swing_time / 2.0], 41 | [total_swing_time]], dtype=np.float32) 42 | 43 | footpos_middle_time = (self.__footpos_init + self.__footpos_final) / 2 44 | footpos_middle_time[2] = self.__swing_height 45 | 46 | # print(footpos_middle_time) 47 | footpos_break_points = np.hstack(( 48 | self.__footpos_init.reshape(3, 1), 49 | footpos_middle_time.reshape(3, 1), 50 | self.__footpos_final.reshape(3, 1) 51 | )) 52 | 53 | vel_break_points = np.zeros((3, 3), dtype=np.float32) 54 | 55 | swing_traj = PiecewisePolynomial.CubicHermite(break_points, footpos_break_points, vel_break_points) 56 | 57 | # print(cur_swing_time) 58 | pos_swingfoot = swing_traj.value(cur_swing_time) 59 | vel_swingfoot = swing_traj.derivative(1).value(cur_swing_time) 60 | # acc_swingfoot = swing_traj.derivate(2).value(cur_swing_time) 61 | 62 | # x = np.linspace(start=0., stop=total_swing_time, num=100) 63 | # y = [swing_traj.value(xi)[0] for xi in x] 64 | # plt.plot(x, y) 65 | # plt.show() 66 | 67 | return np.squeeze(pos_swingfoot), np.squeeze(vel_swingfoot) 68 | 69 | def compute_traj_swingfoot(self, robot_data: RobotData, gait: Gait): 70 | pos_base = np.array(robot_data.pos_base, dtype=np.float32) 71 | vel_base = np.array(robot_data.lin_vel_base, dtype=np.float32) 72 | R_base = robot_data.R_base 73 | 74 | total_swing_time = gait.swing_time 75 | cur_swing_time = total_swing_time - self.__remaining_swing_time 76 | pos_swingfoot_des, vel_swingfoot_des = self.generate_swing_foot_trajectory(total_swing_time, cur_swing_time) 77 | 78 | base_R_world = R_base.T 79 | base_pos_swingfoot_des = base_R_world @ (pos_swingfoot_des - pos_base) 80 | base_vel_swingfoot_des = base_R_world @ (vel_swingfoot_des - vel_base) 81 | 82 | return base_pos_swingfoot_des, base_vel_swingfoot_des 83 | 84 | def set_foot_placement( 85 | self, 86 | robot_data: RobotData, 87 | gait: Gait, 88 | base_vel_base_des, 89 | yaw_turn_rate_des 90 | ): 91 | '''Set foot initial and final placement during current swing. 92 | ''' 93 | pos_base = np.array(robot_data.pos_base, dtype=np.float32) 94 | vel_base = np.array(robot_data.lin_vel_base, dtype=np.float32) 95 | R_base = robot_data.R_base 96 | base_pos_base_thighi = robot_data.base_pos_base_thighs[self.__leg_id] 97 | 98 | total_stance_time = gait.stance_time 99 | total_swing_time = gait.swing_time 100 | swing_state = gait.get_swing_state()[self.__leg_id] 101 | 102 | vel_base_des = R_base @ base_vel_base_des 103 | 104 | # update the remaining swing time 105 | if self.__is_first_swing: 106 | self.__remaining_swing_time = total_swing_time 107 | else: 108 | self.__remaining_swing_time -= self.__dt_control 109 | 110 | # foot placement 111 | RotZ = self.__get_RotZ(yaw_turn_rate_des * 0.5 * total_stance_time) 112 | pos_thigh_corrected = RotZ @ base_pos_base_thighi 113 | 114 | world_footpos_final = pos_base + \ 115 | R_base @ (pos_thigh_corrected + base_vel_base_des * self.__remaining_swing_time) + \ 116 | 0.5 * total_stance_time * vel_base + 0.03 * (vel_base - vel_base_des) 117 | 118 | world_footpos_final[0] += (0.5 * pos_base[2] / self.__gravity) * (vel_base[1] * yaw_turn_rate_des) 119 | world_footpos_final[1] += (0.5 * pos_base[2] / self.__gravity) * (-vel_base[0] * yaw_turn_rate_des) 120 | world_footpos_final[2] = -0.0255 # TODO: what's this? 121 | # world_footpos_final[2] = 0.0 122 | self.__set_final_foot_position(world_footpos_final) 123 | 124 | if self.__is_first_swing: 125 | self.__is_first_swing = False 126 | self.__set_init_foot_position(robot_data.pos_feet[self.__leg_id]) 127 | 128 | if swing_state >= 1: # swing finished 129 | self.__is_first_swing = True 130 | 131 | def visualize_traj(self, x, y): 132 | plt.plot(x, y) 133 | plt.show() 134 | 135 | @staticmethod 136 | def __get_RotZ(theta): 137 | return np.array([[np.cos(theta), -np.sin(theta), 0.], 138 | [np.sin(theta), np.cos(theta), 0.], 139 | [0., 0., 1.]]) 140 | 141 | def test_swing_foot_traj(): 142 | robot_path = os.path.join(os.path.dirname(__file__), '../robot/aliengo/urdf/aliengo.urdf') 143 | 144 | robot_data = RobotData(robot_model=robot_path) 145 | robot_data.update( 146 | pos_base=[0.00727408, 0.00061764, 0.43571295], 147 | lin_vel_base=[0.0189759 , 0.00054278, 0.02322867], 148 | quat_base=[9.99951619e-01, -9.13191258e-03, 3.57360542e-03, 7.72221709e-04], 149 | ang_vel_base=[-0.06964452, -0.01762341, -0.00088601], 150 | q=[0.00687206, 0.52588717, -1.22975589, 151 | 0.02480081, 0.51914926, -1.21463939, 152 | 0.00892169, 0.51229961, -1.20195572, 153 | 0.02621839, 0.50635251, -1.18849609], 154 | qdot=[0.06341452, -0.02158136, 0.16191205, 155 | 0.07448259, -0.04855474, 0.21399941, 156 | 0.06280346, 0.00562435, 0.10597827, 157 | 0.07388069, -0.02180622, 0.15909948], 158 | ) 159 | 160 | gait = Gait.TROTTING10 161 | gait.set_iteration(30, 0) 162 | 163 | test = SwingFootTrajectoryGenerator(0) 164 | test.set_foot_placement(robot_data, gait, np.array([0.5, 0., 0.]), 0.) 165 | base_pos_swingfoot_des, base_vel_swingfoot_des = test.compute_traj_swingfoot(robot_data, gait) 166 | print(base_pos_swingfoot_des, base_vel_swingfoot_des) 167 | 168 | test1 = SwingFootTrajectoryGenerator(1) 169 | test1.set_foot_placement(robot_data, gait, np.array([0.5, 0., 0.]), 0.) 170 | base_pos_swingfoot_des, base_vel_swingfoot_des = test1.compute_traj_swingfoot(robot_data, gait) 171 | print(base_pos_swingfoot_des, base_vel_swingfoot_des) 172 | 173 | test2 = SwingFootTrajectoryGenerator(2) 174 | test2.set_foot_placement(robot_data, gait, np.array([0.5, 0., 0.]), 0.) 175 | base_pos_swingfoot_des, base_vel_swingfoot_des = test2.compute_traj_swingfoot(robot_data, gait) 176 | print(base_pos_swingfoot_des, base_vel_swingfoot_des) 177 | 178 | test3 = SwingFootTrajectoryGenerator(3) 179 | test3.set_foot_placement(robot_data, gait, np.array([0.5, 0., 0.]), 0.) 180 | base_pos_swingfoot_des, base_vel_swingfoot_des = test3.compute_traj_swingfoot(robot_data, gait) 181 | print(base_pos_swingfoot_des, base_vel_swingfoot_des) 182 | 183 | if __name__ == '__main__': 184 | test_swing_foot_traj() -------------------------------------------------------------------------------- /scripts/mujoco_aliengo.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | sys.path.append(os.path.join(os.path.dirname(__file__), '../config')) 4 | sys.path.append(os.path.join(os.path.dirname(__file__), '../linear_mpc')) 5 | sys.path.append(os.path.join(os.path.dirname(__file__), '../utils/')) 6 | 7 | import mujoco_py 8 | from mujoco_py import MjViewer 9 | import numpy as np 10 | 11 | from gait import Gait 12 | from leg_controller import LegController 13 | from linear_mpc_configs import LinearMpcConfig 14 | from mpc import ModelPredictiveController 15 | from robot_configs import AliengoConfig 16 | from robot_data import RobotData 17 | from swing_foot_trajectory_generator import SwingFootTrajectoryGenerator 18 | 19 | 20 | STATE_ESTIMATION = False 21 | 22 | def reset(sim, robot_config): 23 | sim.reset() 24 | # q_pos_init = np.array([ 25 | # 0, 0, 0.116536, 26 | # 1, 0, 0, 0, 27 | # 0, 1.16, -2.77, 28 | # 0, 1.16, -2.77, 29 | # 0, 1.16, -2.77, 30 | # 0, 1.16, -2.77 31 | # ]) 32 | q_pos_init = np.array([ 33 | 0, 0, robot_config.base_height_des, 34 | 1, 0, 0, 0, 35 | 0, 0.8, -1.6, 36 | 0, 0.8, -1.6, 37 | 0, 0.8, -1.6, 38 | 0, 0.8, -1.6 39 | ]) 40 | 41 | q_vel_init = np.array([ 42 | 0, 0, 0, 43 | 0, 0, 0, 44 | 0, 0, 0, 45 | 0, 0, 0, 46 | 0, 0, 0, 47 | 0, 0, 0 48 | ]) 49 | 50 | init_state = mujoco_py.cymj.MjSimState( 51 | time=0.0, 52 | qpos=q_pos_init, 53 | qvel=q_vel_init, 54 | act=None, 55 | udd_state={} 56 | ) 57 | sim.set_state(init_state) 58 | 59 | def get_true_simulation_data(sim): 60 | pos_base = sim.data.body_xpos[1] 61 | vel_base = sim.data.body_xvelp[1] 62 | quat_base = sim.data.sensordata[0:4] 63 | omega_base = sim.data.sensordata[4:7] 64 | pos_joint = sim.data.sensordata[10:22] 65 | vel_joint = sim.data.sensordata[22:34] 66 | touch_state = sim.data.sensordata[34:38] 67 | pos_foothold = [ 68 | sim.data.get_geom_xpos('fl_foot'), 69 | sim.data.get_geom_xpos('fr_foot'), 70 | sim.data.get_geom_xpos('rl_foot'), 71 | sim.data.get_geom_xpos('rr_foot') 72 | ] 73 | vel_foothold = [ 74 | sim.data.get_geom_xvelp('fl_foot'), 75 | sim.data.get_geom_xvelp('fr_foot'), 76 | sim.data.get_geom_xvelp('rl_foot'), 77 | sim.data.get_geom_xvelp('rr_foot') 78 | ] 79 | pos_thigh = [ 80 | sim.data.get_body_xpos("FL_thigh"), 81 | sim.data.get_body_xpos("FR_thigh"), 82 | sim.data.get_body_xpos("RL_thigh"), 83 | sim.data.get_body_xpos("RR_thigh") 84 | ] 85 | 86 | true_simulation_data = [ 87 | pos_base, 88 | vel_base, 89 | quat_base, 90 | omega_base, 91 | pos_joint, 92 | vel_joint, 93 | touch_state, 94 | pos_foothold, 95 | vel_foothold, 96 | pos_thigh 97 | ] 98 | # print(true_simulation_data) 99 | return true_simulation_data 100 | 101 | def get_simulated_sensor_data(sim): 102 | imu_quat = sim.data.sensordata[0:4] 103 | imu_gyro = sim.data.sensordata[4:7] 104 | imu_accelerometer = sim.data.sensordata[7:10] 105 | pos_joint = sim.data.sensordata[10:22] 106 | vel_joint = sim.data.sensordata[22:34] 107 | touch_state = sim.data.sensordata[34:38] 108 | 109 | simulated_sensor_data = [ 110 | imu_quat, 111 | imu_gyro, 112 | imu_accelerometer, 113 | pos_joint, 114 | vel_joint, 115 | touch_state 116 | ] 117 | # print(simulated_sensor_data) 118 | return simulated_sensor_data 119 | 120 | 121 | def initialize_robot(sim, viewer, robot_config, robot_data): 122 | predictive_controller = ModelPredictiveController(LinearMpcConfig, AliengoConfig) 123 | leg_controller = LegController(robot_config.Kp_swing, robot_config.Kd_swing) 124 | init_gait = Gait.STANDING 125 | vel_base_des = [0., 0., 0.] 126 | 127 | for iter_counter in range(800): 128 | 129 | if not STATE_ESTIMATION: 130 | data = get_true_simulation_data(sim) 131 | else: 132 | data = get_simulated_sensor_data(sim) 133 | 134 | robot_data.update( 135 | pos_base=data[0], 136 | lin_vel_base=data[1], 137 | quat_base=data[2], 138 | ang_vel_base=data[3], 139 | q=data[4], 140 | qdot=data[5] 141 | ) 142 | 143 | init_gait.set_iteration(predictive_controller.iterations_between_mpc,iter_counter) 144 | swing_states = init_gait.get_swing_state() 145 | gait_table = init_gait.get_gait_table() 146 | 147 | predictive_controller.update_robot_state(robot_data) 148 | contact_forces = predictive_controller.update_mpc_if_needed( 149 | iter_counter, vel_base_des, 0., gait_table, solver='drake', debug=False, iter_debug=0) 150 | 151 | torque_cmds = leg_controller.update(robot_data, contact_forces, swing_states) 152 | sim.data.ctrl[:] = torque_cmds 153 | 154 | sim.step() 155 | viewer.render() 156 | 157 | def main(): 158 | cur_path = os.path.dirname(__file__) 159 | mujoco_xml_path = os.path.join(cur_path, '../robot/aliengo/aliengo.xml') 160 | model = mujoco_py.load_model_from_path(mujoco_xml_path) 161 | sim = mujoco_py.MjSim(model) 162 | viewer = MjViewer(sim) 163 | 164 | robot_config = AliengoConfig 165 | 166 | reset(sim, robot_config) 167 | sim.step() 168 | 169 | urdf_path = os.path.join(cur_path, '../robot/aliengo/urdf/aliengo.urdf') 170 | robot_data = RobotData(urdf_path, state_estimation=STATE_ESTIMATION) 171 | # initialize_robot(sim, viewer, robot_config, robot_data) 172 | 173 | predictive_controller = ModelPredictiveController(LinearMpcConfig, AliengoConfig) 174 | leg_controller = LegController(robot_config.Kp_swing, robot_config.Kd_swing) 175 | 176 | gait = Gait.TROTTING10 177 | swing_foot_trajs = [SwingFootTrajectoryGenerator(leg_idx) for leg_idx in range(4)] 178 | 179 | vel_base_des = np.array([1.2, 0., 0.]) 180 | yaw_turn_rate_des = 0. 181 | 182 | iter_counter = 0 183 | 184 | while True: 185 | 186 | if not STATE_ESTIMATION: 187 | data = get_true_simulation_data(sim) 188 | else: 189 | data = get_simulated_sensor_data(sim) 190 | 191 | robot_data.update( 192 | pos_base=data[0], 193 | lin_vel_base=data[1], 194 | quat_base=data[2], 195 | ang_vel_base=data[3], 196 | q=data[4], 197 | qdot=data[5] 198 | ) 199 | 200 | gait.set_iteration(predictive_controller.iterations_between_mpc, iter_counter) 201 | swing_states = gait.get_swing_state() 202 | gait_table = gait.get_gait_table() 203 | 204 | predictive_controller.update_robot_state(robot_data) 205 | 206 | contact_forces = predictive_controller.update_mpc_if_needed(iter_counter, vel_base_des, 207 | yaw_turn_rate_des, gait_table, solver='drake', debug=False, iter_debug=0) 208 | 209 | pos_targets_swingfeet = np.zeros((4, 3)) 210 | vel_targets_swingfeet = np.zeros((4, 3)) 211 | 212 | for leg_idx in range(4): 213 | if swing_states[leg_idx] > 0: # leg is in swing state 214 | swing_foot_trajs[leg_idx].set_foot_placement( 215 | robot_data, gait, vel_base_des, yaw_turn_rate_des 216 | ) 217 | base_pos_base_swingfoot_des, base_vel_base_swingfoot_des = \ 218 | swing_foot_trajs[leg_idx].compute_traj_swingfoot( 219 | robot_data, gait 220 | ) 221 | pos_targets_swingfeet[leg_idx, :] = base_pos_base_swingfoot_des 222 | vel_targets_swingfeet[leg_idx, :] = base_vel_base_swingfoot_des 223 | 224 | torque_cmds = leg_controller.update(robot_data, contact_forces, swing_states, pos_targets_swingfeet, vel_targets_swingfeet) 225 | sim.data.ctrl[:] = torque_cmds 226 | 227 | sim.step() 228 | viewer.render() 229 | iter_counter += 1 230 | 231 | 232 | if iter_counter == 50000: 233 | sim.reset() 234 | reset(sim) 235 | iter_counter = 0 236 | break 237 | 238 | 239 | if __name__ == '__main__': 240 | main() -------------------------------------------------------------------------------- /scripts/isaacgym_a1.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from typing import List 4 | sys.path.append(os.path.join(os.path.dirname(__file__), '../config')) 5 | sys.path.append(os.path.join(os.path.dirname(__file__), '../linear_mpc')) 6 | sys.path.append(os.path.join(os.path.dirname(__file__), '../utils/')) 7 | 8 | from isaacgym import gymapi, gymtorch 9 | import numpy as np 10 | 11 | from gait import Gait 12 | from leg_controller import LegController 13 | from linear_mpc_configs import LinearMpcConfig 14 | from isaacgym_utils import * 15 | from mpc import ModelPredictiveController 16 | from robot_configs import A1Config, AliengoConfig 17 | from robot_data import RobotData 18 | from swing_foot_trajectory_generator import SwingFootTrajectoryGenerator 19 | 20 | 21 | NUM_ROBOTS = 4 22 | STATE_ESTIMATION = False 23 | 24 | def reset(gym, sim, robot_config): 25 | base_state_tensor_discription = gym.acquire_actor_root_state_tensor(sim) 26 | base_state_tensor = gymtorch.wrap_tensor(base_state_tensor_discription) 27 | base_state_tensor[:, 0:3] = torch.zeros_like(base_state_tensor[:, 0:3]) # base position 28 | base_state_tensor[:, 2] = robot_config.base_height_des 29 | base_state_tensor[:, 3:7] = torch.zeros_like(base_state_tensor[:, 3:7]) # base orientation 30 | base_state_tensor[:, 6] = 1. 31 | base_state_tensor[:, 7:10] = torch.zeros_like(base_state_tensor[:, 7:10]) # base linear velocity 32 | base_state_tensor[:, 10:13] = torch.zeros_like(base_state_tensor[:, 10:13]) # base angular velocity 33 | base_state_tensor_discription = gymtorch.unwrap_tensor(base_state_tensor) 34 | gym.set_actor_root_state_tensor(sim, base_state_tensor_discription) 35 | 36 | # joint_state_tensor_discription = gym.acquire_dof_state_tensor(sim) 37 | # joint_state_tensor = gymtorch.wrap_tensor(joint_state_tensor_discription) 38 | # joint_state_tensor[:, 1] = torch.zeros_like(joint_state_tensor[:, 1]) 39 | # for robot_idx in range(NUM_ROBOTS): 40 | # joint_state_tensor[0*robot_idx, 0] = 0 41 | # joint_state_tensor[1*robot_idx, 0] = 0.8 42 | # joint_state_tensor[2*robot_idx, 0] = -1.6 43 | # joint_state_tensor[3*robot_idx, 0] = 0 44 | # joint_state_tensor[4*robot_idx, 0] = 0.8 45 | # joint_state_tensor[5*robot_idx, 0] = -1.6 46 | # joint_state_tensor[6*robot_idx, 0] = 0 47 | # joint_state_tensor[7*robot_idx, 0] = 0.8 48 | # joint_state_tensor[8*robot_idx, 0] = -1.6 49 | # joint_state_tensor[9*robot_idx, 0] = 0 50 | # joint_state_tensor[10*robot_idx, 0] = 0.8 51 | # joint_state_tensor[11*robot_idx, 0] = -1.6 52 | # joint_state_tensor_discription = gymtorch.unwrap_tensor(joint_state_tensor) 53 | # gym.set_dof_state_tensor(sim, joint_state_tensor_discription) 54 | 55 | 56 | 57 | def main(): 58 | gym = gymapi.acquire_gym() 59 | sim = create_simulation( 60 | gym, 61 | dt=LinearMpcConfig.dt_control, 62 | use_gpu_pipeline=True 63 | ) 64 | 65 | robot_config = A1Config 66 | 67 | asset_root_path = os.path.join(os.path.dirname(__file__), '../robot/') 68 | robot_path = 'a1/urdf/a1.urdf' 69 | urdf_path = os.path.join(asset_root_path, robot_path) 70 | 71 | l_robot_model = [] 72 | l_robot_data: List[RobotData] = [] 73 | for _ in range(NUM_ROBOTS): 74 | l_robot_model.append(load_model(gym, sim, asset_root_path, robot_path, is_base_fixed=False)) 75 | l_robot_data.append(RobotData(urdf_path, state_estimation=STATE_ESTIMATION)) 76 | 77 | creat_ground_plane(gym, sim) 78 | 79 | for robot_model in l_robot_model: 80 | envs, actors = create_envs_actors(gym, sim, robot_model, robot_config.base_height_des) 81 | 82 | viewer = add_viewer(gym, sim) 83 | 84 | gym.prepare_sim(sim) 85 | 86 | reset(gym, sim, robot_config) 87 | 88 | l_predictive_controller: List[ModelPredictiveController] = [] 89 | l_leg_controller: List[LegController] = [] 90 | l_gait: List[Gait] = [] 91 | l_swing_foot_trajs: List[SwingFootTrajectoryGenerator] = [] 92 | for _ in range(NUM_ROBOTS): 93 | l_predictive_controller.append(ModelPredictiveController(LinearMpcConfig, A1Config)) 94 | l_leg_controller.append(LegController(robot_config.Kp_swing, robot_config.Kd_swing)) 95 | l_gait.append(Gait.TROTTING10) 96 | l_swing_foot_trajs.append([SwingFootTrajectoryGenerator(leg_idx) for leg_idx in range(4)]) 97 | 98 | vel_base_des = np.array([1.4, 0., 0.]) 99 | yaw_turn_rate_des = 0. 100 | 101 | iter_counter = 0 102 | render_counter = 0 103 | torque_cmds_tensor = torch.zeros(NUM_ROBOTS, 12, device='cuda:0', dtype=torch.float32) 104 | 105 | while not gym.query_viewer_has_closed(viewer): 106 | # step graphics 107 | if render_counter % 30 == 0: 108 | gym.step_graphics(sim) 109 | gym.draw_viewer(viewer, sim, True) 110 | gym.sync_frame_time(sim) 111 | render_counter = 0 112 | render_counter += 1 113 | 114 | base_state_tensor_discription = gym.acquire_actor_root_state_tensor(sim) 115 | base_state_tensor: torch.Tensor = gymtorch.wrap_tensor(base_state_tensor_discription) 116 | joint_state_tensor_discription = gym.acquire_dof_state_tensor(sim) 117 | joint_state_tensor: torch.Tensor = gymtorch.wrap_tensor(joint_state_tensor_discription) 118 | 119 | for robot_idx, (robot_data, predictive_controller, leg_controller, gait, swing_foot_trajs) \ 120 | in enumerate(zip(l_robot_data, l_predictive_controller, l_leg_controller, l_gait, l_swing_foot_trajs)): 121 | quat_base_imre=base_state_tensor[robot_idx, 3:7].cpu().numpy() 122 | quat_base_reim = np.array( 123 | [quat_base_imre[3], quat_base_imre[0], quat_base_imre[1], quat_base_imre[2]], 124 | dtype=np.float32 125 | ) 126 | robot_data.update( 127 | pos_base=base_state_tensor[robot_idx, 0:3].cpu().numpy(), 128 | quat_base=quat_base_reim, 129 | lin_vel_base=base_state_tensor[robot_idx, 7:10].cpu().numpy(), 130 | ang_vel_base=base_state_tensor[robot_idx, 10:13].cpu().numpy(), 131 | q = joint_state_tensor[12*robot_idx:12*(robot_idx+1), 0].cpu().numpy(), 132 | qdot = joint_state_tensor[12*robot_idx:12*(robot_idx+1), 1].cpu().numpy() 133 | ) 134 | print(robot_data.pos_base) 135 | print(robot_data.q) 136 | 137 | gait.set_iteration(predictive_controller.iterations_between_mpc, iter_counter) 138 | swing_states = gait.get_swing_state() 139 | gait_table = gait.get_gait_table() 140 | 141 | predictive_controller.update_robot_state(robot_data) 142 | 143 | contact_forces = predictive_controller.update_mpc_if_needed(iter_counter, vel_base_des, 144 | yaw_turn_rate_des, gait_table, solver='drake', debug=False, iter_debug=0) 145 | 146 | pos_targets_swingfeet = np.zeros((4, 3)) 147 | vel_targets_swingfeet = np.zeros((4, 3)) 148 | 149 | for leg_idx in range(4): 150 | if swing_states[leg_idx] > 0: # leg is in swing state 151 | swing_foot_trajs[leg_idx].set_foot_placement( 152 | robot_data, gait, vel_base_des, yaw_turn_rate_des 153 | ) 154 | base_pos_base_swingfoot_des, base_vel_base_swingfoot_des = \ 155 | swing_foot_trajs[leg_idx].compute_traj_swingfoot( 156 | robot_data, gait 157 | ) 158 | pos_targets_swingfeet[leg_idx, :] = base_pos_base_swingfoot_des 159 | vel_targets_swingfeet[leg_idx, :] = base_vel_base_swingfoot_des 160 | 161 | torque_cmds = leg_controller.update(robot_data, contact_forces, swing_states, pos_targets_swingfeet, vel_targets_swingfeet) 162 | torque_cmds_tensor[robot_idx, :] = torch.tensor(torque_cmds, dtype=torch.float32, device='cuda:0') 163 | torque_cmds_tensor_discription = gymtorch.unwrap_tensor(torque_cmds_tensor) 164 | gym.set_dof_actuation_force_tensor(sim, torque_cmds_tensor_discription) 165 | 166 | gym.fetch_results(sim, True) 167 | gym.simulate(sim) 168 | 169 | gym.refresh_dof_state_tensor(sim) 170 | gym.refresh_actor_root_state_tensor(sim) 171 | 172 | iter_counter += 1 173 | 174 | if iter_counter == 3000: 175 | reset(gym, sim, robot_config) 176 | iter_counter = 0 177 | render_counter = 0 178 | 179 | gym.destroy_viewer(viewer) 180 | gym.destroy_sim(sim) 181 | 182 | if __name__ == '__main__': 183 | main() -------------------------------------------------------------------------------- /utils/kinematics.py: -------------------------------------------------------------------------------- 1 | from typing import Optional, Union 2 | import math 3 | import numpy as np 4 | 5 | def vec_format_standardization( 6 | vec: Union[list, np.ndarray], std_type: Optional[str] = 'list' 7 | ) -> Union[list, np.ndarray]: 8 | 9 | assert type(vec) == list or type(vec) == np.ndarray 10 | assert std_type == 'list' or std_type == '1darray' 11 | 12 | if std_type == list: 13 | if type(vec) == np.ndarray: 14 | vec_shape = vec.shape 15 | assert len(vec_shape) == 1 or vec_shape[0] == 1 or vec_shape[1] == 1 16 | 17 | if len(vec_shape) == 1: 18 | vec = list(vec) 19 | elif vec_shape[0] == 1: 20 | vec = list(vec[0]) 21 | elif vec_shape[1] == 1: 22 | num_elements = vec_shape[0] 23 | vec = vec.reshape(1, num_elements) 24 | vec = list(vec[0]) 25 | 26 | return vec 27 | 28 | else: 29 | if type(vec) == list: 30 | vec = np.array(vec) 31 | else: 32 | vec_shape = vec.shape 33 | assert len(vec_shape) == 1 or vec_shape[0] == 1 or vec_shape[1] == 1 34 | 35 | if len(vec_shape) != 1: 36 | vec = vec.flatten() 37 | 38 | return vec 39 | 40 | def quat2ZYXangle(quat: Union[list, np.ndarray]) -> list: 41 | q = vec_format_standardization(quat) 42 | assert len(q) == 4 43 | 44 | # q[0]: real part 45 | # q[1], q[2], q[3]: imaginary part 46 | phi = math.atan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)) 47 | theta = math.asin(2 * (q[0] * q[2] - q[3] * q[1])) 48 | psi = math.atan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] ** 2 + q[3] ** 2)) 49 | return [phi, theta, psi] 50 | 51 | def quat2matrix(quat: Union[list, np.ndarray]) -> np.ndarray: 52 | q = vec_format_standardization(quat) 53 | assert len(q) == 4 54 | 55 | # q[0]: real part 56 | # q[1], q[2], q[3]: imaginary part 57 | r11 = q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2 58 | r12 = 2 * (q[1] * q[2] - q[0] * q[3]) 59 | r13 = 2 * (q[0] * q[2] + q[1] * q[3]) 60 | r21 = 2 * (q[0] * q[3] + q[1] * q[2]) 61 | r22 = q[0]**2 - q[1]**2 + q[2]**2 - q[3]**2 62 | r23 = 2 * (q[2] * q[3] - q[0] * q[1]) 63 | r31 = 2 * (q[1] * q[3] - q[0] * q[2]) 64 | r32 = 2 * (q[0] * q[1] + q[2] * q[3]) 65 | r33 = q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2 66 | rotation_matrix = np.array([ 67 | [r11, r12, r13], 68 | [r21, r22, r23], 69 | [r31, r32, r33] 70 | ]) 71 | return rotation_matrix 72 | 73 | # reference: modern robotics, appendix B.1 74 | def ZYXangles2matrix(ZYX_angle: Union[list, np.ndarray]) -> np.ndarray: 75 | # ZYX_angle = [phi, theta, psi] 76 | # R = Rz(psi)Ry(theta)Rx(phi) 77 | 78 | ZYX_angle = vec_format_standardization(ZYX_angle) 79 | assert len(ZYX_angle) == 3 80 | 81 | phi = ZYX_angle[0] 82 | theta = ZYX_angle[1] 83 | psi = ZYX_angle[2] 84 | 85 | r11 = math.cos(psi) * math.cos(theta) 86 | r12 = math.cos(psi) * math.sin(theta) * math.sin(phi) - math.sin(psi) * math.cos(phi) 87 | r13 = math.cos(psi) * math.sin(theta) * math.cos(phi) + math.sin(psi) * math.sin(phi) 88 | r21 = math.sin(psi) * math.cos(theta) 89 | r22 = math.sin(psi) * math.sin(theta) * math.sin(phi) + math.cos(psi) * math.cos(phi) 90 | r23 = math.sin(psi) * math.sin(theta) * math.cos(phi) - math.cos(psi) * math.sin(phi) 91 | r31 = -math.sin(theta) 92 | r32 = math.cos(theta) * math.sin(phi) 93 | r33 = math.cos(theta) * math.cos(phi) 94 | 95 | rotation_matrix = np.array([ 96 | [r11, r12, r13], 97 | [r21, r22, r23], 98 | [r31, r32, r33] 99 | ]) 100 | return rotation_matrix 101 | 102 | # reference: modern robotics, appendix B.1 103 | def matrix2ZYXangles(rotation_matrix: np.ndarray) -> list: 104 | # ZYX_angle = [phi, theta, psi] 105 | # R = Rz(psi)Ry(theta)Rx(phi) 106 | 107 | assert type(rotation_matrix) == np.ndarray and rotation_matrix.shape == (3, 3) 108 | 109 | r11 = rotation_matrix[0][0] 110 | r12 = rotation_matrix[0][1] 111 | r21 = rotation_matrix[1][0] 112 | r22 = rotation_matrix[1][1] 113 | r31 = rotation_matrix[2][0] 114 | r32 = rotation_matrix[2][1] 115 | r33 = rotation_matrix[2][2] 116 | 117 | if r31 != 1 and r31 != -1: 118 | psi = math.atan2(r21, r11) 119 | theta = math.atan2(-r31, (r11 ** 2 + r21 ** 2) ** 0.5) 120 | phi = math.atan2(r32, r33) 121 | elif r31 == -1: 122 | psi = 0 123 | theta = math.pi / 2 124 | phi = math.atan2(r12, r22) 125 | else: 126 | psi = 0 127 | theta = -math.pi / 2 128 | phi = -math.atan2(r12, r22) 129 | 130 | ZYX_angles = [phi, theta, psi] 131 | return ZYX_angles 132 | 133 | def matrix2quat(rotation_matrix: np.ndarray, quat_data_type: Optional[str] = 'list') -> Union[list, np.ndarray]: 134 | ''' 135 | Reference: modern robotics, appendix B.3 136 | ''' 137 | 138 | assert type(rotation_matrix) == np.ndarray and rotation_matrix.shape == (3, 3) 139 | assert quat_data_type == 'list' or quat_data_type == '1darray' 140 | 141 | r11 = rotation_matrix[0, 0] 142 | r22 = rotation_matrix[1, 1] 143 | r33 = rotation_matrix[2, 2] 144 | 145 | r12 = rotation_matrix[0, 1] 146 | r21 = rotation_matrix[1, 0] 147 | 148 | r13 = rotation_matrix[0, 2] 149 | r31 = rotation_matrix[2, 0] 150 | 151 | r23 = rotation_matrix[1, 2] 152 | r32 = rotation_matrix[2, 1] 153 | 154 | q0 = 0.5 * np.sqrt(1 + r11 + r22 + r33) 155 | 156 | coef = 1 / (4 * q0) 157 | q1 = coef * (r32 - r23) 158 | q2 = coef * (r13 - r31) 159 | q3 = coef * (r21 - r12) 160 | 161 | if quat_data_type == 'list': 162 | return [q0, q1, q2, q3] 163 | else: 164 | return np.array([q0, q1, q2, q3]) 165 | 166 | def vec2so3(vec: Union[list, np.ndarray]) -> np.ndarray: 167 | vec = vec_format_standardization(vec) 168 | assert len(vec) == 3 169 | 170 | so3 = np.zeros((3, 3)) 171 | so3[1, 2] = -vec[0] 172 | so3[0, 2] = vec[1] 173 | so3[0, 1] = -vec[2] 174 | so3[2, 1] = vec[0] 175 | so3[2, 0] = -vec[1] 176 | so3[1, 0] = vec[2] 177 | return so3 178 | 179 | def exp_so3(omega, theta) -> np.ndarray: 180 | omega = vec_format_standardization(omega) 181 | assert len(omega) == 3 182 | 183 | omega_ss = vec2so3(omega) 184 | exp_so3 = np.identity(3) + np.sin(theta) * omega_ss + \ 185 | (1 - np.cos(theta)) * omega_ss @ omega_ss 186 | return exp_so3 187 | 188 | def invT(T: np.ndarray) -> np.ndarray: 189 | assert type(T) == np.ndarray and T.shape == (4, 4) 190 | 191 | R = T[0:3, 0:3] 192 | p = T[0:3, 3] 193 | 194 | invT = np.zeros((4, 4)) 195 | invT[0:3, 0:3] = R.T 196 | invT[0:3, 3] = -R.T @ p 197 | invT[3, 3] = 1 198 | return invT 199 | 200 | def adSE3_T(SE3_matrix: np.ndarray) -> np.ndarray: 201 | assert type(SE3_matrix) == np.ndarray and SE3_matrix.shape == (4, 4) 202 | 203 | R = SE3_matrix[0:3, 0:3] 204 | p = SE3_matrix[0:3, 3] 205 | p_ss = vec2so3(p) 206 | 207 | adSE3 = np.zeros((6, 6)) 208 | adSE3[0:3, 0:3] = R 209 | adSE3[3:6, 3:6] = R 210 | adSE3[3:6, 0:3] = p_ss @ R 211 | return adSE3 212 | 213 | def adSE3_Rp(R: np.ndarray, p: Union[list, np.ndarray]) -> np.ndarray: 214 | p = vec_format_standardization(p) 215 | assert type(R) == np.ndarray and R.shape == (3, 3) 216 | assert len(p) == 3 217 | 218 | p_ss = vec2so3(p) 219 | 220 | adSE3 = np.zeros((6, 6)) 221 | adSE3[0:3, 0:3] = R 222 | adSE3[3:6, 3:6] = R 223 | adSE3[3:6, 0:3] = p_ss @ R 224 | return adSE3 225 | 226 | def Rp2T(R: np.ndarray, p: Union[list, np.ndarray]) -> np.ndarray: 227 | p = vec_format_standardization(p, std_type='1darray') 228 | assert type(p) == np.ndarray and p.shape == (3, ) 229 | assert type(R) == np.ndarray and R.shape == (3, 3) 230 | 231 | T = np.zeros((4, 4)) 232 | T[0:3, 0:3] = R 233 | T[0:3, 3] = p 234 | T[3, 3] = 1 235 | return T 236 | 237 | def exp_se3(screw_axes, theta) -> np.ndarray: 238 | S = vec_format_standardization(screw_axes) 239 | assert len(S) == 6 240 | 241 | omega = np.array([S[0], S[1], S[2]]) 242 | v = np.array([S[3], S[4], S[5]]) 243 | 244 | exp_se3 = np.zeros((4, 4)) 245 | 246 | if np.linalg.norm(omega) == 1: 247 | omega_ss = vec2so3(omega) 248 | exp_omega = exp_so3(omega, theta) 249 | 250 | exp_se3[0:3, 0:3] = exp_omega 251 | exp_se3[0:3, 3] = (theta * np.identity(3) + (1 - np.cos(theta)) \ 252 | * omega_ss + (theta - np.sin(theta)) * omega_ss @ omega_ss) @ v 253 | exp_se3[3, 3] = 1 254 | elif np.linalg.norm(omega) == 0 and np.linalg.norm(v) == 1: 255 | exp_se3[0:3, 0:3] = np.identity(3) 256 | exp_se3[0:3, 3] = theta * v 257 | exp_se3[3, 3] = 1 258 | else: 259 | print('ERROR: invalid se3.') 260 | return None 261 | 262 | return exp_se3 263 | 264 | def compute_screw_axis(omega, q) -> np.ndarray: 265 | omega = vec_format_standardization(omega) 266 | q = vec_format_standardization(q) 267 | assert len(omega) == 3 and len(q) == 3 268 | 269 | omega = np.array(omega) 270 | q = np.array(q) 271 | v = -vec2so3(omega) @ q 272 | S = np.append(omega, v) 273 | return S 274 | 275 | 276 | def twist2se3(twist) -> np.ndarray: 277 | twist = vec_format_standardization(twist) 278 | assert len(twist) == 6 279 | 280 | omega = twist[0:3] 281 | v = twist[3:6] 282 | 283 | se3 = np.zeros((4, 4)) 284 | 285 | se3[0, 3] = v[0] 286 | se3[1, 3] = v[1] 287 | se3[2, 3] = v[2] 288 | 289 | omega_ss = vec2so3(omega) 290 | se3[0:3, 0:3] = omega_ss 291 | 292 | return se3 293 | 294 | def fk_open_chain(home_config, screw_axes_list, theta_list) -> np.ndarray: 295 | assert len(screw_axes_list) == len(theta_list) 296 | 297 | num_joints = len(screw_axes_list) 298 | 299 | T = np.identity(4) 300 | for i in range(num_joints): 301 | Si = screw_axes_list[i] 302 | qi = theta_list[i] 303 | T = T @ exp_se3(Si, qi) 304 | T = T @ home_config 305 | 306 | return T 307 | 308 | def main(): 309 | quat = [0.7071, 0.7071, 0, 0] 310 | matrix = quat2matrix(quat) 311 | quat2 = matrix2quat(matrix) 312 | print(quat2) 313 | 314 | if __name__ == '__main__': 315 | main() -------------------------------------------------------------------------------- /utils/isaacgym_utils.py: -------------------------------------------------------------------------------- 1 | from math import sqrt 2 | from typing import Optional, Tuple 3 | 4 | from isaacgym import gymapi, gymutil 5 | from isaacgym.terrain_utils import * 6 | import numpy as np 7 | import torch 8 | 9 | def create_simulation( 10 | gym, 11 | dt: Optional[float] = 0.005, 12 | gravity: Optional[float] = -9.81, 13 | use_gpu_pipeline: Optional[bool] = False 14 | ): 15 | '''Create a simulation. 16 | 17 | Create a `sim` object with `gymapi.SimParams`. 18 | 19 | Args 20 | ---- 21 | gym: a `gym` object. 22 | dt: (optional) float 23 | simulation step size. 24 | gravity: (optional) float 25 | z-component for the 3-d vector representing 26 | gravity force in Newtons. 27 | use_gpu_pipeline: (optional) bool 28 | # TODO 29 | 30 | Returns 31 | ------- 32 | a `sim` object with `gymapi.SimParams`. 33 | ''' 34 | # get default set of parameters 35 | sim_params = gymapi.SimParams() 36 | 37 | # set common parameters 38 | sim_params.dt = dt 39 | sim_params.substeps = 1 40 | sim_params.up_axis = gymapi.UP_AXIS_Z 41 | sim_params.gravity = gymapi.Vec3(0.0, 0.0, gravity) 42 | 43 | # set PhysX-specific parameters 44 | sim_params.physx.use_gpu = True 45 | sim_params.use_gpu_pipeline = use_gpu_pipeline 46 | sim_params.physx.solver_type = 1 # TGS 47 | sim_params.physx.num_position_iterations = 4 # 4 improve solver convergence 48 | sim_params.physx.num_velocity_iterations = 0 49 | # shapes whose distance is less than the sum of their 50 | # contactOffset values will generate contacts 51 | sim_params.physx.contact_offset = 0.001 52 | # two shapes will come to rest at a distance equal to 53 | # the sum of their restOffset values 54 | sim_params.physx.rest_offset = 0.0 55 | # A contact with a relative velocity below this will not bounce. 56 | sim_params.physx.bounce_threshold_velocity = 0.5 57 | # The maximum velocity permitted to be introduced by the solver 58 | # to correct for penetrations in contacts. 59 | sim_params.physx.max_depenetration_velocity = 1.0 60 | 61 | # create sim with these parameters 62 | sim = gym.create_sim( 63 | compute_device=0, 64 | graphics_device=0, 65 | type=gymapi.SIM_PHYSX, 66 | params=sim_params 67 | ) 68 | 69 | return sim 70 | 71 | 72 | def creat_ground_plane( 73 | gym, 74 | sim, 75 | static_friction: Optional[float] = 1.0, 76 | dynamic_friction: Optional[float] = 1.0 77 | ) -> None: 78 | '''Create a ground plane for simulation. 79 | 80 | Args 81 | ---- 82 | gym: a `gym` object. 83 | sim: a `sim` object. 84 | static_friction: (optional) float 85 | coefficients of static friction. 86 | dynamic friction: (optional) float 87 | coefficients of dynamic friction. 88 | ''' 89 | plane_params = gymapi.PlaneParams() 90 | 91 | # the orientation of the plane. 92 | plane_params.normal = gymapi.Vec3(0, 0, 1) # z-up! 93 | # defines the distance of the plane from the origin. 94 | plane_params.distance = 0 95 | plane_params.static_friction = static_friction 96 | plane_params.dynamic_friction = dynamic_friction 97 | # used to control the elasticity of collisions with the 98 | # ground plane (amount of bounce). 99 | plane_params.restitution = 0 100 | 101 | # create the ground plane 102 | gym.add_ground(sim, plane_params) 103 | 104 | 105 | def load_model( 106 | gym, 107 | sim, 108 | root_path: str, 109 | file_path: str, 110 | is_base_fixed: Optional[bool] = False 111 | ): 112 | '''Loading a robot model. 113 | 114 | Gym currently supports loading URDF, MJCF, and USD file formats. 115 | Loading an asset file creates a GymAsset object that includes 116 | the definiton of all the bodies, collision shapes, visual attachments, 117 | joints, and degrees of freedom (DOFs). 118 | 119 | Args 120 | ---- 121 | gym: a `gym` object. 122 | sim: a `sim` object. 123 | root_path: str 124 | asset root directory, can be specified as an absolute 125 | path or as a path relative to the current working directory. 126 | file_path: str 127 | the path of the model (Ex. the urdf file) relative 128 | to the `root_path`. 129 | is_base_fixed: (optional) bool 130 | is base link fixed. 131 | 132 | Returns 133 | ------- 134 | a `GymAsset` object that includes the definiton of all the bodies, 135 | collision shapes, visual attachments, joints, and degrees of freedom (DOFs). 136 | ''' 137 | asset_options = gymapi.AssetOptions() 138 | asset_options.default_dof_drive_mode = gymapi.DOF_MODE_NONE 139 | asset_options.fix_base_link = is_base_fixed 140 | asset_options.use_mesh_materials = True 141 | asset_options.flip_visual_attachments = True 142 | asset_options.angular_damping = 0.0 143 | asset_options.linear_damping = 0.0 144 | 145 | # added to the diagonal elements of inertia tensors for all of the 146 | # asset’s rigid bodies/links. could improve simulation stability. 147 | asset_options.armature = 0.01 148 | 149 | asset_options.use_mesh_materials = True 150 | robot_model = gym.load_asset(sim, root_path, file_path, asset_options) 151 | return robot_model 152 | 153 | 154 | def create_envs_actors( 155 | gym, 156 | sim, 157 | robot_model, 158 | body_height: float, 159 | num_envs: Optional[int] = 1, 160 | envs_per_row: Optional[int] = -1, 161 | env_spacing: Optional[float] = 1.0, 162 | actor_name: Optional[str] = 'Actor' 163 | ) -> Tuple[list, list]: 164 | '''Create environments and actors. 165 | 166 | An environment consists of a collection of actors and sensors 167 | that are simulated together. Actors within an environment 168 | interact with each other physically. Their state is maintained 169 | by the physics engine and can be controlled using the control API. 170 | Sensors placed in an environment, like cameras, will be able 171 | to capture the actors in that environment. 172 | 173 | Each env has its own coordinate space, which gets embedded in the 174 | global simulation space. When creating an environment, we specify 175 | the local extents of the environment, which depend on the desired 176 | spacing between environment instances. As new environments get 177 | added to the simulation, they will be arranged in a 2D grid one 178 | row at a time. 179 | 180 | An actor is simply an instance of a `GymAsset`. Each actor must 181 | be placed in an environment. You cannot have an actor that doesn't 182 | belong to an environment. The actor pose is defined in env-local 183 | coordinates using position vector p = (0, 0, body_height), and 184 | orientation quaternion r = I. 185 | 186 | Args 187 | ---- 188 | gym: a `gym` object. 189 | sim: a `sim` object. 190 | robot_model: a robot model object. 191 | body_height: float 192 | z-component of the base position expressed in world frame. 193 | num_envs: (optional) int 194 | number of environments. 195 | envs_per_row: (optional) int 196 | number of environments per row. 197 | env_spacing: (optional) float 198 | desired space between environment instances. 199 | actor_name: (optional) str 200 | specify a name for your actor. this makes it possible to 201 | look up the actor by name. If you wish to do this, make sure 202 | that you assign unique names to all your actors within the 203 | same environment. 204 | 205 | Returns 206 | ------- 207 | two lists. the one containing environment instances, the other 208 | containing actor handles. 209 | ''' 210 | 211 | if envs_per_row == -1: 212 | envs_per_row = int(sqrt(num_envs)) 213 | 214 | # set up the env grid 215 | env_lower = gymapi.Vec3(-env_spacing, -env_spacing, 0.0) 216 | env_upper = gymapi.Vec3(env_spacing*2, env_spacing, env_spacing) 217 | 218 | envs = [] 219 | actor_handles = [] 220 | 221 | for i in range(num_envs): 222 | env = gym.create_env(sim, env_lower, env_upper, envs_per_row) 223 | envs.append(env) 224 | 225 | pose = gymapi.Transform() 226 | init_pos_base = torch.zeros(3, dtype=torch.float32, device='cuda:0') 227 | init_pos_base[2] = body_height 228 | pose.p = gymapi.Vec3(*init_pos_base) 229 | 230 | # pose.p = gymapi.Vec3(0.0, 0.0, body_height) 231 | 232 | cur_actor_name = actor_name + str(i) 233 | 234 | actor_handle = gym.create_actor(env, robot_model, pose, cur_actor_name, 235 | group=i, filter=1) 236 | actor_handles.append(actor_handle) 237 | 238 | return envs, actor_handles 239 | 240 | 241 | def add_viewer(gym, sim): 242 | '''Create a viewer. 243 | 244 | By default, the simulation does not create any visual feedback window. 245 | This allows the simulations to run on headless workstations or clusters 246 | with no monitors attached. When developing and testing, however, it is 247 | useful to be able to visualize the simulation. Isaac Gym comes with a 248 | simple integrated viewer that lets you see what’s going on in the 249 | simulation. 250 | 251 | Args 252 | ---- 253 | gym: a `gym` object. 254 | sim: a `sim` object. 255 | 256 | Returns 257 | ------- 258 | a viewer that lets you see what's going on the simulation. 259 | ''' 260 | cam_props = gymapi.CameraProperties() 261 | viewer = gym.create_viewer(sim, cam_props) 262 | 263 | return viewer 264 | 265 | 266 | 267 | def add_terrain(gym, sim, name="slope", x_offset=2., invert=False, width=2.8): 268 | # terrains 269 | num_terrains = 1 270 | terrain_width = 2. 271 | terrain_length = width 272 | horizontal_scale = 0.05 # [m] resolution in x 273 | vertical_scale = 0.005 # [m] resolution in z 274 | num_rows = int(terrain_width/horizontal_scale) 275 | num_cols = int(terrain_length/horizontal_scale) 276 | heightfield = np.zeros((num_terrains*num_rows, num_cols), dtype=np.int16) 277 | 278 | step_height = 0.07 279 | step_width = 0.3 280 | num_steps = terrain_width / step_width 281 | height = step_height * num_steps 282 | slope = height / terrain_width 283 | # num_stairs = height / step_height 284 | # step_width = terrain_length / num_stairs 285 | 286 | def new_sub_terrain(): return SubTerrain(width=num_rows, length=num_cols, vertical_scale=vertical_scale, horizontal_scale=horizontal_scale) 287 | if name=="slope": 288 | heightfield[0: num_rows, :] = sloped_terrain(new_sub_terrain(), slope=slope).height_field_raw 289 | elif name=="stair": 290 | heightfield[0: num_rows, :] = stairs_terrain(new_sub_terrain(), step_width=step_width, step_height=step_height).height_field_raw 291 | elif name=="pyramid": 292 | heightfield[0: num_rows, :] = pyramid_stairs_terrain(new_sub_terrain(), step_width=step_width, step_height=step_height).height_field_raw 293 | else: 294 | raise NotImplementedError("Not support terrains!") 295 | 296 | if invert: 297 | heightfield[0: num_rows, :] = heightfield[0: num_rows, :][::-1] 298 | 299 | # add the terrain as a triangle mesh 300 | vertices, triangles = convert_heightfield_to_trimesh(heightfield, horizontal_scale=horizontal_scale, vertical_scale=vertical_scale, slope_threshold=1.5) 301 | tm_params = gymapi.TriangleMeshParams() 302 | tm_params.nb_vertices = vertices.shape[0] 303 | tm_params.nb_triangles = triangles.shape[0] 304 | tm_params.transform.p.x = x_offset 305 | tm_params.transform.p.y = -1 306 | if name=="stair": 307 | tm_params.transform.p.z = -0.09 308 | elif name=="pyramid": 309 | tm_params.transform.p.z = 0.01 310 | 311 | gym.add_triangle_mesh(sim, vertices.flatten(), triangles.flatten(), tm_params) 312 | return heightfield -------------------------------------------------------------------------------- /utils/robot_data.py: -------------------------------------------------------------------------------- 1 | import os 2 | from typing import List, Optional, Tuple, Union 3 | 4 | import numpy as np 5 | import pinocchio 6 | 7 | from kinematics import adSE3_Rp, quat2matrix, quat2ZYXangle, vec_format_standardization 8 | 9 | 10 | class RobotData(): 11 | ''' 12 | This framework is to use the sensor data (measurement) to generate all the 13 | data of the robot in our control strategy. 14 | 15 | Params 16 | ------ 17 | robot_model: str 18 | path to urdf file of the robot. 19 | state_estimation: (optional) bool 20 | identify whether to use the state estimation module. If the state estimation 21 | module is used, we add noises to the data to simulate the real case. 22 | Otherwise we use the data provided by the simulator directly. 23 | 24 | Measurement 25 | ----------- 26 | 1. the position $q$ and velocity $\dot{q}$ of each joint, provided by the 27 | incremental encoder. 28 | 2. the position $p$, velocity $v$, orientation $R$ and angular velocity 29 | $\omega$ of the base, provided by IMU. Note that the orientation is expressed 30 | as quanternion. 31 | 32 | Sign Convention 33 | --------------- 34 | 1. quanternion is recorded as (real part, imaginary part). 35 | 2. If we want to express a quantity B with respect to C in frame A, we write 36 | A_quantity_C_B. By default, if the quantity does not have the prefix A, it 37 | means this quantity is expressed in world frame, or inertia frame. In such 38 | case, the prefix A is omitted. Similarly, if the quantity does not have the 39 | suffix C, it means this quantity is relative to the world, or the inertia 40 | frame. Now we show some examples: 41 | Ex.1. base_pos_base_foot: the position of foot with respect to base expressed 42 | in base frame. 43 | Ex.2. pos_base_foot: the position of foot with respect to base expressed in 44 | world frame. (it is the same as world_pos_base_foot) 45 | Ex.3. pos_foot: the position of foot with respect world expressed in world 46 | frame. (it is the same as world_pos_world_foot) 47 | ''' 48 | 49 | def __init__( 50 | self, 51 | robot_model: str, 52 | state_estimation: Optional[bool] = False 53 | ) -> None: 54 | self.state_estimation = state_estimation 55 | self.__init_pinocchio(robot_model) 56 | 57 | self.__contact_history = np.zeros((4, 3), dtype=np.float32) 58 | 59 | def update( 60 | self, 61 | # data: Union[list, np.ndarray], 62 | pos_base: Union[list, np.ndarray], 63 | lin_vel_base: Union[list, np.ndarray], 64 | quat_base: Union[list, np.ndarray], 65 | ang_vel_base: Union[list, np.ndarray], 66 | q: Union[list, np.ndarray], 67 | qdot: Union[list, np.ndarray] 68 | ) -> None: 69 | if not self.state_estimation: 70 | self.pos_base: np.ndarray = vec_format_standardization(pos_base, '1darray') 71 | self.lin_vel_base: np.ndarray = vec_format_standardization(lin_vel_base, '1darray') 72 | self.quat_base: np.ndarray = vec_format_standardization(quat_base, '1darray') 73 | self.ang_vel_base: np.ndarray = vec_format_standardization(ang_vel_base, '1darray') 74 | 75 | self.R_base = quat2matrix(self.quat_base) 76 | self.rpy_base = np.array(quat2ZYXangle(self.quat_base)) 77 | 78 | self.q: np.ndarray = vec_format_standardization(q, '1darray') 79 | self.qdot: np.ndarray = vec_format_standardization(qdot, '1darray') 80 | else: 81 | raise NotImplementedError 82 | 83 | # NOTE: quat in mujoco / Isaac Gym: (real part, imaginary part) 84 | # quat in pinocchio: (imaginary part, real part) 85 | quat_base_converted = [self.quat_base[1], self.quat_base[2], 86 | self.quat_base[3], self.quat_base[0]] 87 | # generalized joint position (floating base), dim: 3 + 3 + 12 = 18. 88 | generalized_q = list(self.pos_base) + list(quat_base_converted) \ 89 | + list(self.q) 90 | self.__generalized_q = np.array(generalized_q, dtype=np.float32) 91 | pinocchio.forwardKinematics(self.__pin_model, self.__pin_data, self.__generalized_q) 92 | pinocchio.framesForwardKinematics(self.__pin_model, self.__pin_data, self.__generalized_q) 93 | 94 | # NOTE: jacobian in pinocchio is [Jv, Jw], thus this X is actually X.T defined in modern robotics 95 | self.X_base = adSE3_Rp(self.R_base, self.pos_base) 96 | 97 | self.Jv_feet, self.base_Jv_feet = self.__compute_foot_Jacobian() # geometric jacobian 98 | # the position of feet relative to world expressed in world frame 99 | self.pos_feet = self.__compute_pos_feet() 100 | # the position of feet relative to base expressed in world frame 101 | self.pos_base_feet = self.__compute_pos_base_feet() 102 | # the position of feet relative to base expressed in base frame 103 | self.base_pos_base_feet = self.__compute_base_pos_base_feet() 104 | self.base_vel_base_feet = self.__compute_base_vel_base_feet() 105 | # print(self.base_vel_base_feet) 106 | 107 | self.pos_thighs = self.__compute_pos_thighs() 108 | self.base_pos_base_thighs = self.__compute_base_pos_base_thighs() 109 | 110 | def __init_pinocchio(self, robot_model) -> None: 111 | # NOTE: The second parameter represents the floating base. 112 | # see https://github.com/stack-of-tasks/pinocchio/issues/1596 113 | self.__pin_model = pinocchio.buildModelFromUrdf( 114 | robot_model, pinocchio.JointModelFreeFlyer()) 115 | self.__pin_data = self.__pin_model.createData() 116 | 117 | def __compute_foot_Jacobian(self) -> Tuple[List[np.ndarray], List[np.ndarray]]: 118 | foot_frames = ['FL_foot_fixed', 'FR_foot_fixed', 'RL_foot_fixed', 'RR_foot_fixed'] 119 | foot_frames_ID = [self.__pin_model.getFrameId(foot_frames[i]) for i in range(4)] 120 | 121 | Jv_feet = [] 122 | base_Jv_feet = [] 123 | for i in range(4): 124 | Ji = pinocchio.computeFrameJacobian(self.__pin_model, self.__pin_data, 125 | self.__generalized_q, frame_id=foot_frames_ID[i], 126 | reference_frame=pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED) 127 | # Jacobian in Pinocchio: [[Jv] 128 | # [Jw]] 129 | base_Ji = self.X_base @ Ji 130 | Jv_feet.append(Ji[0:3, :]) 131 | base_Jv_feet.append(base_Ji[0:3, :]) 132 | 133 | return Jv_feet, base_Jv_feet 134 | 135 | def __compute_pos_feet(self) -> List[np.ndarray]: 136 | foot_frames = ['FL_foot_fixed', 'FR_foot_fixed', 'RL_foot_fixed', 'RR_foot_fixed'] 137 | foot_frames_ID = [self.__pin_model.getFrameId(foot_frames[i]) for i in range(4)] 138 | pos_feet = [] 139 | for foot_idx in range(4): 140 | pos_footi = self.__pin_data.oMf[foot_frames_ID[foot_idx]].translation 141 | pos_feet.append(pos_footi) 142 | return pos_feet 143 | 144 | def __compute_pos_base_feet(self) -> List[np.ndarray]: 145 | pos_base_feet = [] 146 | for foot_idx in range(4): 147 | pos_base_footi = self.pos_feet[foot_idx] - self.pos_base 148 | pos_base_feet.append(pos_base_footi) 149 | return pos_base_feet 150 | 151 | def __compute_base_pos_base_feet(self) -> List[np.ndarray]: 152 | base_pos_base_feet = [] 153 | for foot_idx in range(4): 154 | base_pos_base_footi = self.R_base.T @ self.pos_base_feet[foot_idx] 155 | base_pos_base_feet.append(base_pos_base_footi) 156 | return base_pos_base_feet 157 | 158 | def __compute_base_vel_base_feet(self) -> List[np.ndarray]: 159 | base_vel_base_feet = [] 160 | for foot_idx in range(4): 161 | generalized_qdot = list(self.lin_vel_base) + list(self.ang_vel_base) + list(self.qdot) 162 | generalized_qdot = np.array(generalized_qdot, dtype=np.float32) 163 | vel_footi = self.Jv_feet[foot_idx] @ generalized_qdot 164 | vel_base_footi = vel_footi - self.lin_vel_base 165 | base_vel_base_footi = self.R_base.T @ vel_base_footi 166 | base_vel_base_feet.append(base_vel_base_footi) 167 | return base_vel_base_feet 168 | 169 | def __compute_pos_thighs(self) -> List[np.ndarray]: 170 | thigh_frames = ['FL_thigh_joint', 'FR_thigh_joint', 'RL_thigh_joint', 'RR_thigh_joint'] 171 | thigh_frames_ID = [self.__pin_model.getFrameId(thigh_frames[i]) for i in range(4)] 172 | pos_thighs = [] 173 | for thigh_idx in range(4): 174 | pos_thighi = self.__pin_data.oMf[thigh_frames_ID[thigh_idx]].translation 175 | pos_thighs.append(pos_thighi) 176 | return pos_thighs 177 | 178 | def __compute_base_pos_base_thighs(self) -> List[np.ndarray]: 179 | base_pos_base_thighs = [] 180 | for thigh_idx in range(4): 181 | pos_base_thighi = self.pos_thighs[thigh_idx] - self.pos_base 182 | base_pos_base_thighi = self.R_base.T @ pos_base_thighi 183 | base_pos_base_thighs.append(base_pos_base_thighi) 184 | return base_pos_base_thighs 185 | 186 | def init_contact_history(self) -> None: 187 | self.__contact_history == self.pos_feet 188 | 189 | def __update_contact_history(self, cur_contact_state: Union[list, np.ndarray]) -> None: 190 | for foot_idx in range(4): 191 | if cur_contact_state[foot_idx] == 1: 192 | self.__contact_history[foot_idx] = self.pos_feet[foot_idx] 193 | 194 | def update_terrain_normal(self, cur_contact_state: Union[list, np.ndarray]) -> None: 195 | self.__update_contact_history(cur_contact_state) 196 | 197 | # Least squares approach 198 | # A x = b 199 | # [1 p1x p1y] [a0] [p1z] 200 | # [1 p2x p2y] [a1] = [p2z] 201 | # [1 p3x p3y] [a2] [p3z] 202 | # [1 p4x p4y] [p4z] 203 | # A = np.array([ 204 | # [1, self.__contact_history[0, 0], self.__contact_history[0, 1]], 205 | # [1, self.__contact_history[1, 0], self.__contact_history[1, 1]], 206 | # [1, self.__contact_history[2, 0], self.__contact_history[2, 1]], 207 | # [1, self.__contact_history[3, 0], self.__contact_history[3, 1]] 208 | # ]) 209 | # b = np.array([ 210 | # [self.__contact_history[0, 2]], 211 | # [self.__contact_history[1, 2]], 212 | # [self.__contact_history[2, 2]], 213 | # [self.__contact_history[3, 2]] 214 | # ]) 215 | 216 | # x = np.linalg.lstsq(A, b)[0] 217 | 218 | # PCA approach 219 | X = self.__contact_history.T 220 | mu = np.mean(self.__contact_history, axis=0).reshape(3, 1) 221 | one = np.ones((1, 4), dtype=np.float32) 222 | sigma = (X - mu @ one) @ ((X - mu @ one).T) 223 | eigenvalues, eigenvectors = np.linalg.eig(sigma) 224 | self.terrain_normal_est = eigenvectors[np.argmin(eigenvalues)] 225 | if self.terrain_normal_est[2] < 0: 226 | self.terrain_normal_est = -self.terrain_normal_est 227 | self.terrain_normal_est_yaw_aligned = self.R_base.T @ self.terrain_normal_est 228 | # print(self.terrain_normal_est_yaw_aligned) 229 | 230 | def test(): 231 | robot_path = os.path.join(os.path.dirname(__file__), '../robot/aliengo/urdf/aliengo.urdf') 232 | 233 | robot_data = RobotData(robot_model=robot_path) 234 | robot_data.update( 235 | pos_base=[0.00727408, 0.00061764, 0.43571295], 236 | lin_vel_base=[0.0189759 , 0.00054278, 0.02322867], 237 | quat_base=[9.99951619e-01, -9.13191258e-03, 3.57360542e-03, 7.72221709e-04], 238 | ang_vel_base=[-0.06964452, -0.01762341, -0.00088601], 239 | q=[0.00687206, 0.52588717, -1.22975589, 240 | 0.02480081, 0.51914926, -1.21463939, 241 | 0.00892169, 0.51229961, -1.20195572, 242 | 0.02621839, 0.50635251, -1.18849609], 243 | qdot=[0.06341452, -0.02158136, 0.16191205, 244 | 0.07448259, -0.04855474, 0.21399941, 245 | 0.06280346, 0.00562435, 0.10597827, 246 | 0.07388069, -0.02180622, 0.15909948], 247 | ) 248 | 249 | print(robot_data.base_pos_base_feet) 250 | 251 | if __name__ == '__main__': 252 | test() -------------------------------------------------------------------------------- /linear_mpc/mpc.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import time 4 | from typing import Union 5 | 6 | sys.path.append(os.path.join(os.path.dirname(__file__), '../config')) 7 | sys.path.append(os.path.join(os.path.dirname(__file__), '../utils')) 8 | 9 | import matplotlib.pyplot as plt 10 | from numba import jit, vectorize, float32 11 | import numpy as np 12 | from scipy.linalg import expm 13 | from pydrake.all import MathematicalProgram, Solve 14 | from qpsolvers import solve_qp 15 | 16 | from kinematics import quat2ZYXangle, vec2so3 17 | from linear_mpc_configs import LinearMpcConfig 18 | from robot_configs import RobotConfig 19 | from robot_data import RobotData 20 | 21 | 22 | class ModelPredictiveController(): 23 | 24 | def __init__(self, mpc_config: LinearMpcConfig, robot_config: RobotConfig): 25 | # state: x(t) = [\theta, p, \omega, \dot{p}, g], dim: 13 26 | self.num_state = 3 + 3 + 3 + 3 + 1 27 | # input: u(t) = [f1, f2, f3, f4], dim: 12 28 | self.num_input = 3 + 3 + 3 + 3 29 | 30 | self.is_initialized = False 31 | self.is_first_run = True 32 | 33 | self._load_parameters(mpc_config, robot_config) 34 | 35 | def _load_parameters(self, mpc_config: LinearMpcConfig, robot_config: RobotConfig): 36 | self.dt_control = mpc_config.dt_control 37 | self.iterations_between_mpc = mpc_config.iteration_between_mpc 38 | self.dt = 0.05 39 | self.horizon = mpc_config.horizon 40 | self.mu = mpc_config.friction_coef 41 | self.fz_max = robot_config.fz_max 42 | self.gravity = mpc_config.gravity 43 | 44 | self.base_inertia_base = robot_config.base_inertia_base 45 | self.mass = robot_config.mass_base 46 | self.com_height_des = robot_config.base_height_des 47 | 48 | # MPC weights 49 | _Qi = mpc_config.Q 50 | self.Qbar = np.kron(np.identity(self.horizon), _Qi) 51 | _Ri = mpc_config.R 52 | self.Rbar = np.kron(np.identity(self.horizon), _Ri) 53 | 54 | # data: [pos_base, vel_base, quat_base, omega_base, pos_joint, vel_joint, touch_state, pos_foothold, pos_thigh] 55 | def update_robot_state(self, robot_data: RobotData): 56 | if self.is_initialized == False: 57 | self.current_state = np.zeros(13, dtype=np.float32) 58 | self.roll_init = 0.0 59 | self.pitch_init = 0.0 60 | self.is_initialized = True 61 | 62 | self.__robot_data = robot_data 63 | 64 | # update xt 65 | rpy_base = quat2ZYXangle(robot_data.quat_base) 66 | pos_base = np.array(robot_data.pos_base, dtype=np.float32) 67 | # print(pos_base[2]) 68 | omega_base = np.array(robot_data.ang_vel_base, dtype=np.float32) 69 | vel_base = np.array(robot_data.lin_vel_base, dtype=np.float32) 70 | for i in range(3): 71 | self.current_state[i] = rpy_base[i] 72 | self.current_state[3+i] = pos_base[i] 73 | self.current_state[6+i] = omega_base[i] 74 | self.current_state[9+i] = vel_base[i] 75 | # NOTE: this should be negative!!! 76 | self.current_state[12] = -self.gravity 77 | self.yaw = rpy_base[2] 78 | # update ri 79 | self.pos_base_feet = robot_data.pos_base_feet 80 | 81 | def update_mpc_if_needed(self, iter_counter, base_vel_base_des, yaw_turn_rate_des, 82 | gait_table, solver='drake', debug=False, iter_debug=None): 83 | vel_base_des = self.__robot_data.R_base @ base_vel_base_des 84 | if self.is_first_run: 85 | self.xpos_base_desired = 0.0 86 | self.ypos_base_desired = 0.0 87 | self.yaw_desired = self.yaw 88 | self.is_first_run = False 89 | else: 90 | self.xpos_base_desired += self.dt_control * vel_base_des[0] 91 | self.ypos_base_desired += self.dt_control * vel_base_des[1] 92 | self.yaw_desired = self.yaw + self.dt_control * yaw_turn_rate_des 93 | 94 | # decide whether MPC needs to be updated 95 | if iter_counter % self.iterations_between_mpc == 0: 96 | ref_traj = self.generate_reference_trajectory(vel_base_des, yaw_turn_rate_des) 97 | self.ref_traj = ref_traj 98 | solve_start = time.time() 99 | self.__contact_forces = self._solve_mpc(ref_traj, gait_table, solver=solver)[0:12] 100 | solve_end = time.time() 101 | print('MPC solved in {:3f}s.'.format(solve_end - solve_start)) 102 | print(self.yaw, self.yaw_desired) 103 | 104 | if debug and iter_counter == iter_debug: 105 | contact_forces_debug = self._solve_mpc(ref_traj, gait_table, solver=solver, debug=debug) 106 | self.__visulize_com_traj_solution(contact_forces_debug) 107 | 108 | return self.__contact_forces[0:12] 109 | 110 | def generate_reference_trajectory( 111 | self, 112 | vel_base_des: Union[list, np.ndarray], 113 | yaw_turn_rate: float 114 | ) -> np.ndarray: 115 | 116 | # vel_base_des = self.__robot_data.R_base @ base_vel_base_des 117 | 118 | cur_xpos_desired = self.xpos_base_desired 119 | cur_ypos_desired = self.ypos_base_desired 120 | 121 | max_pos_error = 0.1 # define the threshold for error of position 122 | 123 | ''' 124 | compare the desired robot position and the current robot position. 125 | if the error is beyond this threshold, the current position (plus 126 | the threshold) is used as the desired position. else the desired 127 | position does not change. 128 | ''' 129 | if cur_xpos_desired - self.current_state[3] > max_pos_error: 130 | cur_xpos_desired = self.current_state[3] + max_pos_error 131 | if self.current_state[3] - cur_xpos_desired > max_pos_error: 132 | cur_xpos_desired = self.current_state[3] - max_pos_error 133 | 134 | if cur_ypos_desired - self.current_state[4] > max_pos_error: 135 | cur_ypos_desired = self.current_state[4] + max_pos_error 136 | if self.current_state[4] - cur_ypos_desired > max_pos_error: 137 | cur_ypos_desired = self.current_state[4] - max_pos_error 138 | 139 | self.xpos_base_desired = cur_xpos_desired 140 | self.ypos_base_desired = cur_ypos_desired 141 | 142 | # pitch and roll compensation 143 | if np.fabs(self.current_state[9]) > 0.2: 144 | self.pitch_init += self.dt * (0.0 - self.current_state[1]) / self.current_state[9] 145 | if np.fabs(self.current_state[10]) > 0.1: 146 | self.roll_init += self.dt * (0.0 - self.current_state[0]) / self.current_state[10] 147 | 148 | # staturation for pitch and roll compensation 149 | self.roll_init = np.fmin(np.fmax(self.roll_init, -0.25), 0.25) 150 | self.pitch_init = np.fmin(np.fmax(self.pitch_init, -0.25), 0.25) 151 | roll_comp = self.current_state[10] * self.roll_init 152 | pitch_comp = self.current_state[9] * self.pitch_init 153 | 154 | X_ref = np.zeros(self.num_state * self.horizon, dtype=np.float32) 155 | X_ref[0::self.num_state] = roll_comp 156 | X_ref[1::self.num_state] = pitch_comp 157 | X_ref[2] = self.yaw_desired 158 | X_ref[3] = cur_xpos_desired 159 | X_ref[4] = cur_ypos_desired 160 | X_ref[5::self.num_state] = self.com_height_des 161 | X_ref[8::self.num_state] = yaw_turn_rate 162 | X_ref[9::self.num_state] = vel_base_des[0] 163 | X_ref[10::self.num_state] = vel_base_des[1] 164 | X_ref[12::self.num_state] = -self.gravity 165 | for i in range(1, self.horizon): 166 | X_ref[2 + self.num_state*i] = X_ref[2 + self.num_state*(i-1)] + self.dt * yaw_turn_rate 167 | X_ref[3 + self.num_state*i] = X_ref[3 + self.num_state*(i-1)] + self.dt * vel_base_des[0] 168 | X_ref[4 + self.num_state*i] = X_ref[4 + self.num_state*(i-1)] + self.dt * vel_base_des[1] 169 | 170 | return X_ref 171 | 172 | # dynamic constraints: \dot{x} = A_{c}x + B_{c}u 173 | def _generate_state_space_model(self): 174 | # Ac (13 * 13), Bc (13 * 12) 175 | Ac = np.zeros((self.num_state, self.num_state), dtype=np.float32) 176 | Bc = np.zeros((self.num_state, self.num_input), dtype=np.float32) 177 | 178 | Rz = np.array([[np.cos(self.yaw), -np.sin(self.yaw), 0], 179 | [np.sin(self.yaw), np.cos(self.yaw), 0], 180 | [0, 0, 1]], dtype=np.float32) 181 | # Rz = self.__robot_data.R_base 182 | world_I = Rz @ self.base_inertia_base @ Rz.T 183 | 184 | Ac[0:3, 6:9] = Rz.T 185 | Ac[3:6, 9:12] = np.identity(3, dtype=np.float32) 186 | Ac[11, 12] = 1.0 187 | 188 | for i in range(4): 189 | Bc[6:9, 3*i:3*i+3] = np.linalg.inv(world_I) @ vec2so3(self.pos_base_feet[i]) 190 | Bc[9:12, 3*i:3*i+3] = np.identity(3, dtype=np.float32) / self.mass 191 | 192 | return Ac, Bc 193 | 194 | def _discretize_continuous_model(self, Ac, Bc): 195 | # square_matrix = [[Ac (13*13), Bc (13*12)], 196 | # [0 (12*13), 0 (12*12)]] * dt (25*25) 197 | dim = self.num_state + self.num_input 198 | square_matrix = np.zeros((dim, dim), dtype=np.float32) 199 | square_matrix[0:self.num_state, 0:self.num_state] = Ac * self.dt 200 | square_matrix[0:self.num_state, self.num_state:dim] = Bc * self.dt 201 | 202 | # [[Ac, Bc], [[Ad, Bd], 203 | # exp( [ 0, 0]] * dt) = [ 0, I]] 204 | matrix_exponential = expm(square_matrix) 205 | Ad = matrix_exponential[0:self.num_state, 0:self.num_state] 206 | Bd = matrix_exponential[0:self.num_state, self.num_state:dim] 207 | 208 | return Ad, Bd 209 | 210 | # @jit(nopython=False) 211 | def _generate_QP_cost(self, Ad, Bd, xt, Xref, debug=False): 212 | # power_of_A = [A, A^2, A^3, ..., A^k] 213 | power_of_A = [np.identity(self.num_state, dtype=np.float32)] 214 | for i in range(self.horizon): 215 | power_of_A.append(power_of_A[i] @ Ad) 216 | 217 | Sx = np.zeros((self.num_state * self.horizon, self.num_state), dtype=np.float32) 218 | Su = np.zeros((self.num_state * self.horizon, self.num_input * self.horizon), dtype=np.float32) 219 | 220 | if debug: 221 | self.Sx = Sx 222 | self.Su = Su 223 | self.xt = xt 224 | 225 | for i in range(self.horizon): 226 | Sx[self.num_state*i:self.num_state*(i+1), 0:self.num_state] = power_of_A[i+1] 227 | 228 | for j in range(self.horizon): 229 | if i >= j: 230 | Su[self.num_state*i:self.num_state*(i+1), self.num_input*j:self.num_input*(j+1)] = power_of_A[i-j] @ Bd 231 | 232 | qp_H = 2 * (Su.T @ self.Qbar @ Su + self.Rbar) 233 | qp_g = 2 * Su.T @ self.Qbar @ (Sx @ xt - Xref) 234 | 235 | return qp_H, qp_g 236 | 237 | def _generate_QP_constraints(self, gait_table): 238 | # friction cone constraint for one foot 239 | constraint_coef_matrix = np.array([ 240 | [ 1, 0, self.mu], 241 | [-1, 0, self.mu], 242 | [ 0, 1, self.mu], 243 | [ 0, -1, self.mu], 244 | [ 0, 0, 1] 245 | ], dtype=np.float32) 246 | qp_C = np.kron(np.identity(4 * self.horizon, dtype=np.float32), constraint_coef_matrix) 247 | 248 | C_lb = np.zeros(4 * 5 * self.horizon, dtype=np.float32) 249 | C_ub = np.zeros(4 * 5 * self.horizon, dtype=np.float32) 250 | k = 0 251 | for i in range(self.horizon): 252 | for j in range(4): # number of legs 253 | C_ub[5*k] = np.inf 254 | C_ub[5*k+1] = np.inf 255 | C_ub[5*k+2] = np.inf 256 | C_ub[5*k+3] = np.inf 257 | C_ub[5*k+4] = gait_table[4*i+j] * self.fz_max 258 | k += 1 259 | 260 | return qp_C, C_lb, C_ub 261 | 262 | def _solve_mpc(self, ref_traj, gait_table, solver='drake', debug=False): 263 | 264 | assert solver == 'drake' or solver == 'qpsolvers' 265 | 266 | Ac, Bc = self._generate_state_space_model() 267 | self._discretize_continuous_model(Ac, Bc) 268 | Ad, Bd = self._discretize_continuous_model(Ac, Bc) 269 | # start_time = time.time() 270 | qpH, qpg = self._generate_QP_cost(Ad, Bd, self.current_state, ref_traj, debug=debug) 271 | # end_time = time.time() 272 | # print('QP cost generated in {:3f}s.'.format(end_time - start_time)) 273 | 274 | # constraint_coef_matrix, lb, ub = self._generate_force_constraints(gait_table) 275 | qp_C, C_lb, C_ub = self._generate_QP_constraints(gait_table) 276 | 277 | if solver == 'drake': 278 | qp_problem = MathematicalProgram() 279 | contact_forces = qp_problem.NewContinuousVariables(self.num_input*self.horizon, 'contact_forces') 280 | 281 | qp_problem.AddQuadraticCost(qpH, qpg, contact_forces) 282 | qp_problem.AddLinearConstraint(qp_C, C_lb, C_ub, contact_forces) 283 | 284 | result = Solve(qp_problem) 285 | 286 | return result.GetSolution(contact_forces) 287 | 288 | else: 289 | contact_forces = solve_qp(P=qpH, q=qpg, G=qp_C, h=C_ub, A=None, b=None, solver='osqp') 290 | return contact_forces 291 | 292 | 293 | def __visulize_com_traj_solution(self, contact_forces_debug): 294 | com_traj = self.Sx @ self.xt + self.Su @ contact_forces_debug 295 | 296 | trajs = [] 297 | for i in range(12): 298 | trajs.append([]) 299 | 300 | for i in range(self.horizon): 301 | for j in range(12): 302 | trajs[j].append(com_traj[13*i+j]) 303 | 304 | x = range(self.horizon) 305 | # plt.plot(x, trajs[0], label='roll') 306 | # plt.plot(x, trajs[1], label='pitch') 307 | # plt.plot(x, trajs[2], label='yaw') 308 | plt.plot(x, trajs[3], label='x') 309 | plt.plot(x, trajs[4], label='y') 310 | plt.plot(x, trajs[5], label='z') 311 | # plt.plot(x, trajs[6], label='wx') 312 | # plt.plot(x, trajs[7], label='wy') 313 | # plt.plot(x, trajs[8], label='wz') 314 | # plt.plot(x, trajs[9], label='vx') 315 | # plt.plot(x, trajs[10], label='vy') 316 | # plt.plot(x, trajs[11], label='vz') 317 | plt.legend() 318 | plt.show() 319 | -------------------------------------------------------------------------------- /robot/a1/a1_license.txt: -------------------------------------------------------------------------------- 1 | Mozilla Public License Version 2.0 2 | ================================== 3 | 4 | 1. Definitions 5 | -------------- 6 | 7 | 1.1. "Contributor" 8 | means each individual or legal entity that creates, contributes to 9 | the creation of, or owns Covered Software. 10 | 11 | 1.2. "Contributor Version" 12 | means the combination of the Contributions of others (if any) used 13 | by a Contributor and that particular Contributor's Contribution. 14 | 15 | 1.3. "Contribution" 16 | means Covered Software of a particular Contributor. 17 | 18 | 1.4. "Covered Software" 19 | means Source Code Form to which the initial Contributor has attached 20 | the notice in Exhibit A, the Executable Form of such Source Code 21 | Form, and Modifications of such Source Code Form, in each case 22 | including portions thereof. 23 | 24 | 1.5. "Incompatible With Secondary Licenses" 25 | means 26 | 27 | (a) that the initial Contributor has attached the notice described 28 | in Exhibit B to the Covered Software; or 29 | 30 | (b) that the Covered Software was made available under the terms of 31 | version 1.1 or earlier of the License, but not also under the 32 | terms of a Secondary License. 33 | 34 | 1.6. "Executable Form" 35 | means any form of the work other than Source Code Form. 36 | 37 | 1.7. "Larger Work" 38 | means a work that combines Covered Software with other material, in 39 | a separate file or files, that is not Covered Software. 40 | 41 | 1.8. "License" 42 | means this document. 43 | 44 | 1.9. "Licensable" 45 | means having the right to grant, to the maximum extent possible, 46 | whether at the time of the initial grant or subsequently, any and 47 | all of the rights conveyed by this License. 48 | 49 | 1.10. "Modifications" 50 | means any of the following: 51 | 52 | (a) any file in Source Code Form that results from an addition to, 53 | deletion from, or modification of the contents of Covered 54 | Software; or 55 | 56 | (b) any new file in Source Code Form that contains any Covered 57 | Software. 58 | 59 | 1.11. "Patent Claims" of a Contributor 60 | means any patent claim(s), including without limitation, method, 61 | process, and apparatus claims, in any patent Licensable by such 62 | Contributor that would be infringed, but for the grant of the 63 | License, by the making, using, selling, offering for sale, having 64 | made, import, or transfer of either its Contributions or its 65 | Contributor Version. 66 | 67 | 1.12. "Secondary License" 68 | means either the GNU General Public License, Version 2.0, the GNU 69 | Lesser General Public License, Version 2.1, the GNU Affero General 70 | Public License, Version 3.0, or any later versions of those 71 | licenses. 72 | 73 | 1.13. "Source Code Form" 74 | means the form of the work preferred for making modifications. 75 | 76 | 1.14. "You" (or "Your") 77 | means an individual or a legal entity exercising rights under this 78 | License. For legal entities, "You" includes any entity that 79 | controls, is controlled by, or is under common control with You. For 80 | purposes of this definition, "control" means (a) the power, direct 81 | or indirect, to cause the direction or management of such entity, 82 | whether by contract or otherwise, or (b) ownership of more than 83 | fifty percent (50%) of the outstanding shares or beneficial 84 | ownership of such entity. 85 | 86 | 2. License Grants and Conditions 87 | -------------------------------- 88 | 89 | 2.1. Grants 90 | 91 | Each Contributor hereby grants You a world-wide, royalty-free, 92 | non-exclusive license: 93 | 94 | (a) under intellectual property rights (other than patent or trademark) 95 | Licensable by such Contributor to use, reproduce, make available, 96 | modify, display, perform, distribute, and otherwise exploit its 97 | Contributions, either on an unmodified basis, with Modifications, or 98 | as part of a Larger Work; and 99 | 100 | (b) under Patent Claims of such Contributor to make, use, sell, offer 101 | for sale, have made, import, and otherwise transfer either its 102 | Contributions or its Contributor Version. 103 | 104 | 2.2. Effective Date 105 | 106 | The licenses granted in Section 2.1 with respect to any Contribution 107 | become effective for each Contribution on the date the Contributor first 108 | distributes such Contribution. 109 | 110 | 2.3. Limitations on Grant Scope 111 | 112 | The licenses granted in this Section 2 are the only rights granted under 113 | this License. No additional rights or licenses will be implied from the 114 | distribution or licensing of Covered Software under this License. 115 | Notwithstanding Section 2.1(b) above, no patent license is granted by a 116 | Contributor: 117 | 118 | (a) for any code that a Contributor has removed from Covered Software; 119 | or 120 | 121 | (b) for infringements caused by: (i) Your and any other third party's 122 | modifications of Covered Software, or (ii) the combination of its 123 | Contributions with other software (except as part of its Contributor 124 | Version); or 125 | 126 | (c) under Patent Claims infringed by Covered Software in the absence of 127 | its Contributions. 128 | 129 | This License does not grant any rights in the trademarks, service marks, 130 | or logos of any Contributor (except as may be necessary to comply with 131 | the notice requirements in Section 3.4). 132 | 133 | 2.4. Subsequent Licenses 134 | 135 | No Contributor makes additional grants as a result of Your choice to 136 | distribute the Covered Software under a subsequent version of this 137 | License (see Section 10.2) or under the terms of a Secondary License (if 138 | permitted under the terms of Section 3.3). 139 | 140 | 2.5. Representation 141 | 142 | Each Contributor represents that the Contributor believes its 143 | Contributions are its original creation(s) or it has sufficient rights 144 | to grant the rights to its Contributions conveyed by this License. 145 | 146 | 2.6. Fair Use 147 | 148 | This License is not intended to limit any rights You have under 149 | applicable copyright doctrines of fair use, fair dealing, or other 150 | equivalents. 151 | 152 | 2.7. Conditions 153 | 154 | Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted 155 | in Section 2.1. 156 | 157 | 3. Responsibilities 158 | ------------------- 159 | 160 | 3.1. Distribution of Source Form 161 | 162 | All distribution of Covered Software in Source Code Form, including any 163 | Modifications that You create or to which You contribute, must be under 164 | the terms of this License. You must inform recipients that the Source 165 | Code Form of the Covered Software is governed by the terms of this 166 | License, and how they can obtain a copy of this License. You may not 167 | attempt to alter or restrict the recipients' rights in the Source Code 168 | Form. 169 | 170 | 3.2. Distribution of Executable Form 171 | 172 | If You distribute Covered Software in Executable Form then: 173 | 174 | (a) such Covered Software must also be made available in Source Code 175 | Form, as described in Section 3.1, and You must inform recipients of 176 | the Executable Form how they can obtain a copy of such Source Code 177 | Form by reasonable means in a timely manner, at a charge no more 178 | than the cost of distribution to the recipient; and 179 | 180 | (b) You may distribute such Executable Form under the terms of this 181 | License, or sublicense it under different terms, provided that the 182 | license for the Executable Form does not attempt to limit or alter 183 | the recipients' rights in the Source Code Form under this License. 184 | 185 | 3.3. Distribution of a Larger Work 186 | 187 | You may create and distribute a Larger Work under terms of Your choice, 188 | provided that You also comply with the requirements of this License for 189 | the Covered Software. If the Larger Work is a combination of Covered 190 | Software with a work governed by one or more Secondary Licenses, and the 191 | Covered Software is not Incompatible With Secondary Licenses, this 192 | License permits You to additionally distribute such Covered Software 193 | under the terms of such Secondary License(s), so that the recipient of 194 | the Larger Work may, at their option, further distribute the Covered 195 | Software under the terms of either this License or such Secondary 196 | License(s). 197 | 198 | 3.4. Notices 199 | 200 | You may not remove or alter the substance of any license notices 201 | (including copyright notices, patent notices, disclaimers of warranty, 202 | or limitations of liability) contained within the Source Code Form of 203 | the Covered Software, except that You may alter any license notices to 204 | the extent required to remedy known factual inaccuracies. 205 | 206 | 3.5. Application of Additional Terms 207 | 208 | You may choose to offer, and to charge a fee for, warranty, support, 209 | indemnity or liability obligations to one or more recipients of Covered 210 | Software. However, You may do so only on Your own behalf, and not on 211 | behalf of any Contributor. You must make it absolutely clear that any 212 | such warranty, support, indemnity, or liability obligation is offered by 213 | You alone, and You hereby agree to indemnify every Contributor for any 214 | liability incurred by such Contributor as a result of warranty, support, 215 | indemnity or liability terms You offer. You may include additional 216 | disclaimers of warranty and limitations of liability specific to any 217 | jurisdiction. 218 | 219 | 4. Inability to Comply Due to Statute or Regulation 220 | --------------------------------------------------- 221 | 222 | If it is impossible for You to comply with any of the terms of this 223 | License with respect to some or all of the Covered Software due to 224 | statute, judicial order, or regulation then You must: (a) comply with 225 | the terms of this License to the maximum extent possible; and (b) 226 | describe the limitations and the code they affect. Such description must 227 | be placed in a text file included with all distributions of the Covered 228 | Software under this License. Except to the extent prohibited by statute 229 | or regulation, such description must be sufficiently detailed for a 230 | recipient of ordinary skill to be able to understand it. 231 | 232 | 5. Termination 233 | -------------- 234 | 235 | 5.1. The rights granted under this License will terminate automatically 236 | if You fail to comply with any of its terms. However, if You become 237 | compliant, then the rights granted under this License from a particular 238 | Contributor are reinstated (a) provisionally, unless and until such 239 | Contributor explicitly and finally terminates Your grants, and (b) on an 240 | ongoing basis, if such Contributor fails to notify You of the 241 | non-compliance by some reasonable means prior to 60 days after You have 242 | come back into compliance. Moreover, Your grants from a particular 243 | Contributor are reinstated on an ongoing basis if such Contributor 244 | notifies You of the non-compliance by some reasonable means, this is the 245 | first time You have received notice of non-compliance with this License 246 | from such Contributor, and You become compliant prior to 30 days after 247 | Your receipt of the notice. 248 | 249 | 5.2. If You initiate litigation against any entity by asserting a patent 250 | infringement claim (excluding declaratory judgment actions, 251 | counter-claims, and cross-claims) alleging that a Contributor Version 252 | directly or indirectly infringes any patent, then the rights granted to 253 | You by any and all Contributors for the Covered Software under Section 254 | 2.1 of this License shall terminate. 255 | 256 | 5.3. In the event of termination under Sections 5.1 or 5.2 above, all 257 | end user license agreements (excluding distributors and resellers) which 258 | have been validly granted by You or Your distributors under this License 259 | prior to termination shall survive termination. 260 | 261 | ************************************************************************ 262 | * * 263 | * 6. Disclaimer of Warranty * 264 | * ------------------------- * 265 | * * 266 | * Covered Software is provided under this License on an "as is" * 267 | * basis, without warranty of any kind, either expressed, implied, or * 268 | * statutory, including, without limitation, warranties that the * 269 | * Covered Software is free of defects, merchantable, fit for a * 270 | * particular purpose or non-infringing. The entire risk as to the * 271 | * quality and performance of the Covered Software is with You. * 272 | * Should any Covered Software prove defective in any respect, You * 273 | * (not any Contributor) assume the cost of any necessary servicing, * 274 | * repair, or correction. This disclaimer of warranty constitutes an * 275 | * essential part of this License. No use of any Covered Software is * 276 | * authorized under this License except under this disclaimer. * 277 | * * 278 | ************************************************************************ 279 | 280 | ************************************************************************ 281 | * * 282 | * 7. Limitation of Liability * 283 | * -------------------------- * 284 | * * 285 | * Under no circumstances and under no legal theory, whether tort * 286 | * (including negligence), contract, or otherwise, shall any * 287 | * Contributor, or anyone who distributes Covered Software as * 288 | * permitted above, be liable to You for any direct, indirect, * 289 | * special, incidental, or consequential damages of any character * 290 | * including, without limitation, damages for lost profits, loss of * 291 | * goodwill, work stoppage, computer failure or malfunction, or any * 292 | * and all other commercial damages or losses, even if such party * 293 | * shall have been informed of the possibility of such damages. This * 294 | * limitation of liability shall not apply to liability for death or * 295 | * personal injury resulting from such party's negligence to the * 296 | * extent applicable law prohibits such limitation. Some * 297 | * jurisdictions do not allow the exclusion or limitation of * 298 | * incidental or consequential damages, so this exclusion and * 299 | * limitation may not apply to You. * 300 | * * 301 | ************************************************************************ 302 | 303 | 8. Litigation 304 | ------------- 305 | 306 | Any litigation relating to this License may be brought only in the 307 | courts of a jurisdiction where the defendant maintains its principal 308 | place of business and such litigation shall be governed by laws of that 309 | jurisdiction, without reference to its conflict-of-law provisions. 310 | Nothing in this Section shall prevent a party's ability to bring 311 | cross-claims or counter-claims. 312 | 313 | 9. Miscellaneous 314 | ---------------- 315 | 316 | This License represents the complete agreement concerning the subject 317 | matter hereof. If any provision of this License is held to be 318 | unenforceable, such provision shall be reformed only to the extent 319 | necessary to make it enforceable. Any law or regulation which provides 320 | that the language of a contract shall be construed against the drafter 321 | shall not be used to construe this License against a Contributor. 322 | 323 | 10. Versions of the License 324 | --------------------------- 325 | 326 | 10.1. New Versions 327 | 328 | Mozilla Foundation is the license steward. Except as provided in Section 329 | 10.3, no one other than the license steward has the right to modify or 330 | publish new versions of this License. Each version will be given a 331 | distinguishing version number. 332 | 333 | 10.2. Effect of New Versions 334 | 335 | You may distribute the Covered Software under the terms of the version 336 | of the License under which You originally received the Covered Software, 337 | or under the terms of any subsequent version published by the license 338 | steward. 339 | 340 | 10.3. Modified Versions 341 | 342 | If you create software not governed by this License, and you want to 343 | create a new license for such software, you may create and use a 344 | modified version of this License if you rename the license and remove 345 | any references to the name of the license steward (except to note that 346 | such modified license differs from this License). 347 | 348 | 10.4. Distributing Source Code Form that is Incompatible With Secondary 349 | Licenses 350 | 351 | If You choose to distribute Source Code Form that is Incompatible With 352 | Secondary Licenses under the terms of this version of the License, the 353 | notice described in Exhibit B of this License must be attached. 354 | 355 | Exhibit A - Source Code Form License Notice 356 | ------------------------------------------- 357 | 358 | This Source Code Form is subject to the terms of the Mozilla Public 359 | License, v. 2.0. If a copy of the MPL was not distributed with this 360 | file, You can obtain one at http://mozilla.org/MPL/2.0/. 361 | 362 | If it is not possible or desirable to put the notice in a particular 363 | file, then You may include the notice in a location (such as a LICENSE 364 | file in a relevant directory) where a recipient would be likely to look 365 | for such a notice. 366 | 367 | You may add additional accurate notices of copyright ownership. 368 | 369 | Exhibit B - "Incompatible With Secondary Licenses" Notice 370 | --------------------------------------------------------- 371 | 372 | This Source Code Form is "Incompatible With Secondary Licenses", as 373 | defined by the Mozilla Public License, v. 2.0. 374 | -------------------------------------------------------------------------------- /robot/a1/urdf/a1.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 572 | 573 | 574 | 575 | 576 | 577 | 578 | 579 | -------------------------------------------------------------------------------- /robot/aliengo/aliengo.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 263 | -------------------------------------------------------------------------------- /doc/linear_mpc.md: -------------------------------------------------------------------------------- 1 | # Linear Model Predictive Controller 2 | 3 | ### Reference 4 | 5 | [1] Di Carlo J., Wensing P. M., Katz B., et al. Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 1-9. [[PDF]](https://dspace.mit.edu/bitstream/handle/1721.1/138000/convex_mpc_2fix.pdf?sequence=2&isAllowed=y) 6 | 7 | 8 | [2] Bledt G, Powell M J, Katz B, et al. Mit cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 2245-2252. [[PDF]](https://dspace.mit.edu/bitstream/handle/1721.1/126619/IROS.pdf?sequence=2&isAllowed=y) 9 | 10 | 11 | ### Overview 12 | 13 | ## 1. Dynamic Constraints 14 | ### a. Approximated Angular Velocity Dynamics 15 | The robot's orientation is expressed as a vector of Z-Y-X Euler angles $\Theta = [\phi, \theta, \psi]^\text{T}$, where $\psi$ is the yaw, $\theta$ is the pitch, and $\phi$ is the roll. These angles correspond to a sequence of rotations such that the transform from body to world coordinates can be expressed as 16 | 17 | $$ 18 | \mathbf{R} = \mathbf{R}_z(\psi)\mathbf{R}_y(\theta)\mathbf{R}_x(\phi) 19 | $$ 20 | 21 | where $\mathbf{R}_n(\alpha)$ represents a positive rotation of $\alpha$ about the $n$-axis. In details, we can write 22 | 23 | $$ 24 | \mathbf{R}_z(\psi) = \begin{bmatrix} 25 | \cos\psi & -\sin \psi & 0 \\ 26 | \sin\psi & \cos \psi & 0 \\ 27 | 0 & 0 & 1 28 | \end{bmatrix}, 29 | \mathbf{R}_y(\theta) = \begin{bmatrix} 30 | \cos\theta & 0 & \sin\theta \\ 31 | 0 & 1 & 0 \\ 32 | -\sin\theta & 0 & \cos\theta 33 | \end{bmatrix}, 34 | \mathbf{R}_z(\psi) = \begin{bmatrix} 35 | \cos\psi & -\sin \psi & 0 \\ 36 | \sin\psi & \cos \psi & 0 \\ 37 | 0 & 0 & 1 38 | \end{bmatrix} 39 | $$ 40 | 41 | From [2], we have known that $\dot{\mathbf{R}} = [\mathbf{\omega}]\mathbf{R}$, where $\mathbf{\omega} \in \mathbb{R}^3$ is the robot's angular velocity, $[\mathbf{\omega}] \in \mathbb{R}^{3\times3}$ is defined as the skew-symmetric matrix with respect to $\mathbf{\omega}$, and $\mathbf{R}$ is the rotation matrix which transforms from body to world coordinates. Then the angular velocity in world coordinates can be found with 42 | 43 | $$ 44 | \begin{aligned} 45 | [\mathbf{\omega}] &= \dot{\mathbf{R}}\mathbf{R}^{-1} = \dot{\mathbf{R}}\mathbf{R}^\text{T} \\ 46 | &= \left( \frac{\partial R}{\partial \psi} + \frac{\partial R}{\partial \theta} + \frac{\partial R}{\partial \phi} \right)\mathbf{R}^\text{T} \\ 47 | &= \begin{bmatrix} 48 | 0 & \dot{\phi} \sin\theta - \dot{\psi} & \dot{\theta}\cos\psi + \dot{\phi}\cos\theta\sin\psi \\ 49 | -\dot{\phi} \sin\theta + \dot{\psi} & 0 & \dot{\theta}\sin\psi - \dot{\phi}\cos\psi\cos\theta \\ 50 | -\dot{\theta}\cos\psi - \dot{\phi}\cos\theta\sin\psi & -\dot{\theta}\sin\psi + \dot{\phi}\cos\psi\cos\theta & 0 51 | \end{bmatrix} 52 | \end{aligned} 53 | $$ 54 | 55 | The above result is easy to get using the MATLAB script. 56 | ~~~matlab 57 | clc; 58 | clear; 59 | 60 | syms psi theta phi real 61 | syms psidot thetadot phidot real 62 | 63 | Rz = [cos(psi), -sin(psi), 0; 64 | sin(psi), cos(psi), 0; 65 | 0, 0, 1]; 66 | 67 | Ry = [cos(theta), 0, sin(theta); 68 | 0, 1, 0; 69 | -sin(theta), 0, cos(theta)]; 70 | 71 | Rx = [1, 0, 0; 72 | 0, cos(phi), -sin(phi); 73 | 0, sin(phi), cos(phi)]; 74 | 75 | R = simplify(Rz*Ry*Rx); 76 | 77 | Somega = simplify((diff(R,phi)*phidot + diff(R,theta)*thetadot + diff(R,psi)*psidot) * R') 78 | ~~~ 79 | 80 | Now we are ready to build connections between the angular velocity in world coordinates $\mathbf{\omega}$ and the rate of change of Euler angles $\dot{\mathbf{\Theta}} = [\dot{\phi}, \dot{\theta}, \dot{\psi}]^\text{T}$. 81 | 82 | $$ 83 | \mathbf{\omega} = \begin{bmatrix} 84 | -\dot{\theta}\sin\psi + \dot{\phi}\cos\psi\cos\theta \\ 85 | \dot{\theta}\cos\psi + \dot{\phi}\cos\theta\sin\psi \\ 86 | -\dot{\phi} \sin\theta + \dot{\psi} 87 | \end{bmatrix} 88 | = \begin{bmatrix} 89 | \cos\theta\cos\psi & -\sin\psi & 0 \\ 90 | \cos\theta\sin\psi & \cos\psi & 0 \\ 91 | -\sin\theta & 0 & 1 92 | \end{bmatrix}\begin{bmatrix} 93 | \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} 94 | \end{bmatrix} = \mathbf{E}\dot{\mathbf{\Theta}} 95 | $$ 96 | 97 | If the robot is not pointed vertically, which means $\cos\theta \neq 0$, the matrix $\mathbf{E}$ is invertable. In such case, we can get 98 | 99 | $$ 100 | \dot{\mathbf{\Theta}} = \mathbf{E}^{-1}\mathbf{\omega} = \begin{bmatrix} 101 | \frac{\cos\psi}{\cos\theta} & \frac{\sin\psi}{\cos\theta} & 0 \\ 102 | -\sin\psi & \cos\psi & 0 \\ 103 | \cos\psi\tan\theta & \sin\psi\tan\theta & 1 104 | \end{bmatrix}\mathbf{\omega} 105 | $$ 106 | 107 | For small values of roll $\phi$ and pitch $\theta$, the above equation can be approximated as 108 | 109 | $$ 110 | \dot{\mathbf{\Theta}} \approx \begin{bmatrix} 111 | \cos\psi & \sin\psi & 0 \\ 112 | -\sin\psi & \cos\psi & 0 \\ 113 | 0 & 0 & 1 114 | \end{bmatrix} \mathbf{\omega} 115 | $$ 116 | 117 | which is equivalent to 118 | 119 | $$ 120 | \dot{\mathbf{\Theta}} \approx \mathbf{R}_z^\text{T} \mathbf{\omega} 121 | \tag{1} 122 | $$ 123 | 124 | *Note that the order in which the Euler angle rotations are defined is important; with an alternate sequence of rotations, the approximation will be inaccurate for reasonable robot orientations.* 125 | 126 | 127 | ### b. Simplified Single Rigid Body Model 128 | The predictive controller models the robot as a **single rigid body** subject to forces at the contact patches. Although *ignoring leg dynamics is a major simplification*, the controller is still able to stabilize a high-DoF system and is robust to these multi-body effects. 129 | 130 | For the Cheetah 3 robot, this simplification is **reasonable**: the mass of the legs is roughly 10% of the robot's total mass. 131 | 132 | For each ground reaction force $\mathbf{f}_i \in \mathbb{R}^3$, the vector from the CoM to the point where the force is applied is $\mathbf{r}_i \in \mathbb{R}^3$. The rigid body dynamics in world coordinates are given by 133 | 134 | $$ 135 | \begin{aligned} 136 | \ddot{\mathbf{p}} &= \frac{\sum_{i=1}^n\mathbf{f}_i}{m} - \mathbf{g} \\ 137 | \frac{\text{d}}{\text{d}t}(\mathbf{I\omega}) &= \sum_{i=1}^n \mathbf{r}_i \times \mathbf{f}_i\\ 138 | \dot{\mathbf{R}} &= [\mathbf{\omega}]\mathbf{R} 139 | \end{aligned} \tag{2} 140 | $$ 141 | 142 | where $\mathbf{p} \in \mathbb{R}^3$ is the robot's position in world frame, $m \in \mathbb{R}$ is the robot's mass, $\mathbf{g} \in \mathbb{R}^3$ is the acceleration of gravity, and $\mathbf{I} \in \mathbb{R}^3$ is the robot's inertia tensor. *The nonlinear dynamics in the second and third equation of (2) motivate the approximations to avoid the nonconvex optimization that would otherwise be required for model predictive control.* 143 | 144 | The second equation in (2) can be approximated with: 145 | 146 | $$ 147 | \frac{\text{d}}{\text{d}t}(\mathbf{I\omega}) = \mathbf{I\dot{\omega}} + \omega \times (\mathbf{I\omega}) \approx \mathbf{I\dot{\omega}} = \sum_{i=1}^n \mathbf{r}_i \times \mathbf{f}_i \tag{3} 148 | $$ 149 | 150 | This approximation has been made in other people's work. *The $\omega \times (\mathbf{I\omega})$ term is small for bodies with small angular velocities and does not contribute significantly to the dynamics of the robot.* The inertia tensor in the world coordinate system can be found with 151 | 152 | $$ 153 | \mathbf{I} = \mathbf{R}\mathbf{I}_{\mathcal{B}}\mathbf{R}^\text{T} 154 | $$ 155 | 156 | where $\mathbf{I}_\mathcal{B}$ is the inertia tensor in body coordinates. For small roll and pitch angles, This can be approximated by 157 | 158 | $$ 159 | \mathbf{\hat{I}} = \mathbf{R}_z(\psi)\mathbf{I}_{\mathcal{B}}\mathbf{R}_z(\psi)^\text{T} 160 | \tag{4} 161 | $$ 162 | 163 | where $\mathbf{\hat{I}}$ is the approximated robot's inertia tensor in world frame. Combining equations (3)(4), we get 164 | 165 | $$ 166 | \mathbf{\dot{\omega}} = \mathbf{\hat{I}}^{-1}\sum_{i=1}^n \mathbf{r}_i \times \mathbf{f}_i = \mathbf{\hat{I}}^{-1}\sum_{i=1}^n [\mathbf{r}_i] \mathbf{f}_i 167 | \tag{5} 168 | $$ 169 | 170 | For the third equation of (2), we have made the approximation in section 1.a, which gives us 171 | 172 | $$ 173 | \dot{\mathbf{\Theta}} \approx \mathbf{R}_z^\text{T} \mathbf{\omega} 174 | $$ 175 | 176 | 177 | ### c. Continuous-Time State Space Model 178 | From the discussion above, we can write the simplified single rigid body model using equations (1)(2) 179 | 180 | $$ 181 | \begin{aligned} 182 | \dot{\mathbf{\Theta}} &= \mathbf{R}_z^\text{T} \mathbf{\omega} \\ 183 | \dot{\mathbf{p}} &= \dot{\mathbf{p}} \\ 184 | \mathbf{\dot{\omega}} &= \mathbf{\hat{I}}^{-1}\sum_{i=1}^n [\mathbf{r}_i] \mathbf{f}_i \\ 185 | \ddot{\mathbf{p}} &= \frac{\sum_{i=1}^n\mathbf{f}_i}{m} - \mathbf{g} 186 | \end{aligned} 187 | \tag{6} 188 | $$ 189 | 190 | In matrix form: 191 | 192 | $$ 193 | \begin{bmatrix} 194 | \mathbf{\dot{\Theta}} \\ \mathbf{\dot{p}} \\ \mathbf{\dot{\omega}} \\ \mathbf{\ddot{p}} 195 | \end{bmatrix} 196 | = 197 | \begin{bmatrix} 198 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{R}_z^\text{T} & \mathbf{0}_3 \\ 199 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 \\ 200 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ 201 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 202 | \end{bmatrix} 203 | \begin{bmatrix} 204 | \mathbf{\Theta} \\ \mathbf{p} \\ \mathbf{\omega} \\ \mathbf{\dot{p}} 205 | \end{bmatrix} + 206 | \begin{bmatrix} 207 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ 208 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ 209 | \mathbf{\hat{I}}^{-1}[\mathbf{r}_1] & \mathbf{\hat{I}}^{-1}[\mathbf{r}_1] & \mathbf{\hat{I}}^{-1}[\mathbf{r}_1] & \mathbf{\hat{I}}^{-1}[\mathbf{r}_1]\\ 210 | \frac{\mathbf{I}_3}{m} & \frac{\mathbf{I}_3}{m} & \frac{\mathbf{I}_3}{m} & \frac{\mathbf{I}_3}{m} 211 | \end{bmatrix} 212 | \begin{bmatrix} 213 | \mathbf{f}_1 \\ \mathbf{f}_2 \\ \mathbf{f}_3 \\ \mathbf{f}_4 214 | \end{bmatrix} + 215 | \begin{bmatrix} 216 | \mathbf{0}_{31} \\ \mathbf{0}_{31} \\ \mathbf{0}_{31} \\ -\mathbf{g} 217 | \end{bmatrix} 218 | $$ 219 | 220 | This equation can be rewritten with an additional gravity state $g$ (*note that here $g$ is a scalar*) to put the dynamics into the convenient state-space form: 221 | 222 | $$ 223 | \dot{\mathbf{x}}(t) = \mathbf{A_c}(\psi)\mathbf{x}(t) + \mathbf{B_c}(\mathbf{r}_1, \cdots, \mathbf{r}_4, \psi)\mathbf{u}(t) 224 | \tag{7} 225 | $$ 226 | 227 | where $\mathbf{A_c} \in \mathbb{R}^{13\times13}$ and $\mathbf{B_c} \in \mathbb{R}^{13\times12}$. 228 | In details, we have 229 | 230 | $$ 231 | \begin{bmatrix} 232 | \mathbf{\dot{\Theta}} \\ \mathbf{\dot{p}} \\ \mathbf{\dot{\omega}} \\ \mathbf{\ddot{p}} \\ -\dot{g} 233 | \end{bmatrix} = \begin{bmatrix} 234 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{R}_z^\text{T} & \mathbf{0}_3 & 0\\ 235 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & 0\\ 236 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & 0\\ 237 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{e}_z\\ 238 | 0 & 0 & 0 & 0 & 0 239 | \end{bmatrix} 240 | \begin{bmatrix} 241 | \mathbf{\Theta} \\ \mathbf{p} \\ \mathbf{\omega} \\ \mathbf{\dot{p}} \\ -g 242 | \end{bmatrix} + 243 | \begin{bmatrix} 244 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ 245 | \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ 246 | \mathbf{\hat{I}}^{-1}[\mathbf{r}_1] & \mathbf{\hat{I}}^{-1}[\mathbf{r}_1] & \mathbf{\hat{I}}^{-1}[\mathbf{r}_1] & \mathbf{\hat{I}}^{-1}[\mathbf{r}_1]\\ 247 | \frac{\mathbf{I}_3}{m} & \frac{\mathbf{I}_3}{m} & \frac{\mathbf{I}_3}{m} & \frac{\mathbf{I}_3}{m} \\ 248 | 0 & 0 & 0 & 0 249 | \end{bmatrix} 250 | \begin{bmatrix} 251 | \mathbf{f}_1 \\ \mathbf{f}_2 \\ \mathbf{f}_3 \\ \mathbf{f}_4 252 | \end{bmatrix} 253 | \tag{8} 254 | $$ 255 | 256 | *This form depends only on yaw and footstep locations.* If these can be computed ahead of time, the dynamics become linear time-varying, which is suitable for convex model predictive control. 257 | 258 | 259 | ### d. Discretization 260 | See https://en.wikipedia.org/wiki/Discretization for more details about the purposed method. 261 | 262 | $$ 263 | \exp\left(\begin{bmatrix} 264 | \mathbf{A_c} & \mathbf{B_c} \\ 265 | \mathbf{0} & \mathbf{0} 266 | \end{bmatrix} \text{dt}\right) = 267 | \begin{bmatrix} 268 | \mathbf{A_d} & \mathbf{B_d} \\ 269 | \mathbf{0} & \mathbf{I} 270 | \end{bmatrix} \tag{9} 271 | $$ 272 | 273 | This allows us to express the dynamics in the discrete time form 274 | 275 | $$ 276 | \mathbf{x}[k+1] = \mathbf{A_d} \mathbf{x}[k] + \mathbf{B_d}[k]\mathbf{u}[k] 277 | \tag{10} 278 | $$ 279 | 280 | *The above approximation is only accurate if the robot is able to follow the reference trajectory*. Large deviations from the reference trajectory, possibly caused by external or terrain disturbances, will result in $\mathbf{B_d}[k]$ being inaccurate. However, for the first time step, $\mathbf{B_d}[k]$ is calculated from the current robot state, and will always be correct. *If, at any point, the robot is disturbed from following the reference trajectory, the next iteration of the MPC, which happens at most 40 ms after the disturbance, will recompute the reference trajectory based on the disturbed robot state, allowing it compensate for a disturbance.* 281 | 282 | ## 2. Force Constraints 283 | ### a. Equality Constraints 284 | The equality constraint 285 | 286 | $$ 287 | \mathbf{D}_k \mathbf{u}_k = \mathbf{0} \tag{11} 288 | $$ 289 | 290 | is used to set all forces from feet off the ground to zero, enforcing the desired gait, where $\mathbf{D}_k$ is a matrix which selects forces corresponding with feet not in contact with the ground at timestep $k$. 291 | 292 | ### b. Inequality Constraints 293 | The inequality constraints limit the minimum and maximum $z$-force as well as a square pyramid approximation of the friction cone. 294 | 295 | For each foot, we have the following 10 inequality constraints ($i = 1,2,3,4$). 296 | 297 | $$ 298 | \begin{aligned} 299 | f_{\min} \leq &f_{i,z} \leq f_{\max} \\ 300 | -\mu f_{i,z} \leq &f_{i,x} \leq \mu f_{i,z} \\ 301 | -\mu f_{i,z} \leq &f_{i,y} \leq \mu f_{i,z} 302 | \end{aligned} 303 | $$ 304 | 305 | We want to write these constraints in matrix form. Thus we need to look these equations in detail. For example, the constraints $-\mu f_{i,z} \leq \pm f_{i,x} \leq \mu f_{i,z}$ actually are 306 | 307 | $$ 308 | \begin{aligned} 309 | -\mu f_{i,z} &\leq f_{i,x} \\ 310 | -\mu f_{i,z} &\leq -f_{i,x} \\ 311 | f_{i,x} &\leq \mu f_{i,z} \\ 312 | -f_{i,x} &\leq \mu f_{i,z} 313 | \end{aligned} 314 | $$ 315 | 316 | We can rewrite these equations as 317 | 318 | $$ 319 | \begin{aligned} 320 | f_{i,x} + \mu f_{i,z} &\geq 0 \\ 321 | -f_{i,x} + \mu f_{i,z} &\geq 0 \\ 322 | f_{i,x} - \mu f_{i,z} &\leq 0 \\ 323 | -f_{i,x} - \mu f_{i,z} &\leq 0 324 | \end{aligned} 325 | $$ 326 | 327 | We can see that the first two equations and the last two equations are the same. Thus we can use the first two equations to replace these four equations. We can also note that $f_{\min} = 0$ as always. With the observation discussed above, we can write the force constraints for one foot **on the ground** as 328 | 329 | $$ 330 | \begin{bmatrix} 331 | 0 \\ 0 \\ 0 \\ 0 \\ 0 332 | \end{bmatrix} 333 | \leq 334 | \begin{bmatrix} 335 | 1 & 0 & \mu \\ 336 | -1 & 0 & \mu \\ 337 | 0 & 1 & \mu \\ 338 | 0 & -1 & \mu \\ 339 | 0 & 0 & 1 340 | \end{bmatrix} 341 | \begin{bmatrix} 342 | f_{i,x} \\ f_{i,y} \\ f_{i,z} 343 | \end{bmatrix} 344 | \leq 345 | \begin{bmatrix} 346 | \infty \\ \infty \\ \infty \\ \infty \\ f_{\max} 347 | \end{bmatrix} 348 | $$ 349 | 350 | ## 3. Reference Trajectory Generation 351 | The desired robot behavior is used to construct the reference trajectory. In the application, our reference trajectories are simple and only contain non-zero $xy$-velocity, $xy$-position, $z$-postion, yaw, and yaw rate. All parameters are commanded directly by the robot operator except for yaw and $xy$-position, which are determined by integrating the appropriate velocities. The other states (roll, pitch, roll rate, pitch rate and $z$-velocity) are always set to 0. The reference trajectory is also used to determine the dynamics constraints and future foot placement locations. 352 | 353 | In practice, the reference trajectory is short (between 0.5 and 0.3 seconds) and recalculated often (every 0.05 to 0.03 seconds) to ensure the simplified dynamics remain accurate if the robot is disturbed. 354 | 355 | ## 4. QP Formulation 356 | 357 | ### a. Batch Formulation for Dynamic Constraints 358 | **Key idea:** For the state space model, express $x_0, x_1, \cdots, x_k$ as function of $u_0$. 359 | 360 | $$ 361 | \begin{aligned} 362 | x_1 &= Ax_0 + Bu_0 \\ 363 | x_2 &= Ax_1 + Bu_1 = A^2x_0 + ABu_0 + Bu_1 \\ 364 | &\vdots \\ 365 | x_k & = A^kx_0 + A^{k-1}Bu_0 + A^{k-2}Bu_1 + \cdots + Bu_{k-1} 366 | \end{aligned} 367 | $$ 368 | 369 | We can write these equations in matrix form 370 | 371 | $$ 372 | \begin{bmatrix} 373 | x_1 \\ x_2 \\ \vdots \\ x_k 374 | \end{bmatrix} = 375 | \begin{bmatrix} 376 | A \\ A^2 \\ \vdots \\A^k 377 | \end{bmatrix} 378 | x_0 379 | + 380 | \begin{bmatrix} 381 | B & 0 & \cdots & 0 \\ 382 | AB & B & \cdots & 0 \\ 383 | \vdots & \vdots & \ddots & \vdots \\ 384 | A^{k-1}B & A^{k-2}B & \cdots & B 385 | \end{bmatrix} 386 | \begin{bmatrix} 387 | u_0 \\ u_1 \\ \vdots \\ u_{k-1} 388 | \end{bmatrix} 389 | $$ 390 | 391 | where $k$ is the **horizon length**. Let $x_t$ denotes the system state at time step $t$, i.e. at **current state**, then we can write 392 | 393 | $$ 394 | \begin{bmatrix} 395 | x_{t+1} \\ x_{t+2} \\ \vdots \\ x_{t+k} 396 | \end{bmatrix} = 397 | \begin{bmatrix} 398 | A \\ A^2 \\ \vdots \\A^k 399 | \end{bmatrix} 400 | x_t 401 | + 402 | \begin{bmatrix} 403 | B & 0 & \cdots & 0 \\ 404 | AB & B & \cdots & 0 \\ 405 | \vdots & \vdots & \ddots & \vdots \\ 406 | A^{k-1}B & A^{k-2}B & \cdots & B 407 | \end{bmatrix} 408 | \begin{bmatrix} 409 | u_t \\ u_{t+1} \\ \vdots \\ u_{t+k-1} 410 | \end{bmatrix} 411 | $$ 412 | 413 | We can denote this equation as 414 | 415 | $$ 416 | X = S^x x_t + S^u U 417 | $$ 418 | 419 | For a 2-norm cost function, we can write 420 | 421 | $$ 422 | \begin{aligned} 423 | J_k(x_t,\mathbf{U}) &= J_f(x_k) + \sum_{i=0}^{k-1}l(x_i,u_i) \\ 424 | &= x_k^\text{T} Q_f x_k + \sum_{i=0}^{k-1}\left(x_i^\text{T}Q_ix_i + u_i^\text{T}R_iu_i\right) \\ 425 | &= X^\text{T}\bar{Q}X + U^\text{T}\bar{R}U 426 | \end{aligned} 427 | $$ 428 | 429 | where 430 | 431 | $$ 432 | \mathbf{U} = 433 | \begin{bmatrix} 434 | X \\ U 435 | \end{bmatrix} \quad 436 | \bar{Q} = \begin{bmatrix} 437 | Q_1 & & & \\ 438 | & \ddots & & \\ 439 | & & Q_{k-1} & \\ 440 | & & & Q_f 441 | \end{bmatrix} \quad 442 | \bar{R} = \begin{bmatrix} 443 | R & & \\ 444 | & \ddots & \\ 445 | & & R \\ 446 | \end{bmatrix} 447 | $$ 448 | 449 | Substituting the expression of state space model into the cost, we have 450 | 451 | $$ 452 | \begin{aligned} 453 | J_k(x_t,\mathbf{U}) &= X^\text{T}\bar{Q}X + U^\text{T}\bar{R}U \\ 454 | &= \left(S^x x_t + S^u U\right)^\text{T}\bar{Q}\left(S^x x_t + S^u U\right) + U^\text{T}\bar{R}U \\ 455 | &= U^\text{T}\underbrace{\left((S^u)^\text{T}\bar{Q}S^u+\bar{R}\right)}_H U + 2x_t^\text{T}\underbrace{(S^x)^\text{T}\bar{Q}S^u}_F U + x_t^\text{T}\underbrace{(S^x)^\text{T}\bar{Q}S^x}_Y x_t\\ 456 | &= U^\text{T}HU + 2x_tFU + x_t^\text{T}Yx_t 457 | \end{aligned} 458 | $$ 459 | Compare with the standard form of cost in QP formulation $\frac{1}{2}U^\text{T}HU + U^\text{T}g$, we can easily get 460 | $$ 461 | \begin{aligned} 462 | H &= 2\left((S^u)^\text{T}\bar{Q}S^u + \bar{R}\right) \\ 463 | g &= 2(S^u)^\text{T}\bar{Q}S^xx_t 464 | \end{aligned} 465 | $$ 466 | 467 | ### b. Create QP Cost 468 | Recall that the form of our MPC problem is 469 | 470 | $$ 471 | \begin{aligned} 472 | \min_{x, u} \quad& \sum_{i=0}^{k-1}(x_{i+1}-x_{i+1,\text{ref}})^{\text{T}}Q_i(x_{i+1}-x_{i+1,\text{ref}}) + u_i^\text{T}R_iu_i \\ 473 | \text{s.t.} \quad& x_{i+1} = A_ix_i + B_iu_i \\ 474 | & \underline{c}_i \leq C_iu_i \leq \overline{c}_i \\ 475 | & D_iu_i = 0 476 | \end{aligned} 477 | $$ 478 | 479 | where $i = 0, \cdots, k-1$. 480 | 481 | In this project, we choose $Q_1 = \cdots = Q_{k-1} = Q_f$, i.e. 482 | 483 | $$ 484 | \bar{Q} = \begin{bmatrix} 485 | Q & & & \\ 486 | & \ddots & & \\ 487 | & & Q & \\ 488 | & & & Q 489 | \end{bmatrix} \quad 490 | \bar{R} = \begin{bmatrix} 491 | R & & \\ 492 | & \ddots & \\ 493 | & & R \\ 494 | \end{bmatrix} 495 | $$ 496 | 497 | ~~~python 498 | _Qi = np.diag(np.array(parameters['Q'], dtype=float)) 499 | Qbar = np.kron(np.identity(horizon), _Qi) 500 | 501 | _r = parameters['R'] 502 | _Ri = _r * np.identity(num_input, dtype=float) 503 | Rbar = np.kron(np.identity(horizon), _Ri) 504 | ~~~ 505 | 506 | Let's substitute the dynamic constraint into the cost function, then we can get 507 | 508 | $$ 509 | \begin{aligned} 510 | J &= \sum_{i=0}^{k-1}(x_{i+1}-x_{i+1,\text{ref}})^{\text{T}}Q_i(x_{i+1}-x_{i+1,\text{ref}}) + u_i^\text{T}R_iu_i \\ 511 | &= \sum_{i=0}^{k-1}(A_ix_i+B_iu_i-x_{i+1,\text{ref}})^{\text{T}}Q_i(A_ix_i + B_iu_i-x_{i+1,\text{ref}}) + u_i^\text{T}R_iu_i \\ 512 | &= (S^x x_t + S^u U - X_{\text{ref}})^\text{T}\bar{Q}(S^x x_t + S^u U - X_\text{ref}) + U^\text{T}\bar{R}U 513 | \end{aligned} 514 | $$ 515 | 516 | where $X_\text{ref} = [x_{1,\text{ref}}, \cdots, x_{k,\text{ref}}]^\text{T}$. We can get the QP matrix with the same procedure. 517 | 518 | $$ 519 | \begin{aligned} 520 | H &= 2\left((S^u)^\text{T}\bar{Q}S^u + \bar{R}\right) \\ 521 | g &= 2(S^u)^\text{T}\bar{Q}(S^xx_t-X_\text{ref}) 522 | \end{aligned} 523 | $$ 524 | 525 | 526 | ### c. Create QP Constraint 527 | 528 | ### d. QP Solver 529 | **qpsolvers**: https://pypi.org/project/qpsolvers/ 530 | 531 | As mentioned in the project description of qpsolvers, we need to write our formulation in the form 532 | 533 | $$ 534 | \begin{aligned} 535 | \min_x &\quad \frac{1}{2}x^\text{T}Px + q^\text{T}x \\ 536 | \text{s.t.} & \quad Gx \leq h \\ 537 | &\quad Ax = b \\ 538 | &\quad \text{lb} \leq x \leq \text{ub} 539 | \end{aligned} 540 | $$ 541 | 542 | Then we can get the solution using the code below 543 | ~~~python 544 | from qpsolvers import solve_qp 545 | x = solve_qp(P, q, G, h, A, b, lb, ub) 546 | print("QP solution: x = {}".format(x)) 547 | ~~~ 548 | 549 | The QP problem in the paper is written as 550 | 551 | $$ 552 | \begin{aligned} 553 | \min_\mathbf{U} &\quad \frac{1}{2}\mathbf{U}^\text{T}\mathbf{HU} + \mathbf{U}^\text{T}\mathbf{g} \\ 554 | \text{s.t.} &\quad \underline{\mathbf{c}} \leq \mathbf{CU} \leq \overline{\mathbf{c}} 555 | \end{aligned} 556 | $$ -------------------------------------------------------------------------------- /robot/aliengo/urdf/aliengo.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 66 | 67 | 68 | 69 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 125 | 126 | 127 | 132 | 133 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | transmission_interface/SimpleTransmission 225 | 226 | hardware_interface/EffortJointInterface 227 | 228 | 229 | hardware_interface/EffortJointInterface 230 | 1 231 | 232 | 233 | 234 | transmission_interface/SimpleTransmission 235 | 236 | hardware_interface/EffortJointInterface 237 | 238 | 239 | hardware_interface/EffortJointInterface 240 | 1 241 | 242 | 243 | 244 | transmission_interface/SimpleTransmission 245 | 246 | hardware_interface/EffortJointInterface 247 | 248 | 249 | hardware_interface/EffortJointInterface 250 | 1 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 280 | 281 | 282 | 287 | 288 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | transmission_interface/SimpleTransmission 380 | 381 | hardware_interface/EffortJointInterface 382 | 383 | 384 | hardware_interface/EffortJointInterface 385 | 1 386 | 387 | 388 | 389 | transmission_interface/SimpleTransmission 390 | 391 | hardware_interface/EffortJointInterface 392 | 393 | 394 | hardware_interface/EffortJointInterface 395 | 1 396 | 397 | 398 | 399 | transmission_interface/SimpleTransmission 400 | 401 | hardware_interface/EffortJointInterface 402 | 403 | 404 | hardware_interface/EffortJointInterface 405 | 1 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 435 | 436 | 437 | 442 | 443 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | transmission_interface/SimpleTransmission 535 | 536 | hardware_interface/EffortJointInterface 537 | 538 | 539 | hardware_interface/EffortJointInterface 540 | 1 541 | 542 | 543 | 544 | transmission_interface/SimpleTransmission 545 | 546 | hardware_interface/EffortJointInterface 547 | 548 | 549 | hardware_interface/EffortJointInterface 550 | 1 551 | 552 | 553 | 554 | transmission_interface/SimpleTransmission 555 | 556 | hardware_interface/EffortJointInterface 557 | 558 | 559 | hardware_interface/EffortJointInterface 560 | 1 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 572 | 573 | 574 | 575 | 576 | 577 | 578 | 579 | 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 590 | 591 | 592 | 597 | 598 | 606 | 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | 630 | 631 | 633 | 634 | 635 | 636 | 637 | 638 | 639 | 640 | 641 | 642 | 643 | 644 | 645 | 646 | 647 | 648 | 649 | 650 | 651 | 652 | 653 | 654 | 655 | 656 | 657 | 658 | 659 | 660 | 662 | 663 | 664 | 665 | 666 | 667 | 668 | 669 | 670 | 671 | 672 | 673 | 674 | 675 | 676 | 677 | 678 | 679 | 680 | 681 | 682 | 683 | 684 | 685 | 686 | 687 | 688 | 689 | transmission_interface/SimpleTransmission 690 | 691 | hardware_interface/EffortJointInterface 692 | 693 | 694 | hardware_interface/EffortJointInterface 695 | 1 696 | 697 | 698 | 699 | transmission_interface/SimpleTransmission 700 | 701 | hardware_interface/EffortJointInterface 702 | 703 | 704 | hardware_interface/EffortJointInterface 705 | 1 706 | 707 | 708 | 709 | transmission_interface/SimpleTransmission 710 | 711 | hardware_interface/EffortJointInterface 712 | 713 | 714 | hardware_interface/EffortJointInterface 715 | 1 716 | 717 | 718 | 719 | 720 | 721 | --------------------------------------------------------------------------------