├── .gitignore ├── LICENSE ├── README.md ├── doc ├── compare │ ├── backward.gif │ ├── forward.gif │ └── sidewalk.gif ├── gazebo_a1.png ├── hardware_a1.png ├── isaac_a1.png └── sys.png ├── docker └── Dockerfile ├── docker_noetic_focal ├── Dockerfile └── test.txt ├── gazebo_docker ├── Dockerfile └── run_gazebo_docker.bash └── src ├── a1_cpp ├── CMakeLists.txt ├── config │ ├── gazebo_a1_mpc.yaml │ ├── gazebo_a1_mpc_2x.yaml │ ├── gazebo_a1_qp.yaml │ ├── hardware_a1_mpc.yaml │ ├── hardware_a1_mpc_new.yaml │ ├── hardware_a1_qp.yaml │ ├── isaac_a1_mpc.yaml │ └── isaac_a1_qp.yaml ├── launch │ └── a1_ctrl.launch ├── package.xml └── src │ ├── A1BasicEKF.cpp │ ├── A1BasicEKF.h │ ├── A1CtrlStates.h │ ├── A1Params.h │ ├── A1RobotControl.cpp │ ├── A1RobotControl.h │ ├── ConvexMpc.cpp │ ├── ConvexMpc.h │ ├── GazeboA1ROS.cpp │ ├── GazeboA1ROS.h │ ├── HardwareA1ROS.cpp │ ├── HardwareA1ROS.h │ ├── IsaacA1ROS.cpp │ ├── IsaacA1ROS.h │ ├── MainGazebo.cpp │ ├── MainHardware.cpp │ ├── MainIsaac.cpp │ ├── legKinematics │ ├── A1Kinematics.cpp │ ├── A1Kinematics.h │ └── README.md │ ├── test │ ├── test_bezier.cpp │ ├── test_mpc.cpp │ ├── test_rotation.cpp │ └── test_unitree_msgs.cpp │ └── utils │ ├── Utils.cpp │ ├── Utils.h │ └── filter.hpp ├── data_collection ├── CMakeLists.txt ├── data │ ├── qSineSignal.txt │ └── qSineSignalClean.txt ├── launch │ └── lowlevel_demo.launch ├── package.xml └── src │ ├── HighLevel.cpp │ ├── HighLevel.hpp │ ├── Lowlevel.cpp │ ├── Lowlevel.hpp │ ├── MainHardware.cpp │ ├── signal_sine.cpp │ └── signal_sine.hpp ├── go1_rl_ctrl_cpp ├── CMakeLists.txt ├── config │ ├── hardware_parameters.yaml │ ├── parameters.yaml │ └── xml │ │ ├── act.xml │ │ ├── foot_force.xml │ │ ├── foot_force_hardware.xml │ │ ├── foot_pos.xml │ │ ├── joint_pos.xml │ │ ├── joint_vel.xml │ │ ├── obs.xml │ │ └── torque.xml ├── launch │ ├── a1_ctrl.launch │ ├── go1_ctrl.launch │ └── go1_ctrl_hardware.launch ├── package.xml ├── resource │ ├── cpp_model.pt │ ├── mass.pt │ ├── position.pt │ └── stand_cpp_model.pt └── src │ ├── EKF │ ├── Go1BasicEKF.cpp │ └── Go1BasicEKF.hpp │ ├── Go1CtrlStates.hpp │ ├── Go1Params.hpp │ ├── Go1RLController.cpp │ ├── Go1RLController.hpp │ ├── Go1RLHardwareController.cpp │ ├── Go1RLHardwareController.hpp │ ├── MainGazebo.cpp │ ├── SwitchController.hpp │ ├── TestHardware.cpp │ ├── legKinematics │ ├── Go1Kinematics.cpp │ ├── Go1Kinematics.hpp │ └── README.md │ ├── observation │ ├── Go1HardwareObservation.hpp │ └── Go1Observation.hpp │ ├── servo_stand_policy │ ├── GazeboServo.cpp │ ├── GazeboServo.hpp │ ├── HardwareServo.cpp │ ├── HardwareServo.hpp │ ├── HardwareServoSwitch.cpp │ ├── HardwareServoSwitch.hpp │ ├── body.cpp │ ├── body.h │ └── servo.cpp │ ├── torch_eigen │ ├── TorchEigen.cpp │ └── TorchEigen.hpp │ └── utils │ ├── Utils.cpp │ ├── Utils.hpp │ └── filter.hpp └── pytorch_debug ├── CMakeLists.txt ├── actor_architecture_default_1.pth ├── cpp_model.pt ├── main.cpp ├── module.py ├── py_model.pt ├── rl_policy_module.py └── student_traced_debug.pt /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | 35 | build 36 | devel 37 | logs 38 | src/a1_cpp/cmake-build-debug 39 | .catkin_tools 40 | .idea 41 | .vscode 42 | src/a1_cpp/cmake-build-debug-remote/ 43 | 44 | src/a1_cpp/src/mpc_osqp.cc 45 | 46 | 47 | src/pytorch_debug/libtorch 48 | src/go1_rl_ctrl_cpp/libtorch 49 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Go1-RL-Controller 2 | ## Introduction 3 | 4 | This repo provides a quadruped control stack that controls the Unitree Go1 robot, one of the most popular quadruped robots used in academic research in recent years. This control stack implements model free reinforcement learning control methods. Additionally, we use Docker and ROS1 to make it easy to install and develop. 5 | 6 | To facilitate quadruped research and application development, we provide interfaces to use this controller to control either simulated robots or real robots. We support following types of robots 7 | 8 | 1. Go1 Robot simulated in Gazebo Simulator 9 | 2. Go1 Robot hardware (coming soon) 10 | 11 | Even if you do not have an Go1 robot hardware, our simulation setup can let you do research in either the Gazebo simulator. Various efforts have been done to reduce the sim2real gap in this work. 12 | 13 | ## Controller Installation 14 | Will update for full installation procedure later. For now, please use google doc for reference. 15 | 16 | https://docs.google.com/document/d/1oeaLzuxC3kVKrw5ra6BAsyoRr79fPUHETEeBppi0BQo/edit 17 | 18 | After the installation setup is ready 19 | 20 | ## Controller Learning 21 | Train in issac gym, in which we can achieve a massive parallel reinforcement learning pipeline. Details will be updated later. 22 | 23 | ## Gazebo Demo 24 | Open the gazebo 25 | ```shell 26 | roslaunch unitree_gazebo normal.launch rname:=go1 27 | ``` 28 | 29 | ![Gazebo Go1](doc/compare/forward.gif) 30 | ![Gazebo Go1](doc/compare/backward.gif) 31 | ![Gazebo Go1](doc/compare/sidewalk.gif) 32 | 33 | 34 | ### Adjust the robot in the Gazebo 35 | During the development, we can let the robot stand up by two Unitree testing scripts. The following two commands are very handy. 36 | ```shell 37 | rosrun unitree_controller unitree_servo # let the robot stretch legs 38 | rosrun unitree_controller unitree_move_kinetic # place the robot back to origin 39 | ``` 40 | We oftern running the two commands alternatively by running one and ctrl-C after a while to adjust the pose of the robot. 41 | After the robot properly stands up, it is ready to be controlled. 42 | 43 | ### Start the controller 44 | 45 | Continue to run the controller 46 | ```shell 47 | roslaunch roslaunch go1_rl_ctrl_cpp go1_ctrl.launch 48 | ``` 49 | 50 | Now, the controller engages the robot in Gazebo. The robot has two modes: "stand" (default) and "walk". The robot still stands at the beginning. If using QP controller, your robot may be not very stable when standing up, you can use the two unitree_controller commands we mentioned above to adjust the robot to make it stands up first. The Convex MPC controller is more stable. 51 | 52 | Different computers have different performance. If the robot is very unstable, chances are the simulation parameters of the Gazebo needs to be tuned (unitree_ros/unitree_gazebo/worlds). See [Gazebo tutorial](https://gazebosim.org/tutorials?tut=physics_params&cat=physics) for more information. 53 | 54 | Connect a USB joystick to the host computer. 55 | 56 | Currently, we need a Linux-supported joystick like an Xbox One Controller to drive the robot around. 57 | You should follow the instructions in http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick to learn how to setup a joystick controller. 58 | 59 | Take Xbox One Controller as an example: 60 | 61 | | Button | Function | 62 | | :----- | :------- | 63 | | Left Joystick | Control the robot's yaw orientation | 64 | | Right Joystick | Control the robot's walking direction | 65 | | `A`| Switch the mode between "stand" and "walk" | 66 | 67 | 68 | ## Troubleshooting 69 | Please create issues for any installation problem. 70 | 71 | ## Acknowledgement 72 | We referenced the controller architecture written by Shuo Yang and Zixin Zhang in there A1-QP-MPC-Controller. 73 | -------------------------------------------------------------------------------- /doc/compare/backward.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/doc/compare/backward.gif -------------------------------------------------------------------------------- /doc/compare/forward.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/doc/compare/forward.gif -------------------------------------------------------------------------------- /doc/compare/sidewalk.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/doc/compare/sidewalk.gif -------------------------------------------------------------------------------- /doc/gazebo_a1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/doc/gazebo_a1.png -------------------------------------------------------------------------------- /doc/hardware_a1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/doc/hardware_a1.png -------------------------------------------------------------------------------- /doc/isaac_a1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/doc/isaac_a1.png -------------------------------------------------------------------------------- /doc/sys.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/doc/sys.png -------------------------------------------------------------------------------- /docker_noetic_focal/test.txt: -------------------------------------------------------------------------------- 1 | root@shuoyang-Z390:/tmp# wget http://ftp.de.debian.org/debian/pool/main/g/glib2.0/libglib2.0-0_2.66.8-1_amd64.deb 2 | 3 | 4 | root@shuoyang-Z390:/tmp# dpkg -i libglib2.0-0_2.66.8-1_amd64.deb 5 | dpkg: warning: downgrading libglib2.0-0:amd64 from 2.68.4-1ubuntu1 to 2.66.8-1 6 | (Reading database ... 119026 files and directories currently installed.) 7 | Preparing to unpack libglib2.0-0_2.66.8-1_amd64.deb ... 8 | Unpacking libglib2.0-0:amd64 (2.66.8-1) over (2.68.4-1ubuntu1) ... 9 | dpkg: dependency problems prevent configuration of libglib2.0-0:amd64: 10 | libglib2.0-0:amd64 depends on libmount1 (>= 2.35.2-7~); however: 11 | Version of libmount1:amd64 on system is 2.34-0.1ubuntu9.3. 12 | libglib2.0-0:amd64 depends on libselinux1 (>= 3.1~); however: 13 | Version of libselinux1:amd64 on system is 3.0-1build2. 14 | 15 | dpkg: error processing package libglib2.0-0:amd64 (--install): 16 | dependency problems - leaving unconfigured 17 | Processing triggers for libc-bin (2.31-0ubuntu9.7) ... 18 | Errors were encountered while processing: 19 | libglib2.0-0:amd64 20 | root@shuoyang-Z390:/tmp# wget http://archive.ubuntu.com/ubuntu/pool/main/libs/libselinux/libselinux1_3.1-3build2_amd64.deb 21 | 22 | root@shuoyang-Z390:/tmp# dpkg -i libselinux1_3.1-3build2_amd64.deb 23 | 24 | root@shuoyang-Z390:/tmp# wget http://archive.ubuntu.com/ubuntu/pool/main/g/glibc/libc6_2.34-0ubuntu3_amd64.deb 25 | --2022-05-30 22:33:19-- http://archive.ubuntu.com/ubuntu/pool/main/g/glibc/libc6_2.34-0ubuntu3_amd64.deb 26 | 27 | root@shuoyang-Z390:/tmp# dpkg -i libc6_2.34-0ubuntu3_amd64.deb 28 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.33' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 29 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.34' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 30 | root@shuoyang-Z390:/tmp# sudo dpkg -i libc6_2.34-0ubuntu3_amd64.deb 31 | sudo: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.33' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 32 | sudo: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.34' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 33 | root@shuoyang-Z390:/tmp# dpkg -i libc6_2.34-0ubuntu3_amd64.deb 34 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.33' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 35 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.34' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 36 | root@shuoyang-Z390:/tmp# dpkg -i libselinux1_3.1-3build2_amd64.deb 37 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.33' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 38 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.34' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 39 | root@shuoyang-Z390:/tmp# wget http://archive.ubuntu.com/ubuntu/pool/main/u/util-linux/libmount1_2.36.1-8ubuntu1_amd64.deb 40 | 41 | root@shuoyang-Z390:/tmp# ls 42 | ls: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.33' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 43 | ls: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.34' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 44 | 45 | root@shuoyang-Z390:/tmp# dpkg -i *.deb 46 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.33' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 47 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.34' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 48 | root@shuoyang-Z390:/tmp# apt-get -f install 49 | Reading package lists... Done 50 | Building dependency tree 51 | Reading state information... Done 52 | Correcting dependencies... failed. 53 | The following packages have unmet dependencies: 54 | libglib2.0-0 : Depends: libmount1 (>= 2.35.2-7~) but 2.34-0.1ubuntu9.3 is installed 55 | Recommends: xdg-user-dirs but it is not installed 56 | libglib2.0-bin : Depends: libglib2.0-0 (= 2.64.6-1~ubuntu20.04.4) but 2.66.8-1 is installed 57 | libglib2.0-dev : Depends: libglib2.0-0 (= 2.64.6-1~ubuntu20.04.4) but 2.66.8-1 is installed 58 | libselinux1 : Depends: libc6 (>= 2.34) but 2.31-0ubuntu9.9 is installed 59 | libselinux1-dev : Depends: libselinux1 (= 3.0-1build2) but 3.1-3build2 is installed 60 | W: Target Packages (main/binary-amd64/Packages) is configured multiple times in /etc/apt/sources.list.d/ros-latest.list:1 and /etc/apt/sources.list.d/ros1-latest.list:1 61 | W: Target Packages (main/binary-all/Packages) is configured multiple times in /etc/apt/sources.list.d/ros-latest.list:1 and /etc/apt/sources.list.d/ros1-latest.list:1 62 | E: Error, pkgProblemResolver::Resolve generated breaks, this may be caused by held packages. 63 | E: Unable to correct dependencies 64 | root@shuoyang-Z390:/tmp# dpkg -i libc6_2.34-0ubuntu3_amd64.deb 65 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.33' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) 66 | dpkg: /lib/x86_64-linux-gnu/libc.so.6: version `GLIBC_2.34' not found (required by /lib/x86_64-linux-gnu/libselinux.so.1) -------------------------------------------------------------------------------- /gazebo_docker/Dockerfile: -------------------------------------------------------------------------------- 1 | # we build this docker based on a docker with ros already installed 2 | FROM osrf/ros:melodic-desktop-full 3 | 4 | LABEL maintainer="shuoyang@andrew.cmu.edu" 5 | ENV REFRESH_AT 2022-03-22 6 | 7 | # nvidia-container-runtime 8 | ENV NVIDIA_VISIBLE_DEVICES \ 9 | ${NVIDIA_VISIBLE_DEVICES:-all} 10 | ENV NVIDIA_DRIVER_CAPABILITIES \ 11 | ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics 12 | 13 | # install necessary dependencies 14 | RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; \ 15 | else export USE_PROC=$(($(nproc)/2)) ; fi && \ 16 | apt-get update && apt-get install -y \ 17 | vim \ 18 | libatlas-base-dev \ 19 | libeigen3-dev \ 20 | libgoogle-glog-dev \ 21 | libsuitesparse-dev \ 22 | python-catkin-tools \ 23 | python3-matplotlib \ 24 | gfortran \ 25 | autoconf \ 26 | coinor-libipopt-dev \ 27 | libgfortran3 \ 28 | curl \ 29 | libopenmpi-dev \ 30 | apt-utils \ 31 | software-properties-common \ 32 | build-essential \ 33 | libssl-dev \ 34 | wget \ 35 | libtool \ 36 | cmake 37 | 38 | # install python 3.6 39 | RUN apt-get update && \ 40 | apt-get install -y python3.6 41 | RUN update-alternatives --install /usr/bin/python python /usr/bin/python3.6 1 42 | RUN curl https://bootstrap.pypa.io/pip/3.6/get-pip.py -o get-pip.py 43 | RUN python get-pip.py 44 | RUN rm get-pip.py 45 | 46 | # change timezone (this is very important otherwise many ROS topic time will be strange) 47 | ENV TZ=America/New_York 48 | RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone 49 | 50 | # make unitree workspace 51 | ENV SUPPORT_WS=/root/support_files 52 | ENV UNITREE_WS=/root/unitree_ws 53 | RUN mkdir -p $SUPPORT_WS 54 | RUN mkdir -p $UNITREE_WS/src 55 | WORKDIR $UNITREE_WS 56 | RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin init;" 57 | 58 | # install unitree sdk dependencies 59 | WORKDIR $SUPPORT_WS 60 | RUN git clone https://github.com/lcm-proj/lcm.git && \ 61 | cd ${SUPPORT_WS}/lcm && \ 62 | git checkout tags/v1.4.0 && \ 63 | mkdir build && \ 64 | cd build && \ 65 | cmake .. && \ 66 | make -j12 && \ 67 | make install 68 | 69 | # notice we must use v3.2 70 | WORKDIR $SUPPORT_WS 71 | RUN git clone https://github.com/unitreerobotics/unitree_legged_sdk.git && \ 72 | cd ${SUPPORT_WS}/unitree_legged_sdk && git checkout v3.2 && \ 73 | mkdir build && \ 74 | cd build && \ 75 | cmake .. && \ 76 | make -j12 77 | 78 | WORKDIR $SUPPORT_WS 79 | RUN git clone https://github.com/unitreerobotics/aliengo_sdk.git && \ 80 | cd ${SUPPORT_WS}/aliengo_sdk && \ 81 | mkdir build && \ 82 | cd build && \ 83 | cmake .. && \ 84 | make -j12 85 | 86 | # install necessary dependencies 87 | RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; \ 88 | else export USE_PROC=$(($(nproc)/2)) ; fi && \ 89 | apt-get update && apt-get install -y \ 90 | ros-${ROS_DISTRO}-ros-control \ 91 | ros-${ROS_DISTRO}-gazebo-ros \ 92 | ros-${ROS_DISTRO}-joy \ 93 | ros-${ROS_DISTRO}-ros-controllers \ 94 | ros-${ROS_DISTRO}-robot-state-publisher \ 95 | ros-${ROS_DISTRO}-plotjuggler-ros 96 | 97 | WORKDIR $UNITREE_WS/src 98 | RUN git clone https://github.com/ShuoYangRobotics/unitree_ros.git 99 | WORKDIR $UNITREE_WS 100 | 101 | ENV UNITREE_SDK_VERSION=3_2 102 | ENV UNITREE_LEGGED_SDK_PATH=${SUPPORT_WS}/unitree_legged_sdk 103 | ENV ALIENGO_SDK_PATH=${SUPPORT_WS}/aliengo_sdk 104 | ENV UNITREE_PLATFORM=amd64 105 | RUN echo "#unitree config" >> ~/.bashrc 106 | RUN echo "export export UNITREE_SDK_VERSION=3_2" >> ~/.bashrc 107 | RUN echo "export UNITREE_LEGGED_SDK_PATH=${SUPPORT_WS}/unitree_legged_sdk" >> ~/.bashrc 108 | RUN echo "export ALIENGO_SDK_PATH=${SUPPORT_WS}/aliengo_sdk" >> ~/.bashrc 109 | RUN echo "export UNITREE_PLATFORM=\"amd64\"" >> ~/.bashrc 110 | RUN echo "source ${UNITREE_WS}/devel/setup.bash" >> ~/.bashrc 111 | 112 | # compile unitree ros 113 | RUN ls $UNITREE_WS/src/unitree_ros 114 | RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin build unitree_legged_msgs; source ${UNITREE_WS}/devel/setup.bash; catkin build" 115 | 116 | 117 | RUN echo "export ROS_MASTER_URI=http://localhost:11311;export ROS_IP=localhost;export ROS_HOSTNAME=localhost" >> ~/.bashrc 118 | 119 | # To use rosparam load yaml files 120 | RUN pip install pyyaml 121 | 122 | # default use python 2.7, or type this command when docker starts 123 | RUN update-alternatives --install /usr/bin/python python /usr/bin/python2.7 3 -------------------------------------------------------------------------------- /gazebo_docker/run_gazebo_docker.bash: -------------------------------------------------------------------------------- 1 | XAUTH=/tmp/.docker.xauth 2 | if [ ! -f $XAUTH ] 3 | then 4 | xauth_list=$(xauth nlist :0 | sed -e 's/^..../ffff/') 5 | if [ ! -z "$xauth_list" ] 6 | then 7 | sudo echo $xauth_list | sudo xauth -f $XAUTH nmerge - 8 | else 9 | sudo touch $XAUTH 10 | fi 11 | sudo chmod a+r $XAUTH 12 | fi 13 | 14 | sudo docker run -it \ 15 | --env="DISPLAY=$DISPLAY" \ 16 | --env="QT_X11_NO_MITSHM=1" \ 17 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 18 | --env="XAUTHORITY=$XAUTH" \ 19 | --volume="$XAUTH:$XAUTH" \ 20 | --runtime=nvidia \ 21 | --privileged \ 22 | --network host \ 23 | --name a1_unitree_gazebo_docker \ 24 | a1_unitree_gazebo_image 25 | -------------------------------------------------------------------------------- /src/a1_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.1) 2 | project(a1_cpp) 3 | set(CMAKE_BUILD_TYPE "Release") 4 | #set(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_CXX_STANDARD 14) 7 | #-DEIGEN_USE_MKL_ALL") 8 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 9 | 10 | #check environmental variables 11 | message(STATUS "CMAKE_PREFIX_PATH: ${CMAKE_PREFIX_PATH}") 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | rospy 16 | std_msgs 17 | geometry_msgs 18 | unitree_legged_msgs 19 | ) 20 | 21 | find_package(Eigen3) 22 | include_directories( 23 | ${EIGEN3_INCLUDE_DIR} 24 | ) 25 | 26 | catkin_package( 27 | # INCLUDE_DIRS include 28 | # LIBRARIES rc_sim_oub 29 | # CATKIN_DEPENDS roscpp rospy std_msgs 30 | # DEPENDS system_lib 31 | ) 32 | 33 | include_directories( 34 | ${catkin_INCLUDE_DIRS} 35 | ) 36 | 37 | # to find osqp.h 38 | include_directories($ENV{UNITREE_LOCAL_INSTALL}/include/osqp) 39 | find_package(OsqpEigen REQUIRED) 40 | 41 | # add unitree hardware library, these flags must be correctly set 42 | include_directories( 43 | $ENV{UNITREE_LEGGED_SDK_PATH}/include 44 | ) 45 | 46 | link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib/cpp/amd64) 47 | set(EXTRA_LIBS -pthread libunitree_legged_sdk.so) 48 | 49 | # Declare cpp libraries 50 | # common robot controller every type of robots need 51 | add_library(a1_lib 52 | src/legKinematics/A1Kinematics.h 53 | src/legKinematics/A1Kinematics.cpp 54 | src/A1Params.h 55 | src/A1CtrlStates.h 56 | src/utils/Utils.cpp 57 | src/utils/Utils.h 58 | src/A1RobotControl.cpp 59 | src/A1RobotControl.h 60 | src/A1BasicEKF.cpp 61 | src/A1BasicEKF.h 62 | src/ConvexMpc.cpp 63 | src/ConvexMpc.h 64 | ) 65 | target_link_libraries(a1_lib ${catkin_LIBRARIES} OsqpEigen::OsqpEigen) 66 | 67 | # lib of different types of robots 68 | add_library(gazebo_a1_lib 69 | src/GazeboA1ROS.h 70 | src/GazeboA1ROS.cpp 71 | ) 72 | 73 | add_library(hardware_a1_lib 74 | src/HardwareA1ROS.h 75 | src/HardwareA1ROS.cpp 76 | ) 77 | 78 | # all robots depends on the a1_lib 79 | target_link_libraries(gazebo_a1_lib a1_lib) 80 | target_link_libraries(hardware_a1_lib a1_lib ${EXTRA_LIBS}) 81 | 82 | # Declare a cpp executable for gazebo robot 83 | add_executable(gazebo_a1_ctrl src/MainGazebo.cpp) 84 | target_link_libraries(gazebo_a1_ctrl 85 | gazebo_a1_lib 86 | ${catkin_LIBRARIES} 87 | OsqpEigen::OsqpEigen 88 | ) 89 | 90 | # Declare a cpp executable for hardware robot 91 | add_executable(hardware_a1_ctrl src/MainHardware.cpp) 92 | target_link_libraries(hardware_a1_ctrl 93 | hardware_a1_lib 94 | ${catkin_LIBRARIES} 95 | OsqpEigen::OsqpEigen 96 | ) 97 | 98 | # tests 99 | # Declare a cpp executable 100 | 101 | #add_executable(test_rotation src/test/test_rotation.cpp 102 | # src/utils/Utils.cpp 103 | # src/utils/Utils.h 104 | # ) 105 | 106 | add_executable(test_bezier src/test/test_bezier.cpp 107 | src/utils/Utils.cpp 108 | src/utils/Utils.h 109 | ) 110 | 111 | add_executable(test_mpc src/test/test_mpc.cpp) 112 | target_link_libraries(test_mpc 113 | a1_lib 114 | ${catkin_LIBRARIES} 115 | OsqpEigen::OsqpEigen 116 | ) 117 | 118 | add_executable(test_unitree_legged_msgs src/test/test_unitree_msgs.cpp) 119 | target_link_libraries(test_unitree_legged_msgs 120 | ${catkin_LIBRARIES} 121 | ) 122 | -------------------------------------------------------------------------------- /src/a1_cpp/config/gazebo_a1_mpc.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: true 2 | stance_leg_control_type: 1 # 0: QP; 1: MPC 3 | 4 | # physics parameters 5 | 6 | a1_robot_mass: 12.0 7 | 8 | a1_trunk_inertia_xx: 0.0168352186 9 | a1_trunk_inertia_xy: 0.0 10 | a1_trunk_inertia_xz: 0.0 11 | a1_trunk_inertia_yz: 0.0 12 | a1_trunk_inertia_yy: 0.0656071082 13 | a1_trunk_inertia_zz: 0.0742720659 14 | 15 | # default foothold position 16 | 17 | a1_default_foot_pos_FL_x: 0.17 18 | a1_default_foot_pos_FL_y: 0.15 19 | a1_default_foot_pos_FL_z: -0.35 20 | 21 | a1_default_foot_pos_FR_x: 0.17 22 | a1_default_foot_pos_FR_y: -0.15 23 | a1_default_foot_pos_FR_z: -0.35 24 | 25 | a1_default_foot_pos_RL_x: -0.17 26 | a1_default_foot_pos_RL_y: 0.15 27 | a1_default_foot_pos_RL_z: -0.35 28 | 29 | a1_default_foot_pos_RR_x: -0.17 30 | a1_default_foot_pos_RR_y: -0.15 31 | a1_default_foot_pos_RR_z: -0.35 32 | 33 | a1_gait_counter_speed_FL: 1.0 34 | a1_gait_counter_speed_FR: 1.0 35 | a1_gait_counter_speed_RL: 1.0 36 | a1_gait_counter_speed_RR: 1.0 37 | 38 | # MPC parameters 39 | 40 | q_weights_0: 20.0 # roll 41 | q_weights_1: 10.0 # pitch 42 | q_weights_2: 1.0 # yaw 43 | 44 | q_weights_3: 0.0 # pos x 45 | q_weights_4: 0.0 # pos y 46 | q_weights_5: 420.0 # pos z 47 | 48 | q_weights_6: 0.05 # omega x 49 | q_weights_7: 0.05 # omega y 50 | q_weights_8: 0.05 # omega z 51 | 52 | q_weights_9: 30.0 # vel x 53 | q_weights_10: 30.0 # vel y 54 | q_weights_11: 10.0 # vel z 55 | 56 | q_weights_12: 0.0 57 | 58 | r_weights_0: 0.0000001 59 | r_weights_1: 0.0000001 60 | r_weights_2: 0.0000001 61 | 62 | r_weights_3: 0.0000001 63 | r_weights_4: 0.0000001 64 | r_weights_5: 0.0000001 65 | 66 | r_weights_6: 0.0000001 67 | r_weights_7: 0.0000001 68 | r_weights_8: 0.0000001 69 | 70 | r_weights_9: 0.0000001 71 | r_weights_10: 0.0000001 72 | r_weights_11: 0.0000001 73 | 74 | # foot swing controller parameters 75 | # force_tgt = km_foot * kp_foot * foot_pos_error 76 | 77 | a1_kp_foot_x: 200.0 78 | a1_kp_foot_y: 200.0 79 | a1_kp_foot_z: 150.0 80 | 81 | a1_kd_foot_x: 10.0 82 | a1_kd_foot_y: 10.0 83 | a1_kd_foot_z: 5.0 84 | 85 | a1_km_foot_x: 0.1 86 | a1_km_foot_y: 0.1 87 | a1_km_foot_z: 0.1 88 | -------------------------------------------------------------------------------- /src/a1_cpp/config/gazebo_a1_mpc_2x.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: true 2 | stance_leg_control_type: 1 # 0: QP; 1: MPC 3 | 4 | # physics parameters 5 | 6 | a1_robot_mass: 12.0 7 | 8 | a1_trunk_inertia_xx: 0.0168352186 9 | a1_trunk_inertia_xy: 0.0 10 | a1_trunk_inertia_xz: 0.0 11 | a1_trunk_inertia_yz: 0.0 12 | a1_trunk_inertia_yy: 0.0656071082 13 | a1_trunk_inertia_zz: 0.0742720659 14 | 15 | # default foothold position 16 | 17 | a1_default_foot_pos_FL_x: 0.17 18 | a1_default_foot_pos_FL_y: 0.12 19 | a1_default_foot_pos_FL_z: -0.32 20 | 21 | a1_default_foot_pos_FR_x: 0.17 22 | a1_default_foot_pos_FR_y: -0.12 23 | a1_default_foot_pos_FR_z: -0.32 24 | 25 | a1_default_foot_pos_RL_x: -0.17 26 | a1_default_foot_pos_RL_y: 0.12 27 | a1_default_foot_pos_RL_z: -0.32 28 | 29 | a1_default_foot_pos_RR_x: -0.17 30 | a1_default_foot_pos_RR_y: -0.12 31 | a1_default_foot_pos_RR_z: -0.32 32 | 33 | a1_gait_counter_speed_FL: 1.5 34 | a1_gait_counter_speed_FR: 1.5 35 | a1_gait_counter_speed_RL: 1.5 36 | a1_gait_counter_speed_RR: 1.5 37 | 38 | # MPC parameters 39 | 40 | q_weights_0: 20.0 # roll 41 | q_weights_1: 10.0 # pitch 42 | q_weights_2: 20.0 # yaw 43 | 44 | q_weights_3: 0.0 # pos x 45 | q_weights_4: 0.0 # pos y 46 | q_weights_5: 320.0 # pos z 47 | 48 | q_weights_6: 0.05 # omega x 49 | q_weights_7: 0.05 # omega y 50 | q_weights_8: 0.05 # omega z 51 | 52 | q_weights_9: 30.0 # vel x 53 | q_weights_10: 30.0 # vel y 54 | q_weights_11: 10.0 # vel z 55 | 56 | q_weights_12: 0.0 57 | 58 | r_weights_0: 0.00004 59 | r_weights_1: 0.00004 60 | r_weights_2: 0.00004 61 | 62 | r_weights_3: 0.00004 63 | r_weights_4: 0.00004 64 | r_weights_5: 0.00004 65 | 66 | r_weights_6: 0.0005 67 | r_weights_7: 0.0005 68 | r_weights_8: 0.00004 69 | 70 | r_weights_9: 0.0005 71 | r_weights_10: 0.0005 72 | r_weights_11: 0.00004 73 | 74 | # foot swing controller parameters 75 | # force_tgt = km_foot * kp_foot * foot_pos_error 76 | 77 | a1_kp_foot_x: 200.0 78 | a1_kp_foot_y: 200.0 79 | a1_kp_foot_z: 150.0 80 | 81 | a1_kd_foot_x: 10.0 82 | a1_kd_foot_y: 10.0 83 | a1_kd_foot_z: 5.0 84 | 85 | a1_km_foot_x: 0.1 86 | a1_km_foot_y: 0.1 87 | a1_km_foot_z: 0.1 88 | -------------------------------------------------------------------------------- /src/a1_cpp/config/gazebo_a1_qp.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: true 2 | stance_leg_control_type: 0 # 0: QP; 1: MPC 3 | 4 | # physics parameters 5 | 6 | a1_robot_mass: 12.0 7 | 8 | # default foothold position 9 | 10 | a1_default_foot_pos_FL_x: 0.17 11 | a1_default_foot_pos_FL_y: 0.15 12 | a1_default_foot_pos_FL_z: -0.35 13 | 14 | a1_default_foot_pos_FR_x: 0.17 15 | a1_default_foot_pos_FR_y: -0.15 16 | a1_default_foot_pos_FR_z: -0.35 17 | 18 | a1_default_foot_pos_RL_x: -0.17 19 | a1_default_foot_pos_RL_y: 0.15 20 | a1_default_foot_pos_RL_z: -0.35 21 | 22 | a1_default_foot_pos_RR_x: -0.17 23 | a1_default_foot_pos_RR_y: -0.15 24 | a1_default_foot_pos_RR_z: -0.35 25 | 26 | a1_gait_counter_speed_FL: 0.7 27 | a1_gait_counter_speed_FR: 0.7 28 | a1_gait_counter_speed_RL: 0.7 29 | a1_gait_counter_speed_RR: 0.7 30 | 31 | # foot swing controller parameters 32 | # force_tgt = km_foot * kp_foot * foot_pos_error 33 | 34 | a1_kp_foot_x: 200.0 35 | a1_kp_foot_y: 300.0 36 | a1_kp_foot_z: 200.0 37 | 38 | a1_kd_foot_x: 4.0 39 | a1_kd_foot_y: 4.0 40 | a1_kd_foot_z: 4.0 41 | 42 | a1_km_foot_x: 0.1 43 | a1_km_foot_y: 0.1 44 | a1_km_foot_z: 0.1 45 | 46 | # foot stance force controller parameters 47 | # root_acc = [root_ddot_p_deisre; root_ddot_w_desire] 48 | # root_ddot_p_deisre = kp_linear (pos error) 49 | # + kd_linear (linear velocity error) 50 | # root_ddot_w_desire = kp_angular (euler error) 51 | # + kd_linear (rotation velocity error) 52 | # root_acc are used to compute ground contact force 53 | 54 | a1_kp_linear_x: 100.0 55 | a1_kp_linear_y: 100.0 56 | a1_kp_linear_z: 300.0 57 | 58 | a1_kd_linear_x: 70.0 59 | a1_kd_linear_y: 70.0 60 | a1_kd_linear_z: 120.0 61 | 62 | a1_kp_angular_x: 150.0 63 | a1_kp_angular_y: 150.0 64 | a1_kp_angular_z: 1.0 65 | 66 | a1_kd_angular_x: 4.5 67 | a1_kd_angular_y: 4.5 68 | a1_kd_angular_z: 30.0 69 | -------------------------------------------------------------------------------- /src/a1_cpp/config/hardware_a1_mpc.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: false 2 | a1_hardware_power_level: 10 3 | stance_leg_control_type: 1 # 0: QP; 1: MPC 4 | 5 | # physics parameters 6 | 7 | a1_robot_mass: 13.5 8 | 9 | a1_trunk_inertia_xx: 0.0168352186 10 | a1_trunk_inertia_xy: 0.0 11 | a1_trunk_inertia_xz: 0.0 12 | a1_trunk_inertia_yz: 0.0 13 | a1_trunk_inertia_yy: 0.0656071082 14 | a1_trunk_inertia_zz: 0.0742720659 15 | 16 | # default foothold position 17 | 18 | a1_default_foot_pos_FL_x: 0.17 19 | a1_default_foot_pos_FL_y: 0.15 20 | a1_default_foot_pos_FL_z: -0.3 21 | 22 | a1_default_foot_pos_FR_x: 0.17 23 | a1_default_foot_pos_FR_y: -0.15 24 | a1_default_foot_pos_FR_z: -0.3 25 | 26 | a1_default_foot_pos_RL_x: -0.17 27 | a1_default_foot_pos_RL_y: 0.15 28 | a1_default_foot_pos_RL_z: -0.3 29 | 30 | a1_default_foot_pos_RR_x: -0.17 31 | a1_default_foot_pos_RR_y: -0.15 32 | a1_default_foot_pos_RR_z: -0.3 33 | 34 | a1_gait_counter_speed_FL: 1.4 35 | a1_gait_counter_speed_FR: 1.4 36 | a1_gait_counter_speed_RL: 1.4 37 | a1_gait_counter_speed_RR: 1.4 38 | 39 | # MPC parameters 40 | 41 | q_weights_0: 150.0 # roll 42 | q_weights_1: 150.0 # pitch 43 | q_weights_2: 50.0 # yaw 44 | 45 | q_weights_3: 0.0 # pos x 46 | q_weights_4: 0.0 # pos y 47 | q_weights_5: 80.0 # pos z 48 | 49 | q_weights_6: 0.2 # omega x 50 | q_weights_7: 0.2 # omega y 51 | q_weights_8: 0.2 # omega z 52 | 53 | q_weights_9: 0.3 # vel x 54 | q_weights_10: 0.3 # vel y 55 | q_weights_11: 0.3 # vel z 56 | 57 | q_weights_12: 0.0 58 | 59 | r_weights_0: 0.01 60 | r_weights_1: 0.01 61 | r_weights_2: 0.001 62 | 63 | r_weights_3: 0.01 64 | r_weights_4: 0.01 65 | r_weights_5: 0.001 66 | 67 | r_weights_6: 0.01 68 | r_weights_7: 0.01 69 | r_weights_8: 0.001 70 | 71 | r_weights_9: 0.01 72 | r_weights_10: 0.01 73 | r_weights_11: 0.001 74 | 75 | # foot swing controller parameters 76 | # force_tgt = km_foot * kp_foot * foot_pos_error 77 | 78 | a1_kp_foot_x: 120.0 79 | a1_kp_foot_y: 120.0 80 | a1_kp_foot_z: 80.0 81 | 82 | a1_kd_foot_x: 6.0 83 | a1_kd_foot_y: 6.0 84 | a1_kd_foot_z: 5.0 85 | 86 | a1_km_foot_x: 0.1 87 | a1_km_foot_y: 0.1 88 | a1_km_foot_z: 0.1 89 | -------------------------------------------------------------------------------- /src/a1_cpp/config/hardware_a1_mpc_new.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: false 2 | a1_hardware_power_level: 9 3 | stance_leg_control_type: 1 # 0: QP; 1: MPC 4 | 5 | # physics parameters 6 | 7 | a1_robot_mass: 12.5 8 | 9 | a1_trunk_inertia_xx: 0.0178533 10 | a1_trunk_inertia_xy: 0.0 11 | a1_trunk_inertia_xz: 0.0 12 | a1_trunk_inertia_yz: 0.0 13 | a1_trunk_inertia_yy: 0.0377999 14 | a1_trunk_inertia_zz: 0.0456542 15 | 16 | # default foothold position 17 | 18 | a1_default_foot_pos_FL_x: 0.17 19 | a1_default_foot_pos_FL_y: 0.12 20 | a1_default_foot_pos_FL_z: -0.32 21 | 22 | a1_default_foot_pos_FR_x: 0.17 23 | a1_default_foot_pos_FR_y: -0.12 24 | a1_default_foot_pos_FR_z: -0.32 25 | 26 | a1_default_foot_pos_RL_x: -0.17 27 | a1_default_foot_pos_RL_y: 0.12 28 | a1_default_foot_pos_RL_z: -0.32 29 | 30 | a1_default_foot_pos_RR_x: -0.17 31 | a1_default_foot_pos_RR_y: -0.12 32 | a1_default_foot_pos_RR_z: -0.32 33 | 34 | a1_gait_counter_speed_FL: 0.55 35 | a1_gait_counter_speed_FR: 0.55 36 | a1_gait_counter_speed_RL: 0.55 37 | a1_gait_counter_speed_RR: 0.55 38 | 39 | # MPC parameters 40 | 41 | q_weights_0: 10.0 # roll 60 42 | q_weights_1: 20.0 # pitch 80 43 | q_weights_2: 60.0 # yaw 44 | 45 | q_weights_3: 0 # pos x 46 | q_weights_4: 0 # pos y 47 | q_weights_5: 50 # pos z 48 | 49 | q_weights_6: 0.02 # omega x 50 | q_weights_7: 0.02 # omega y 51 | q_weights_8: 10.0 # omega z 52 | 53 | q_weights_9: 5 # vel x 54 | q_weights_10: 5 # vel y 55 | q_weights_11: 1 # vel z 56 | 57 | 58 | q_weights_12: 0.0 59 | 60 | r_weights_0: 0.000011 61 | r_weights_1: 0.000011 62 | r_weights_2: 0.000011 63 | 64 | r_weights_3: 0.000011 65 | r_weights_4: 0.000011 66 | r_weights_5: 0.000011 67 | 68 | r_weights_6: 0.000011 69 | r_weights_7: 0.000011 70 | r_weights_8: 0.000011 71 | 72 | r_weights_9: 0.000011 73 | r_weights_10: 0.000011 74 | r_weights_11: 0.000011 75 | 76 | # foot swing controller parameters 77 | # force_tgt = km_foot * kp_foot * foot_pos_error 78 | 79 | a1_kp_foot_x: 40.0 80 | a1_kp_foot_y: 40.0 81 | a1_kp_foot_z: 30.0 82 | 83 | a1_kd_foot_x: 2.0 84 | a1_kd_foot_y: 2.0 85 | a1_kd_foot_z: 2.0 86 | 87 | a1_km_foot_x: 0.1 88 | a1_km_foot_y: 0.1 89 | a1_km_foot_z: 0.1 -------------------------------------------------------------------------------- /src/a1_cpp/config/hardware_a1_qp.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: false 2 | a1_hardware_power_level: 8 3 | stance_leg_control_type: 0 # 0: QP; 1: MPC 4 | 5 | # physics parameters 6 | 7 | a1_robot_mass: 15.0 8 | 9 | # default foothold position 10 | 11 | a1_default_foot_pos_FL_x: 0.17 12 | a1_default_foot_pos_FL_y: 0.12 13 | a1_default_foot_pos_FL_z: -0.32 14 | 15 | a1_default_foot_pos_FR_x: 0.17 16 | a1_default_foot_pos_FR_y: -0.12 17 | a1_default_foot_pos_FR_z: -0.32 18 | 19 | a1_default_foot_pos_RL_x: -0.17 20 | a1_default_foot_pos_RL_y: 0.12 21 | a1_default_foot_pos_RL_z: -0.32 22 | 23 | a1_default_foot_pos_RR_x: -0.17 24 | a1_default_foot_pos_RR_y: -0.12 25 | a1_default_foot_pos_RR_z: -0.32 26 | 27 | a1_gait_counter_speed_FL: 0.2 28 | a1_gait_counter_speed_FR: 0.2 29 | a1_gait_counter_speed_RL: 0.2 30 | a1_gait_counter_speed_RR: 0.2 31 | 32 | # foot swing controller parameters 33 | # force_tgt = km_foot * kp_foot * foot_pos_error 34 | 35 | a1_kp_foot_x: 160.0 36 | a1_kp_foot_y: 160.0 37 | a1_kp_foot_z: 250.0 38 | 39 | a1_kd_foot_x: 1.5 40 | a1_kd_foot_y: 1.5 41 | a1_kd_foot_z: 1.5 42 | 43 | a1_km_foot_x: 0.1 44 | a1_km_foot_y: 0.1 45 | a1_km_foot_z: 0.1 46 | 47 | # foot stance force controller parameters 48 | # root_acc = [root_ddot_p_deisre; root_ddot_w_desire] 49 | # root_ddot_p_deisre = kp_linear (pos error) 50 | # + kd_linear (linear velocity error) 51 | # root_ddot_w_desire = kp_angular (euler error) 52 | # + kd_linear (rotation velocity error) 53 | # root_acc are used to compute ground contact force 54 | 55 | a1_kp_linear_x: 600.0 56 | a1_kp_linear_y: 600.0 57 | a1_kp_linear_z: 1500.0 58 | 59 | a1_kd_linear_x: 150.0 60 | a1_kd_linear_y: 150.0 61 | a1_kd_linear_z: 200.0 62 | 63 | a1_kp_angular_x: 30.0 64 | a1_kp_angular_y: 30.0 65 | a1_kp_angular_z: 10.0 66 | 67 | a1_kd_angular_x: 1.0 68 | a1_kd_angular_y: 1.0 69 | a1_kd_angular_z: 3.5 70 | -------------------------------------------------------------------------------- /src/a1_cpp/config/isaac_a1_mpc.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: true 2 | stance_leg_control_type: 1 # 0: QP; 1: MPC 3 | use_terrain_adapt: 0 # 0 do not use terrain adaptation 4 | 5 | # physics parameters 6 | 7 | a1_robot_mass: 12.0 8 | 9 | a1_trunk_inertia_xx: 0.0158533 10 | a1_trunk_inertia_xy: 0.0 11 | a1_trunk_inertia_xz: 0.0 12 | a1_trunk_inertia_yz: 0.0 13 | a1_trunk_inertia_yy: 0.0377999 14 | a1_trunk_inertia_zz: 0.0456542 15 | 16 | # default foothold position 17 | 18 | a1_default_foot_pos_FL_x: 0.24 19 | a1_default_foot_pos_FL_y: 0.15 20 | a1_default_foot_pos_FL_z: -0.35 21 | 22 | a1_default_foot_pos_FR_x: 0.24 23 | a1_default_foot_pos_FR_y: -0.15 24 | a1_default_foot_pos_FR_z: -0.35 25 | 26 | a1_default_foot_pos_RL_x: -0.17 27 | a1_default_foot_pos_RL_y: 0.15 28 | a1_default_foot_pos_RL_z: -0.35 29 | 30 | a1_default_foot_pos_RR_x: -0.17 31 | a1_default_foot_pos_RR_y: -0.15 32 | a1_default_foot_pos_RR_z: -0.35 33 | 34 | a1_gait_counter_speed_FL: 2.5 35 | a1_gait_counter_speed_FR: 2.5 36 | a1_gait_counter_speed_RL: 2.5 37 | a1_gait_counter_speed_RR: 2.5 38 | 39 | # MPC parameters 40 | 41 | q_weights_0: 100.0 # roll 42 | q_weights_1: 100.0 # pitch 43 | q_weights_2: 50.0 # yaw 44 | 45 | q_weights_3: 0.0 # pos x 46 | q_weights_4: 0.0 # pos y 47 | q_weights_5: 420.0 # pos z 48 | 49 | q_weights_6: 0.01 # omega x 50 | q_weights_7: 0.01 # omega y 51 | q_weights_8: 0.05 # omega z 52 | 53 | q_weights_9: 30.0 # vel x 54 | q_weights_10: 30.0 # vel y 55 | q_weights_11: 10.0 # vel z 56 | 57 | q_weights_12: 0.0 58 | 59 | r_weights_0: 1e-7 60 | r_weights_1: 1e-7 61 | r_weights_2: 1e-7 62 | 63 | r_weights_3: 1e-7 64 | r_weights_4: 1e-7 65 | r_weights_5: 1e-7 66 | 67 | r_weights_6: 1e-7 68 | r_weights_7: 1e-7 69 | r_weights_8: 1e-7 70 | 71 | r_weights_9: 1e-7 72 | r_weights_10: 1e-7 73 | r_weights_11: 1e-7 74 | 75 | # foot swing controller parameters 76 | # force_tgt = km_foot * kp_foot * foot_pos_error 77 | 78 | a1_kp_foot_x: 3250.0 79 | a1_kp_foot_y: 3250.0 80 | a1_kp_foot_z: 4000.0 81 | 82 | a1_kd_foot_x: 5.0 83 | a1_kd_foot_y: 5.0 84 | a1_kd_foot_z: 5.0 85 | 86 | a1_km_foot_x: 0.5 87 | a1_km_foot_y: 0.5 88 | a1_km_foot_z: 0.5 89 | -------------------------------------------------------------------------------- /src/a1_cpp/config/isaac_a1_qp.yaml: -------------------------------------------------------------------------------- 1 | use_sim_time: true 2 | stance_leg_control_type: 0 # 0: QP; 1: MPC 3 | 4 | # physics parameters 5 | 6 | a1_robot_mass: 12.0 7 | 8 | # default foothold position 9 | 10 | a1_default_foot_pos_FL_x: 0.25 11 | a1_default_foot_pos_FL_y: 0.15 12 | a1_default_foot_pos_FL_z: -0.33 13 | 14 | a1_default_foot_pos_FR_x: 0.25 15 | a1_default_foot_pos_FR_y: -0.15 16 | a1_default_foot_pos_FR_z: -0.33 17 | 18 | a1_default_foot_pos_RL_x: -0.17 19 | a1_default_foot_pos_RL_y: 0.15 20 | a1_default_foot_pos_RL_z: -0.33 21 | 22 | a1_default_foot_pos_RR_x: -0.17 23 | a1_default_foot_pos_RR_y: -0.15 24 | a1_default_foot_pos_RR_z: -0.33 25 | 26 | a1_gait_counter_speed_FL: 2.0 27 | a1_gait_counter_speed_FR: 2.0 28 | a1_gait_counter_speed_RL: 2.0 29 | a1_gait_counter_speed_RR: 2.0 30 | 31 | # foot swing controller parameters 32 | # force_tgt = km_foot * kp_foot * foot_pos_error 33 | 34 | a1_kp_foot_x: 4250.0 35 | a1_kp_foot_y: 4250.0 36 | a1_kp_foot_z: 3000.0 37 | 38 | a1_kd_foot_x: 0.0 39 | a1_kd_foot_y: 0.0 40 | a1_kd_foot_z: 0.0 41 | 42 | a1_km_foot_x: 0.5 43 | a1_km_foot_y: 0.5 44 | a1_km_foot_z: 0.5 45 | 46 | # foot stance force controller parameters 47 | # root_acc = [root_ddot_p_deisre; root_ddot_w_desire] 48 | # root_ddot_p_deisre = kp_linear (pos error) 49 | # + kd_linear (linear velocity error) 50 | # root_ddot_w_desire = kp_angular (euler error) 51 | # + kd_linear (rotation velocity error) 52 | # root_acc are used to compute ground contact force 53 | 54 | a1_kp_linear_x: 1450.0 55 | a1_kp_linear_y: 1450.0 56 | a1_kp_linear_z: 3800.0 57 | 58 | a1_kd_linear_x: 2600.0 59 | a1_kd_linear_y: 2600.0 60 | a1_kd_linear_z: 0.0 61 | 62 | a1_kp_angular_x: 420.0 63 | a1_kp_angular_y: 420.0 64 | a1_kp_angular_z: 150.0 65 | 66 | a1_kd_angular_x: 0.0 67 | a1_kd_angular_y: 0.0 68 | a1_kd_angular_z: 560.0 69 | -------------------------------------------------------------------------------- /src/a1_cpp/launch/a1_ctrl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/a1_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | a1_cpp 4 | 0.0.1 5 | The A1 Cpp Controller 6 | 7 | 8 | 9 | 10 | shuo 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | unitree_legged_msgs 47 | roscpp 48 | rospy 49 | std_msgs 50 | unitree_legged_msgs 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /src/a1_cpp/src/A1BasicEKF.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/1/21. 3 | // 4 | 5 | #include "A1BasicEKF.h" 6 | 7 | A1BasicEKF::A1BasicEKF () { 8 | // constructor 9 | eye3.setIdentity(); 10 | // C is fixed 11 | C.setZero(); 12 | for (int i=0; i(i*3,0) = -eye3; //-pos 14 | C.block<3,3>(i*3,6+i*3) = eye3; //foot pos 15 | C.block<3,3>(NUM_LEG*3+i*3,3) = eye3; // vel 16 | C(NUM_LEG*6+i,6+i*3+2) = 1; // height z of foot 17 | } 18 | 19 | // Q R are fixed 20 | Q.setIdentity(); 21 | Q.block<3,3>(0,0) = PROCESS_NOISE_PIMU*eye3; // position transition 22 | Q.block<3,3>(3,3) = PROCESS_NOISE_VIMU*eye3; // velocity transition 23 | for (int i=0; i(6+i*3,6+i*3) = PROCESS_NOISE_PFOOT*eye3; // foot position transition 25 | } 26 | 27 | R.setIdentity(); 28 | for (int i=0; i(i*3,i*3) = SENSOR_NOISE_PIMU_REL_FOOT*eye3; // fk estimation 30 | R.block<3,3>(NUM_LEG*3+i*3,NUM_LEG*3+i*3) = SENSOR_NOISE_VIMU_REL_FOOT*eye3; // vel estimation 31 | R(NUM_LEG*6+i,NUM_LEG*6+i) = SENSOR_NOISE_ZFOOT; // height z estimation 32 | } 33 | 34 | // set A to identity 35 | A.setIdentity(); 36 | 37 | // set B to zero 38 | B.setZero(); 39 | 40 | assume_flat_ground = true; 41 | 42 | } 43 | 44 | A1BasicEKF::A1BasicEKF (bool assume_flat_ground_):A1BasicEKF() { 45 | // constructor 46 | assume_flat_ground = assume_flat_ground_; 47 | // change R according to this flag, if we do not assume the robot moves on flat ground, 48 | // then we cannot infer height z using this way 49 | if (assume_flat_ground == false) { 50 | for (int i=0; i(0) = Eigen::Vector3d(0, 0, 0.09); 64 | for (int i = 0; i < NUM_LEG; ++i) { 65 | Eigen::Vector3d fk_pos = state.foot_pos_rel.block<3, 1>(0, i); 66 | x.segment<3>(6 + i * 3) = state.root_rot_mat * fk_pos + x.segment<3>(0); 67 | } 68 | } 69 | 70 | void A1BasicEKF::update_estimation(A1CtrlStates& state, double dt) { 71 | // update A B using latest dt 72 | A.block<3, 3>(0, 3) = dt * eye3; 73 | B.block<3, 3>(3, 0) = dt * eye3; 74 | 75 | // control input u is Ra + ag 76 | Eigen::Vector3d u = state.root_rot_mat * state.imu_acc + Eigen::Vector3d(0, 0, -9.81); 77 | 78 | // contact estimation, do something very simple first 79 | if (state.movement_mode == 0) { // stand 80 | for (int i = 0; i < NUM_LEG; ++i) estimated_contacts[i] = 1.0; 81 | } else { // walk 82 | for (int i = 0; i < NUM_LEG; ++i) { 83 | estimated_contacts[i] = std::min(std::max((state.foot_force(i)) / (100.0 - 0.0), 0.0), 1.0); 84 | // estimated_contacts[i] = 1.0/(1.0+std::exp(-(state.foot_force(i)-100))); 85 | } 86 | } 87 | // update Q 88 | Q.block<3, 3>(0, 0) = PROCESS_NOISE_PIMU * dt / 20.0 * eye3; 89 | Q.block<3, 3>(3, 3) = PROCESS_NOISE_VIMU * dt * 9.8 / 20.0 * eye3; 90 | // update Q R for legs not in contact 91 | for (int i = 0; i < NUM_LEG; ++i) { 92 | Q.block<3, 3>(6 + i * 3, 6 + i * 3) 93 | = 94 | (1 + (1 - estimated_contacts[i]) * 1e3) * dt * PROCESS_NOISE_PFOOT * eye3; // foot position transition 95 | // for estimated_contacts[i] == 1, Q = 0.002 96 | // for estimated_contacts[i] == 0, Q = 1001*Q 97 | 98 | R.block<3, 3>(i * 3, i * 3) 99 | = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_PIMU_REL_FOOT * 100 | eye3; // fk estimation 101 | R.block<3, 3>(NUM_LEG * 3 + i * 3, NUM_LEG * 3 + i * 3) 102 | = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_VIMU_REL_FOOT * eye3; // vel estimation 103 | if (assume_flat_ground) { 104 | R(NUM_LEG * 6 + i, NUM_LEG * 6 + i) 105 | = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_ZFOOT; // height z estimation 106 | } 107 | } 108 | 109 | 110 | // process update 111 | xbar = A*x + B*u ; 112 | Pbar = A * P * A.transpose() + Q; 113 | 114 | // measurement construction 115 | yhat = C*xbar; 116 | // leg_v = (-J_rf*av-skew(omega)*p_rf); 117 | // r((i-1)*3+1:(i-1)*3+3) = body_v - R_er*leg_v; 118 | // actual measurement 119 | for (int i=0; i(0,i); 121 | y.block<3,1>(i*3,0) = state.root_rot_mat*fk_pos; // fk estimation 122 | Eigen::Vector3d leg_v = -state.foot_vel_rel.block<3,1>(0,i) - Utils::skew(state.imu_ang_vel)*fk_pos; 123 | y.block<3,1>(NUM_LEG*3+i*3,0) = 124 | (1.0-estimated_contacts[i])*x.segment<3>(3) + estimated_contacts[i]*state.root_rot_mat*leg_v; // vel estimation 125 | 126 | y(NUM_LEG*6+i) = 127 | (1.0-estimated_contacts[i])*(x(2)+fk_pos(2)) + estimated_contacts[i]*0; // height z estimation 128 | } 129 | 130 | S = C * Pbar *C.transpose() + R; 131 | S = 0.5*(S+S.transpose()); 132 | 133 | error_y = y - yhat; 134 | Serror_y = S.fullPivHouseholderQr().solve(error_y); 135 | 136 | x = xbar + Pbar * C.transpose() * Serror_y; 137 | 138 | SC = S.fullPivHouseholderQr().solve(C); 139 | P = Pbar - Pbar * C.transpose() * SC * Pbar; 140 | P = 0.5 * (P + P.transpose()); 141 | 142 | // reduce position drift 143 | if (P.block<2, 2>(0, 0).determinant() > 1e-6) { 144 | P.block<2, 16>(0, 2).setZero(); 145 | P.block<16, 2>(2, 0).setZero(); 146 | P.block<2, 2>(0, 0) /= 10.0; 147 | } 148 | 149 | // final step 150 | // put estimated values back to A1CtrlStates& state 151 | for (int i = 0; i < NUM_LEG; ++i) { 152 | if (estimated_contacts[i] < 0.5) { 153 | state.estimated_contacts[i] = false; 154 | } else { 155 | state.estimated_contacts[i] = true; 156 | } 157 | } 158 | // std::cout << x.transpose() <(0); 160 | state.estimated_root_vel = x.segment<3>(3); 161 | 162 | state.root_pos = x.segment<3>(0); 163 | state.root_lin_vel = x.segment<3>(3); 164 | } -------------------------------------------------------------------------------- /src/a1_cpp/src/A1BasicEKF.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/1/21. 3 | // 4 | 5 | #ifndef A1_CPP_A1BASICEKF_H 6 | #define A1_CPP_A1BASICEKF_H 7 | 8 | #include "A1Params.h" 9 | #include "A1CtrlStates.h" 10 | #include "utils/Utils.h" 11 | 12 | 13 | // state estimator parameters 14 | #define STATE_SIZE 18 15 | #define MEAS_SIZE 28 16 | #define PROCESS_NOISE_PIMU 0.01 17 | #define PROCESS_NOISE_VIMU 0.01 18 | #define PROCESS_NOISE_PFOOT 0.01 19 | #define SENSOR_NOISE_PIMU_REL_FOOT 0.001 20 | #define SENSOR_NOISE_VIMU_REL_FOOT 0.1 21 | #define SENSOR_NOISE_ZFOOT 0.001 22 | 23 | // implement a basic error state KF to estimate robot pose 24 | // assume orientation is known from a IMU (state.root_rot_mat) 25 | class A1BasicEKF { 26 | public: 27 | A1BasicEKF (); 28 | A1BasicEKF (bool assume_flat_ground_); 29 | void init_state(A1CtrlStates& state); 30 | void update_estimation(A1CtrlStates& state, double dt); 31 | bool is_inited() {return filter_initialized;} 32 | private: 33 | bool filter_initialized = false; 34 | // state 35 | // 0 1 2 pos 3 4 5 vel 6 7 8 foot pos FL 9 10 11 foot pos FR 12 13 14 foot pos RL 15 16 17 foot pos RR 36 | Eigen::Matrix x; // estimation state 37 | Eigen::Matrix xbar; // estimation state after process update 38 | Eigen::Matrix P; // estimation state covariance 39 | Eigen::Matrix Pbar; // estimation state covariance after process update 40 | Eigen::Matrix A; // estimation state transition 41 | Eigen::Matrix B; // estimation state transition 42 | Eigen::Matrix Q; // estimation state transition noise 43 | 44 | // observation 45 | // 0 1 2 FL pos residual 46 | // 3 4 5 FR pos residual 47 | // 6 7 8 RL pos residual 48 | // 9 10 11 RR pos residual 49 | // 12 13 14 vel residual from FL 50 | // 15 16 17 vel residual from FR 51 | // 18 19 20 vel residual from RL 52 | // 21 22 23 vel residual from RR 53 | // 24 25 26 27 foot height 54 | Eigen::Matrix y; // observation 55 | Eigen::Matrix yhat; // estimated observation 56 | Eigen::Matrix error_y; // estimated observation 57 | Eigen::Matrix Serror_y; // S^-1*error_y 58 | Eigen::Matrix C; // estimation state observation 59 | Eigen::Matrix SC; // S^-1*C 60 | Eigen::Matrix R; // estimation state observation noise 61 | // helper matrices 62 | Eigen::Matrix eye3; // 3x3 identity 63 | Eigen::Matrix S; // Innovation (or pre-fit residual) covariance 64 | Eigen::Matrix K; // kalman gain 65 | 66 | 67 | bool assume_flat_ground = false; 68 | 69 | // variables to process foot force 70 | double smooth_foot_force[4]; 71 | double estimated_contacts[4]; 72 | }; 73 | 74 | 75 | #endif //A1_CPP_A1BASICEKF_H 76 | -------------------------------------------------------------------------------- /src/a1_cpp/src/A1Params.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/18/21. 3 | // 4 | 5 | #ifndef A1_CPP_A1PARAMS_H 6 | #define A1_CPP_A1PARAMS_H 7 | 8 | // control time related 9 | //#define CTRL_FREQUENCY 2.5 // ms 10 | #define GRF_UPDATE_FREQUENCY 0.5 // ms 11 | #define MAIN_UPDATE_FREQUENCY 0.5 // ms 12 | #define HARDWARE_FEEDBACK_FREQUENCY 1.0 // ms 13 | 14 | // constant define 15 | // joy stick command interprate 16 | #define JOY_CMD_BODY_HEIGHT_MAX 0.32 // m 17 | #define JOY_CMD_BODY_HEIGHT_MIN 0.1 // m 18 | #define JOY_CMD_BODY_HEIGHT_VEL 0.04 // m/s 19 | #define JOY_CMD_VELX_MAX 0.6 // m/s 20 | #define JOY_CMD_VELY_MAX 0.3 // m/s 21 | #define JOY_CMD_YAW_MAX 0.8 // rad 22 | #define JOY_CMD_PITCH_MAX 0.4 // rad 23 | #define JOY_CMD_ROLL_MAX 0.4 // rad 24 | 25 | // mpc 26 | #define PLAN_HORIZON 10 27 | #define MPC_STATE_DIM 13 28 | #define MPC_CONSTRAINT_DIM 20 29 | 30 | // robot constant 31 | #define NUM_LEG 4 32 | #define NUM_DOF_PER_LEG 3 33 | #define DIM_GRF 12 34 | #define NUM_DOF 12 35 | 36 | #define LOWER_LEG_LENGTH 0.21 37 | 38 | #define FOOT_FORCE_LOW 30.0 39 | #define FOOT_FORCE_HIGH 80.0 40 | 41 | #define FOOT_SWING_CLEARANCE1 0.0f 42 | #define FOOT_SWING_CLEARANCE2 0.4f 43 | 44 | #define FOOT_DELTA_X_LIMIT 0.1 45 | #define FOOT_DELTA_Y_LIMIT 0.1 46 | 47 | #define ERROR_CURVE_ALREADY_SET 184 48 | #define ERROR_CURVE_NOT_SET 185 49 | 50 | #endif //A1_CPP_A1PARAMS_H 51 | -------------------------------------------------------------------------------- /src/a1_cpp/src/A1RobotControl.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/19/21. 3 | // 4 | 5 | #ifndef A1_CPP_A1ROBOTCONTROL_H 6 | #define A1_CPP_A1ROBOTCONTROL_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | // to debug 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // osqp-eigen 21 | #include "OsqpEigen/OsqpEigen.h" 22 | #include 23 | 24 | #include "A1Params.h" 25 | #include "A1CtrlStates.h" 26 | #include "utils/Utils.h" 27 | #include "ConvexMpc.h" 28 | 29 | #include "utils/filter.hpp" 30 | 31 | 32 | class A1RobotControl { 33 | public: 34 | A1RobotControl(); 35 | 36 | A1RobotControl(ros::NodeHandle &_nh); 37 | 38 | void update_plan(A1CtrlStates &state, double dt); 39 | 40 | void generate_swing_legs_ctrl(A1CtrlStates &state, double dt); 41 | 42 | void compute_joint_torques(A1CtrlStates &state); 43 | 44 | Eigen::Matrix compute_grf(A1CtrlStates &state, double dt); 45 | 46 | Eigen::Vector3d compute_walking_surface(A1CtrlStates &state); 47 | 48 | private: 49 | BezierUtils bezierUtils[NUM_LEG]; 50 | 51 | Eigen::Matrix root_acc; 52 | // allocate the problem weight matrices 53 | Eigen::DiagonalMatrix Q; 54 | double R; 55 | // ground friction coefficient 56 | double mu; 57 | double F_min; 58 | double F_max; 59 | // allocate QP problem matrices and vectors 60 | Eigen::SparseMatrix hessian; 61 | Eigen::VectorXd gradient; 62 | Eigen::SparseMatrix linearMatrix; 63 | Eigen::VectorXd lowerBound; 64 | Eigen::VectorXd upperBound; 65 | 66 | 67 | OsqpEigen::Solver solver; 68 | 69 | //add a number of ROS debug topics 70 | ros::NodeHandle nh; 71 | ros::Publisher pub_foot_start[NUM_LEG]; 72 | ros::Publisher pub_foot_end[NUM_LEG]; 73 | ros::Publisher pub_foot_path[NUM_LEG]; 74 | visualization_msgs::Marker foot_start_marker[NUM_LEG]; 75 | visualization_msgs::Marker foot_end_marker[NUM_LEG]; 76 | visualization_msgs::Marker foot_path_marker[NUM_LEG]; 77 | 78 | //debug topics 79 | // ros::Publisher pub_root_lin_vel; 80 | // ros::Publisher pub_root_lin_vel_d; 81 | ros::Publisher pub_terrain_angle; 82 | 83 | ros::Publisher pub_foot_pose_target_FL; 84 | ros::Publisher pub_foot_pose_target_FR; 85 | ros::Publisher pub_foot_pose_target_RL; 86 | ros::Publisher pub_foot_pose_target_RR; 87 | 88 | ros::Publisher pub_foot_pose_target_rel_FL; 89 | ros::Publisher pub_foot_pose_target_rel_FR; 90 | ros::Publisher pub_foot_pose_target_rel_RL; 91 | ros::Publisher pub_foot_pose_target_rel_RR; 92 | 93 | ros::Publisher pub_foot_pose_error_FL; 94 | ros::Publisher pub_foot_pose_error_FR; 95 | ros::Publisher pub_foot_pose_error_RL; 96 | ros::Publisher pub_foot_pose_error_RR; 97 | 98 | ros::Publisher pub_euler; 99 | 100 | //MPC does not start for the first 10 ticks to prevent uninitialized NAN goes into joint_torques 101 | int mpc_init_counter; 102 | 103 | std::string use_sim_time; 104 | 105 | // filters 106 | MovingWindowFilter terrain_angle_filter; 107 | MovingWindowFilter recent_contact_x_filter[NUM_LEG]; 108 | MovingWindowFilter recent_contact_y_filter[NUM_LEG]; 109 | MovingWindowFilter recent_contact_z_filter[NUM_LEG]; 110 | }; 111 | 112 | 113 | #endif //A1_CPP_A1ROBOTCONTROL_H 114 | -------------------------------------------------------------------------------- /src/a1_cpp/src/ConvexMpc.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zixin on 12/09/21. 3 | // 4 | 5 | #ifndef A1_CPP_CONVEXMPC_H 6 | #define A1_CPP_CONVEXMPC_H 7 | 8 | #define EIGEN_STACK_ALLOCATION_LIMIT 0 9 | 10 | #include 11 | #include 12 | 13 | #include "OsqpEigen/OsqpEigen.h" 14 | #include 15 | #include 16 | #include 17 | 18 | #include "A1CtrlStates.h" 19 | #include "A1Params.h" 20 | #include "utils/Utils.h" 21 | 22 | class ConvexMpc { 23 | public: 24 | ConvexMpc(Eigen::VectorXd &q_weights_, Eigen::VectorXd &r_weights_); 25 | 26 | void reset(); 27 | 28 | void calculate_A_mat_c(Eigen::Vector3d root_euler); 29 | 30 | void calculate_B_mat_c(double robot_mass, const Eigen::Matrix3d &a1_trunk_inertia, Eigen::Matrix3d root_rot_mat, 31 | Eigen::Matrix foot_pos); 32 | 33 | void state_space_discretization(double dt); 34 | 35 | void calculate_qp_mats(A1CtrlStates &state); 36 | 37 | //private: 38 | // parameters initialized with class ConvexMpc 39 | double mu; 40 | double fz_min; 41 | double fz_max; 42 | 43 | // Eigen::VectorXd q_weights_mpc; // (state_dim * horizon) x 1 44 | // Eigen::VectorXd r_weights_mpc; // (action_dim * horizon) x 1 45 | Eigen::Matrix q_weights_mpc; 46 | Eigen::Matrix r_weights_mpc; 47 | 48 | Eigen::DiagonalMatrix Q; 49 | Eigen::DiagonalMatrix R; 50 | Eigen::SparseMatrix Q_sparse; 51 | Eigen::SparseMatrix R_sparse; 52 | // Eigen::Matrix Q; 53 | // Eigen::Matrix R; 54 | 55 | // parameters initialized in the function reset() 56 | // Eigen::MatrixXd A_mat_c; 57 | // Eigen::MatrixXd B_mat_c; 58 | // Eigen::MatrixXd AB_mat_c; 59 | // 60 | // Eigen::MatrixXd A_mat_d; 61 | // Eigen::MatrixXd B_mat_d; 62 | // Eigen::MatrixXd AB_mat_d; 63 | // 64 | // Eigen::MatrixXd A_qp; 65 | // Eigen::MatrixXd B_qp; 66 | 67 | Eigen::Matrix A_mat_c; 68 | Eigen::Matrix B_mat_c; 69 | Eigen::Matrix B_mat_c_list; 70 | Eigen::Matrix AB_mat_c; 71 | 72 | Eigen::Matrix A_mat_d; 73 | Eigen::Matrix B_mat_d; 74 | Eigen::Matrix B_mat_d_list; 75 | Eigen::Matrix AB_mat_d; 76 | 77 | Eigen::Matrix A_qp; 78 | Eigen::Matrix B_qp; 79 | Eigen::SparseMatrix A_qp_sparse; 80 | Eigen::SparseMatrix B_qp_sparse; 81 | 82 | // standard QP formulation 83 | // minimize 1/2 * x' * P * x + q' * x 84 | // subject to lb <= Ac * x <= ub 85 | Eigen::SparseMatrix hessian; // P 86 | // Eigen::VectorXd gradient; // q 87 | Eigen::SparseMatrix linear_constraints; // Ac 88 | // Eigen::VectorXd lb; 89 | // Eigen::VectorXd ub; 90 | Eigen::Matrix gradient; // q 91 | Eigen::Matrix lb; 92 | Eigen::Matrix ub; 93 | 94 | }; 95 | 96 | #endif //A1_CPP_CONVEXMPC_H 97 | -------------------------------------------------------------------------------- /src/a1_cpp/src/GazeboA1ROS.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zixin on 11/1/21. 3 | // 4 | 5 | #ifndef A1_CPP_GAZEBOA1ROS_H 6 | #define A1_CPP_GAZEBOA1ROS_H 7 | 8 | // std 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // ROS 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | // #include 36 | 37 | // control parameters 38 | #include "A1Params.h" 39 | #include "A1CtrlStates.h" 40 | #include "A1RobotControl.h" 41 | #include "A1BasicEKF.h" 42 | #include "legKinematics/A1Kinematics.h" 43 | #include "utils/Utils.h" 44 | 45 | #include "utils/filter.hpp" 46 | 47 | class GazeboA1ROS { 48 | public: 49 | GazeboA1ROS(ros::NodeHandle &_nh); 50 | 51 | bool update_foot_forces_grf(double dt); 52 | 53 | bool main_update(double t, double dt); 54 | 55 | bool send_cmd(); 56 | 57 | // callback functions 58 | void gt_pose_callback(const nav_msgs::Odometry::ConstPtr &odom); 59 | 60 | void imu_callback(const sensor_msgs::Imu::ConstPtr &imu); 61 | 62 | void joy_callback(const sensor_msgs::Joy::ConstPtr &joy_msg); 63 | 64 | void FL_hip_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 65 | 66 | void FL_thigh_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 67 | 68 | void FL_calf_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 69 | 70 | void FR_hip_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 71 | 72 | void FR_thigh_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 73 | 74 | void FR_calf_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 75 | 76 | void RL_hip_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 77 | 78 | void RL_thigh_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 79 | 80 | void RL_calf_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 81 | 82 | void RR_hip_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 83 | 84 | void RR_thigh_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 85 | 86 | void RR_calf_state_callback(const unitree_legged_msgs::MotorState &a1_joint_state); 87 | 88 | void FL_foot_contact_callback(const geometry_msgs::WrenchStamped &force); 89 | 90 | void FR_foot_contact_callback(const geometry_msgs::WrenchStamped &force); 91 | 92 | void RL_foot_contact_callback(const geometry_msgs::WrenchStamped &force); 93 | 94 | void RR_foot_contact_callback(const geometry_msgs::WrenchStamped &force); 95 | 96 | // terminal state check 97 | bool isTerminalState(Eigen::Matrix pos); 98 | 99 | 100 | private: 101 | ros::NodeHandle nh; 102 | 103 | // 0, 1, 2: FL_hip, FL_thigh, FL_calf 104 | // 3, 4, 5: FR_hip, FR_thigh, FR_calf 105 | // 6, 7, 8: RL_hip, RL_thigh, RL_calf 106 | // 9, 10, 11: RR_hip, RR_thigh, RR_calf 107 | ros::Publisher pub_joint_cmd[12]; 108 | ros::Subscriber sub_joint_msg[12]; 109 | ros::Publisher pub_euler_d; 110 | 111 | // 0, 1, 2, 3: FL, FR, RL, RR 112 | ros::Subscriber sub_foot_contact_msg[4]; 113 | ros::Subscriber sub_gt_pose_msg; 114 | ros::Subscriber sub_imu_msg; 115 | ros::Subscriber sub_joy_msg; 116 | 117 | // debug estimation 118 | ros::Publisher pub_estimated_pose; 119 | 120 | // joystick command 121 | double joy_cmd_velx = 0.0; 122 | double joy_cmd_velx_forward = 0.0; 123 | double joy_cmd_velx_backward = 0.0; 124 | double joy_cmd_vely = 0.0; 125 | double joy_cmd_velz = 0.0; 126 | 127 | double joy_cmd_pitch_rate = 0.0; 128 | double joy_cmd_roll_rate = 0.0; 129 | double joy_cmd_yaw_rate = 0.0; 130 | 131 | double joy_cmd_pitch_ang = 0.0; 132 | double joy_cmd_roll_ang = 0.0; 133 | double joy_cmd_body_height = 0.2; 134 | 135 | // 0 is standing, 1 is walking 136 | int joy_cmd_ctrl_state = 0; 137 | bool joy_cmd_ctrl_state_change_request = false; 138 | int prev_joy_cmd_ctrl_state = 0; 139 | bool joy_cmd_exit = false; 140 | 141 | // following cade is also in VILEOM 142 | // add leg kinematics 143 | // the leg kinematics is relative to body frame, which is the center of the robot 144 | // following are some parameters that defines the transformation between IMU frame(b) and robot body frame(r) 145 | Eigen::Vector3d p_br; 146 | Eigen::Matrix3d R_br; 147 | // for each leg, there is an offset between the body frame and the hip motor (fx, fy) 148 | double leg_offset_x[4] = {}; 149 | double leg_offset_y[4] = {}; 150 | // for each leg, there is an offset between the body frame and the hip motor (fx, fy) 151 | double motor_offset[4] = {}; 152 | double upper_leg_length[4] = {}; 153 | double lower_leg_length[4] = {}; 154 | std::vector rho_fix_list; 155 | std::vector rho_opt_list; 156 | A1Kinematics a1_kin; 157 | // variables related to control 158 | A1CtrlStates a1_ctrl_states; 159 | A1RobotControl _root_control; 160 | A1BasicEKF a1_estimate; 161 | 162 | // filters 163 | MovingWindowFilter acc_x; 164 | MovingWindowFilter acc_y; 165 | MovingWindowFilter acc_z; 166 | MovingWindowFilter gyro_x; 167 | MovingWindowFilter gyro_y; 168 | MovingWindowFilter gyro_z; 169 | MovingWindowFilter quat_w; 170 | MovingWindowFilter quat_x; 171 | MovingWindowFilter quat_y; 172 | MovingWindowFilter quat_z; 173 | 174 | // pos limit 175 | std::vector> posLimits = { 176 | {-1.047, 1.047}, //Hip 177 | {-0.663, 2.966}, //Thigh 178 | {-2.721, -0.837}, //Calf 179 | }; 180 | }; 181 | 182 | 183 | #endif //A1_CPP_GAZEBOA1ROS_H 184 | -------------------------------------------------------------------------------- /src/a1_cpp/src/HardwareA1ROS.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/7/21. 3 | // 4 | 5 | #ifndef A1_CPP_HARDWAREA1ROS_H 6 | #define A1_CPP_HARDWAREA1ROS_H 7 | 8 | // std 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // ROS 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // control parameters 32 | #include "A1Params.h" 33 | #include "A1CtrlStates.h" 34 | #include "A1RobotControl.h" 35 | #include "A1BasicEKF.h" 36 | #include "legKinematics/A1Kinematics.h" 37 | #include "utils/Utils.h" 38 | 39 | // a1 hardware 40 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 41 | 42 | #define FOOT_FILTER_WINDOW_SIZE 5 43 | 44 | // one of the most important thing: A1 hardware return info with FR, FL, RR, RL order and receives info in this order 45 | // we need to take of this order in this function 46 | class HardwareA1ROS { 47 | public: 48 | HardwareA1ROS(ros::NodeHandle &_nh); 49 | 50 | ~HardwareA1ROS() { 51 | destruct = true; 52 | thread_.join(); 53 | } 54 | 55 | bool update_foot_forces_grf(double dt); 56 | 57 | bool main_update(double t, double dt); 58 | 59 | bool send_cmd(); 60 | 61 | void joy_callback(const sensor_msgs::Joy::ConstPtr &joy_msg); 62 | 63 | private: 64 | ros::NodeHandle nh; 65 | ros::Publisher pub_joint_cmd; 66 | ros::Publisher pub_joint_angle; 67 | ros::Publisher pub_imu; 68 | sensor_msgs::JointState joint_foot_msg; 69 | sensor_msgs::Imu imu_msg; 70 | ros::Subscriber sub_joy_msg; 71 | 72 | // debug estimated position 73 | ros::Publisher pub_estimated_pose; 74 | 75 | // a1 hardware 76 | UNITREE_LEGGED_SDK::UDP udp; 77 | UNITREE_LEGGED_SDK::Safety safe; 78 | UNITREE_LEGGED_SDK::LowState state = {0}; 79 | UNITREE_LEGGED_SDK::LowCmd cmd = {0}; 80 | // a1 hardware reading thread 81 | std::thread thread_; 82 | bool destruct = false; 83 | 84 | void udp_init_send(); 85 | 86 | void receive_low_state(); 87 | 88 | // a1 hardware switch foot order 89 | Eigen::Matrix swap_joint_indices; 90 | Eigen::Matrix swap_foot_indices; 91 | 92 | // a1 hardware foot force filter 93 | Eigen::Matrix foot_force_filters; 94 | Eigen::Matrix foot_force_filters_idx; 95 | Eigen::Matrix foot_force_filters_sum; 96 | 97 | 98 | // joystic command 99 | double joy_cmd_velx = 0.0; 100 | double joy_cmd_vely = 0.0; 101 | double joy_cmd_velz = 0.0; 102 | double joy_cmd_roll_rate = 0.0; 103 | double joy_cmd_pitch_rate = 0.0; 104 | double joy_cmd_yaw_rate = 0.0; 105 | double joy_cmd_pitch_ang = 0.0; 106 | double joy_cmd_roll_ang = 0.0; 107 | double joy_cmd_body_height = 0.17; 108 | 109 | // 0 is standing, 1 is walking 110 | int joy_cmd_ctrl_state = 0; 111 | bool joy_cmd_ctrl_state_change_request = false; 112 | int prev_joy_cmd_ctrl_state = 0; 113 | bool joy_cmd_exit = false; 114 | 115 | // following cade is also in VILEOM 116 | // add leg kinematics 117 | // the leg kinematics is relative to body frame, which is the center of the robot 118 | // following are some parameters that defines the transformation between IMU frame(b) and robot body frame(r) 119 | Eigen::Vector3d p_br; 120 | Eigen::Matrix3d R_br; 121 | // for each leg, there is an offset between the body frame and the hip motor (fx, fy) 122 | double leg_offset_x[4] = {}; 123 | double leg_offset_y[4] = {}; 124 | // for each leg, there is an offset between the body frame and the hip motor (fx, fy) 125 | double motor_offset[4] = {}; 126 | double upper_leg_length[4] = {}; 127 | double lower_leg_length[4] = {}; 128 | std::vector rho_fix_list; 129 | std::vector rho_opt_list; 130 | A1Kinematics a1_kin; 131 | // variables related to control and estimation 132 | A1CtrlStates a1_ctrl_states; 133 | A1RobotControl _root_control; 134 | A1BasicEKF a1_estimate; 135 | }; 136 | 137 | #endif //A1_CPP_HARDWAREA1ROS_H 138 | -------------------------------------------------------------------------------- /src/a1_cpp/src/IsaacA1ROS.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/17/21. 3 | // 4 | 5 | #ifndef A1_CPP_ISAACA1ROS_H 6 | #define A1_CPP_ISAACA1ROS_H 7 | 8 | // std 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // ROS 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // control parameters 32 | #include "A1Params.h" 33 | #include "A1CtrlStates.h" 34 | #include "A1RobotControl.h" 35 | #include "A1BasicEKF.h" 36 | #include "legKinematics/A1Kinematics.h" 37 | #include "utils/Utils.h" 38 | 39 | class IsaacA1ROS { 40 | public: 41 | IsaacA1ROS(ros::NodeHandle &_nh); 42 | 43 | bool update_foot_forces_grf(double dt); 44 | 45 | bool main_update(double t, double dt); 46 | 47 | bool send_cmd(); 48 | 49 | // callback functions 50 | void gt_pose_callback(const geometry_msgs::PoseStamped::ConstPtr &pose_data); 51 | 52 | void imu_callback(const sensor_msgs::Imu::ConstPtr &imu); 53 | 54 | void joint_state_callback(const sensor_msgs::JointState::ConstPtr &a1_state); 55 | 56 | void joy_callback(const sensor_msgs::Joy::ConstPtr &joy_msg); 57 | 58 | private: 59 | ros::NodeHandle nh; 60 | ros::Publisher pub_joint_cmd; 61 | ros::Subscriber sub_gt_pose_msg; 62 | ros::Subscriber sub_imu_msg; 63 | ros::Subscriber sub_joint_foot_msg; 64 | ros::Subscriber sub_joy_msg; 65 | 66 | // debug estimated position 67 | ros::Publisher pub_estimated_pose; 68 | 69 | // joystic command 70 | double joy_cmd_velx = 0.0; 71 | double joy_cmd_vely = 0.0; 72 | double joy_cmd_velz = 0.0; 73 | 74 | double joy_cmd_pitch_rate = 0.0; 75 | double joy_cmd_roll_rate = 0.0; 76 | double joy_cmd_yaw_rate = 0.0; 77 | 78 | double joy_cmd_pitch_ang = 0.0; 79 | double joy_cmd_roll_ang = 0.0; 80 | double joy_cmd_body_height = 0.32; 81 | 82 | // 0 is standing, 1 is walking 83 | int joy_cmd_ctrl_state = 0; 84 | bool joy_cmd_ctrl_state_change_request = false; 85 | int prev_joy_cmd_ctrl_state = 0; 86 | bool joy_cmd_exit = false; 87 | 88 | // following cade is also in VILEOM 89 | // add leg kinematics 90 | // the leg kinematics is relative to body frame, which is the center of the robot 91 | // following are some parameters that defines the transformation between IMU frame(b) and robot body frame(r) 92 | Eigen::Vector3d p_br; 93 | Eigen::Matrix3d R_br; 94 | // for each leg, there is an offset between the body frame and the hip motor (fx, fy) 95 | double leg_offset_x[4] = {}; 96 | double leg_offset_y[4] = {}; 97 | // for each leg, there is an offset between the body frame and the hip motor (fx, fy) 98 | double motor_offset[4] = {}; 99 | double upper_leg_length[4] = {}; 100 | double lower_leg_length[4] = {}; 101 | std::vector rho_fix_list; 102 | std::vector rho_opt_list; 103 | A1Kinematics a1_kin; 104 | // variables related to control and estimation 105 | A1CtrlStates a1_ctrl_states; 106 | A1RobotControl _root_control; 107 | A1BasicEKF a1_estimate; 108 | 109 | // filters 110 | MovingWindowFilter acc_x; 111 | MovingWindowFilter acc_y; 112 | MovingWindowFilter acc_z; 113 | MovingWindowFilter gyro_x; 114 | MovingWindowFilter gyro_y; 115 | MovingWindowFilter gyro_z; 116 | }; 117 | 118 | #endif //A1_CPP_ISAACA1ROS_H 119 | -------------------------------------------------------------------------------- /src/a1_cpp/src/MainGazebo.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zixin on 11/1/21. 3 | // 4 | // stl 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // ROS 12 | #include 13 | #include 14 | #include 15 | 16 | // control parameters 17 | #include "A1Params.h" 18 | // A1 control 19 | #include "GazeboA1ROS.h" 20 | 21 | int main(int argc, char **argv) { 22 | ros::init(argc, argv, "gazebo_a1_qp_ctrl"); 23 | ros::NodeHandle nh; 24 | 25 | // change ros logger 26 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { 27 | ros::console::notifyLoggerLevelsChanged(); 28 | } 29 | 30 | // make sure the ROS infra using sim time, otherwise the controller cannot run with correct time steps 31 | std::string use_sim_time; 32 | if (ros::param::get("/use_sim_time", use_sim_time)) { 33 | if (use_sim_time != "true") { 34 | std::cout << "ROS must set use_sim_time in order to use this program!" << std::endl; 35 | return -1; 36 | } 37 | } 38 | 39 | // create a1 controller 40 | std::unique_ptr a1 = std::make_unique(nh); 41 | 42 | std::atomic control_execute{}; 43 | control_execute.store(true, std::memory_order_release); 44 | 45 | // Thread 1: compute desired ground forces 46 | std::cout << "Enter thread 1: compute desired ground forces" << std::endl; 47 | std::thread compute_foot_forces_grf_thread([&]() { 48 | // prepare variables to monitor time and control the while loop 49 | ros::Time start = ros::Time::now(); 50 | ros::Time prev = ros::Time::now(); 51 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 52 | ros::Duration dt(0); 53 | 54 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 55 | // auto t1 = std::chrono::high_resolution_clock::now(); 56 | 57 | ros::Duration(GRF_UPDATE_FREQUENCY / 1000).sleep(); 58 | 59 | // get t and dt 60 | now = ros::Time::now(); 61 | dt = now - prev; 62 | prev = now; 63 | ros::Duration elapsed = now - start; 64 | 65 | auto t1 = std::chrono::high_resolution_clock::now(); 66 | 67 | // compute desired ground forces 68 | bool running = a1->update_foot_forces_grf(dt.toSec()); 69 | 70 | auto t2 = std::chrono::high_resolution_clock::now(); 71 | std::chrono::duration ms_double = t2 - t1; 72 | std::cout << "MPC solution is updated in " << ms_double.count() << "ms" << std::endl; 73 | 74 | if (!running) { 75 | std::cout << "Thread 1 loop is terminated because of errors." << std::endl; 76 | ros::shutdown(); 77 | std::terminate(); 78 | break; 79 | } 80 | } 81 | }); 82 | 83 | // Thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands 84 | std::cout << "Enter thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands" 85 | << std::endl; 86 | std::thread main_thread([&]() { 87 | // prepare variables to monitor time and control the while loop 88 | ros::Time start = ros::Time::now(); 89 | ros::Time prev = ros::Time::now(); 90 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 91 | ros::Duration dt(0); 92 | 93 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 94 | auto t3 = std::chrono::high_resolution_clock::now(); 95 | 96 | ros::Duration(MAIN_UPDATE_FREQUENCY / 1000).sleep(); 97 | 98 | // get t and dt 99 | now = ros::Time::now(); 100 | dt = now - prev; 101 | prev = now; 102 | ros::Duration elapsed = now - start; 103 | 104 | // compute desired ground forces 105 | bool main_update_running = a1->main_update(elapsed.toSec(), dt.toSec()); 106 | bool send_cmd_running = a1->send_cmd(); 107 | 108 | auto t4 = std::chrono::high_resolution_clock::now(); 109 | std::chrono::duration ms_double = t4 - t3; 110 | // std::cout << "Thread 2 is updated in " << ms_double.count() << "ms" << std::endl; 111 | 112 | if (!main_update_running || !send_cmd_running) { 113 | std::cout << "Thread 2 loop is terminated because of errors." << std::endl; 114 | ros::shutdown(); 115 | std::terminate(); 116 | break; 117 | } 118 | } 119 | }); 120 | 121 | ros::AsyncSpinner spinner(12); 122 | spinner.start(); 123 | 124 | compute_foot_forces_grf_thread.join(); 125 | main_thread.join(); 126 | 127 | return 0; 128 | } 129 | -------------------------------------------------------------------------------- /src/a1_cpp/src/MainHardware.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/7/21. 3 | // 4 | // stl 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // ROS 12 | #include 13 | #include 14 | #include 15 | 16 | // control parameters 17 | #include "A1Params.h" 18 | // A1 control 19 | #include "HardwareA1ROS.h" 20 | 21 | int main(int argc, char **argv) { 22 | ros::init(argc, argv, "hardware_a1_qp_ctrl"); 23 | ros::NodeHandle nh; 24 | 25 | // change ros logger 26 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { 27 | ros::console::notifyLoggerLevelsChanged(); 28 | } 29 | 30 | // make sure the ROS infra DO NOT use sim time, otherwise the controller cannot run with correct time steps 31 | std::string use_sim_time; 32 | if (ros::param::get("/use_sim_time", use_sim_time)) { 33 | if (use_sim_time != "false") { 34 | std::cout << "hardware must have real time in order to use this program!" << std::endl; 35 | return -1; 36 | } 37 | } 38 | 39 | // create A1 controller 40 | std::unique_ptr a1 = std::make_unique(nh); 41 | 42 | std::atomic control_execute{}; 43 | control_execute.store(true, std::memory_order_release); 44 | 45 | // Thread 1: compute desired ground forces 46 | std::cout << "Enter thread 1: compute desired ground forces" << std::endl; 47 | std::thread compute_foot_forces_grf_thread([&]() { 48 | // prepare variables to monitor time and control the while loop 49 | ros::Time start = ros::Time::now(); 50 | ros::Time prev = ros::Time::now(); 51 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 52 | ros::Duration dt(0); 53 | ros::Duration dt_solver_time(0); 54 | 55 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 56 | // auto t1 = std::chrono::high_resolution_clock::now(); 57 | 58 | // ros::Duration(GRF_UPDATE_FREQUENCY / 1000).sleep(); 59 | 60 | // get t and dt 61 | now = ros::Time::now(); 62 | dt = now - prev; 63 | prev = now; 64 | // ros::Duration elapsed = now - start; 65 | // std::cout << "Thread 1 is updated in " << dt.toSec() << "s" << std::endl; 66 | 67 | 68 | // compute desired ground forces 69 | bool running = a1->update_foot_forces_grf(dt.toSec()); 70 | 71 | 72 | dt_solver_time = ros::Time::now() - now; 73 | 74 | // auto t2 = std::chrono::high_resolution_clock::now(); 75 | // std::chrono::duration ms_double = t2 - t1; 76 | // std::cout << "foot force is solved in " << dt_solver_time.toSec() << "s" << std::endl; 77 | 78 | if (!running) { 79 | std::cout << "Thread 1 loop is terminated because of errors." << std::endl; 80 | ros::shutdown(); 81 | std::terminate(); 82 | break; 83 | } 84 | 85 | if (dt_solver_time.toSec() < GRF_UPDATE_FREQUENCY / 1000) { 86 | ros::Duration( GRF_UPDATE_FREQUENCY / 1000 - dt_solver_time.toSec() ).sleep(); 87 | } 88 | } 89 | }); 90 | 91 | // Thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands 92 | // std::cout << "Enter thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands" 93 | // << std::endl; 94 | std::thread main_thread([&]() { 95 | // prepare variables to monitor time and control the while loop 96 | ros::Time start = ros::Time::now(); 97 | ros::Time prev = ros::Time::now(); 98 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 99 | ros::Duration dt(0); 100 | ros::Duration dt_solver_time(0); 101 | 102 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 103 | // auto t3 = std::chrono::high_resolution_clock::now(); 104 | 105 | 106 | // get t and dt 107 | now = ros::Time::now(); 108 | dt = now - prev; 109 | prev = now; 110 | ros::Duration elapsed = now - start; 111 | 112 | // compute desired ground forces 113 | bool main_update_running = a1->main_update(elapsed.toSec(), dt.toSec()); 114 | bool send_cmd_running = a1->send_cmd(); 115 | 116 | // auto t4 = std::chrono::high_resolution_clock::now(); 117 | // std::chrono::duration ms_double = t4 - t3; 118 | // std::cout << "Thread 2 is updated in " << dt.toSec()<< "s" << std::endl; 119 | 120 | if (!main_update_running || !send_cmd_running) { 121 | // std::cout << "Thread 2 loop is terminated because of errors." << std::endl; 122 | ros::shutdown(); 123 | std::terminate(); 124 | break; 125 | } 126 | 127 | dt_solver_time = ros::Time::now() - now; 128 | if (dt_solver_time.toSec() < MAIN_UPDATE_FREQUENCY / 1000) { 129 | ros::Duration( MAIN_UPDATE_FREQUENCY / 1000 - dt_solver_time.toSec() ).sleep(); 130 | } 131 | } 132 | }); 133 | 134 | ros::AsyncSpinner spinner(12); 135 | spinner.start(); 136 | 137 | compute_foot_forces_grf_thread.join(); 138 | main_thread.join(); 139 | 140 | return 0; 141 | } 142 | -------------------------------------------------------------------------------- /src/a1_cpp/src/MainIsaac.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/17/21. 3 | // 4 | // stl 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // ROS 12 | #include 13 | #include 14 | #include 15 | 16 | // control parameters 17 | #include "A1Params.h" 18 | // A1 control 19 | #include "IsaacA1ROS.h" 20 | 21 | int main(int argc, char **argv) { 22 | ros::init(argc, argv, "isaac_a1_qp_ctrl"); 23 | ros::NodeHandle nh; 24 | 25 | // change ros logger 26 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { 27 | ros::console::notifyLoggerLevelsChanged(); 28 | } 29 | 30 | // make sure the ROS infra using sim time, otherwise the controller cannot run with correct time steps 31 | std::string use_sim_time; 32 | if (ros::param::get("/use_sim_time", use_sim_time)) { 33 | if (use_sim_time != "true") { 34 | std::cout << "ROS must set use_sim_time in order to use this program!" << std::endl; 35 | return -1; 36 | } 37 | } 38 | else { 39 | std::cout << "ROS must set use_sim_time in order to use this program!" << std::endl; 40 | return -1; 41 | } 42 | 43 | // create a1 controller 44 | std::unique_ptr a1 = std::make_unique(nh); 45 | 46 | std::atomic control_execute{}; 47 | control_execute.store(true, std::memory_order_release); 48 | 49 | // Thread 1: compute desired ground forces 50 | std::cout << "Enter thread 1: compute desired ground forces" << std::endl; 51 | std::thread compute_foot_forces_grf_thread([&]() { 52 | // prepare variables to monitor time and control the while loop 53 | ros::Time start = ros::Time::now(); 54 | ros::Time prev = ros::Time::now(); 55 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 56 | ros::Duration dt(0); 57 | 58 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 59 | auto t1 = std::chrono::high_resolution_clock::now(); 60 | 61 | ros::Duration(GRF_UPDATE_FREQUENCY / 1000).sleep(); 62 | 63 | // get t and dt 64 | now = ros::Time::now(); 65 | dt = now - prev; 66 | prev = now; 67 | ros::Duration elapsed = now - start; 68 | 69 | // compute desired ground forces 70 | bool running = a1->update_foot_forces_grf(dt.toSec()); 71 | 72 | auto t2 = std::chrono::high_resolution_clock::now(); 73 | std::chrono::duration ms_double = t2 - t1; 74 | // std::cout << "Thread 1 is updated in " << ms_double.count() << "ms" << std::endl; 75 | 76 | if (!running) { 77 | std::cout << "Thread 1 loop is terminated because of errors." << std::endl; 78 | ros::shutdown(); 79 | std::terminate(); 80 | break; 81 | } 82 | } 83 | std::terminate(); 84 | }); 85 | 86 | // Thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands 87 | std::cout << "Enter thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands" 88 | << std::endl; 89 | std::thread main_thread([&]() { 90 | // prepare variables to monitor time and control the while loop 91 | ros::Time start = ros::Time::now(); 92 | ros::Time prev = ros::Time::now(); 93 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 94 | ros::Duration dt(0); 95 | 96 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 97 | auto t3 = std::chrono::high_resolution_clock::now(); 98 | 99 | ros::Duration(MAIN_UPDATE_FREQUENCY / 1000).sleep(); 100 | 101 | // get t and dt 102 | now = ros::Time::now(); 103 | dt = now - prev; 104 | prev = now; 105 | ros::Duration elapsed = now - start; 106 | 107 | // compute desired ground forces 108 | bool main_update_running = a1->main_update(elapsed.toSec(), dt.toSec()); 109 | bool send_cmd_running = a1->send_cmd(); 110 | 111 | auto t4 = std::chrono::high_resolution_clock::now(); 112 | std::chrono::duration ms_double = t4 - t3; 113 | // std::cout << "Thread 2 is updated in " << ms_double.count() << "ms" << std::endl; 114 | 115 | if (!main_update_running || !send_cmd_running) { 116 | std::cout << "Thread 2 loop is terminated because of errors." << std::endl; 117 | ros::shutdown(); 118 | std::terminate(); 119 | break; 120 | } 121 | } 122 | std::terminate(); 123 | }); 124 | 125 | ros::AsyncSpinner spinner(12); 126 | spinner.start(); 127 | 128 | compute_foot_forces_grf_thread.join(); 129 | main_thread.join(); 130 | 131 | return 0; 132 | } 133 | -------------------------------------------------------------------------------- /src/a1_cpp/src/legKinematics/A1Kinematics.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 8/10/21. 3 | // 4 | 5 | #ifndef VILEOM_A1KINEMATICS_H 6 | #define VILEOM_A1KINEMATICS_H 7 | 8 | #include 9 | 10 | class A1Kinematics { 11 | 12 | public: 13 | A1Kinematics() = default; 14 | ~A1Kinematics() = default; 15 | 16 | const int RHO_OPT_SIZE = 3; 17 | const int RHO_FIX_SIZE = 5; 18 | // rho opt are contact offset cx cy cz 19 | // rho fix are body offset x& y, thigh offset, upper leg length, lower leg length 20 | // functions with eigen interface 21 | // forward kinematics 3x1 22 | Eigen::Vector3d fk(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 23 | // jacobian 3x3 24 | Eigen::Matrix3d jac(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 25 | // the partial derivative of fk wrt rho opt 3x3 26 | Eigen::Matrix3d dfk_drho(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 27 | // the partial derivative of jacobian wrt q 9x3 28 | Eigen::Matrix dJ_dq(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 29 | // the partial derivative of jacobian wrt rho opt 9x3 30 | Eigen::Matrix dJ_drho(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 31 | private: 32 | // functions with basic C++ interface, generated by Matlab 33 | void autoFunc_fk_derive(const double in1[3], const double in2[3], const double in3[5], double p_bf[3]); 34 | void autoFunc_d_fk_dq(const double in1[3], const double in2[3], const double in3[5], double jacobian[9]); 35 | void autoFunc_d_fk_dc(const double in1[3], const double in2[3], const double in3[5], double d_fk_dc[9]); 36 | void autoFunc_dJ_dq(const double in1[3], const double in2[3], const double in3[5], double dJ_dq[27]); 37 | void autoFunc_dJ_dpho(const double in1[3], const double [3], const double [5], double dJ_dpho[27]); 38 | }; 39 | 40 | 41 | #endif //VILEOM_A1KINEMATICS_H 42 | -------------------------------------------------------------------------------- /src/a1_cpp/src/legKinematics/README.md: -------------------------------------------------------------------------------- 1 | This folder should contain robot leg kinematics 2 | 3 | Currently we have A1 robot's leg kinematics. 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/a1_cpp/src/test/test_bezier.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/18/21. 3 | // 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include "../utils/Utils.h" 10 | 11 | int main(int, char**) { 12 | 13 | Eigen::Vector3d foot_pos_start(0.2,0.15,-0.33); 14 | Eigen::Vector3d foot_pos_final(0.25,0.16,-0.33); 15 | 16 | BezierUtils bs_utils; 17 | Eigen::MatrixXd interp_pos_rst(3,11); 18 | int i = 0; 19 | for (double t=0.0; t<=1.0; t += 0.1) { 20 | interp_pos_rst.col(i) = bs_utils.get_foot_pos_curve(t,foot_pos_start, 21 | foot_pos_final,0); 22 | i++; 23 | } 24 | std::cout<<"position interpolation"< 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "../ConvexMpc.h" 11 | #include "../A1CtrlStates.h" 12 | #include "../A1Params.h" 13 | 14 | int main(int, char **) { 15 | A1CtrlStates state; 16 | state.reset(); // 0, 1, 2, 3: FL, FR, RL, RR 17 | 18 | state.robot_mass = 15; 19 | state.a1_trunk_inertia << 0.0158533, 0.0, 0.0, 20 | 0.0, 0.0377999, 0.0, 21 | 0.0, 0.0, 0.0456542; 22 | 23 | state.root_euler << 0.0, 0.0, 0.0; 24 | 25 | Eigen::AngleAxisd rollAngle(state.root_euler[0], Eigen::Vector3d::UnitZ()); 26 | Eigen::AngleAxisd yawAngle(state.root_euler[1], Eigen::Vector3d::UnitY()); 27 | Eigen::AngleAxisd pitchAngle(state.root_euler[2], Eigen::Vector3d::UnitX()); 28 | Eigen::Quaternion q = rollAngle * yawAngle * pitchAngle; 29 | 30 | state.root_rot_mat = q.matrix(); 31 | 32 | state.root_pos << 0.0, 0.0, 0.15; 33 | state.root_ang_vel << 0.0, 0.0, 0.0; 34 | state.root_lin_vel << 0.0, 0.0, 0.0; 35 | state.root_euler_d << 0.0, 0.0, 0.0; 36 | state.root_ang_vel_d << 0.0, 0.0, 0.0; 37 | state.root_lin_vel_d << 0.0, 0.0, 0.0; 38 | // 0, 1, 2, 3: FL, FR, RL, RR 39 | state.foot_pos_rel << 0.17, 0.17, -0.17, -0.17, 40 | 0.15, -0.15, 0.15, -0.15, 41 | -0.35, -0.35, -0.35, -0.35; 42 | 43 | state.contacts[0] = true; 44 | state.contacts[1] = false; 45 | state.contacts[2] = true; 46 | state.contacts[3] = false; 47 | 48 | double dt = 0.0025; 49 | 50 | Eigen::VectorXd q_weights(13); 51 | Eigen::VectorXd r_weights(12); 52 | q_weights << 1.0, 1.0, 1.0, 53 | 0.0, 0.0, 50.0, 54 | 0.0, 0.0, 1.0, 55 | 1.0, 1.0, 1.0, 56 | 0.0; 57 | r_weights << 1e-6, 1e-6, 1e-6, 58 | 1e-6, 1e-6, 1e-6, 59 | 1e-6, 1e-6, 1e-6, 60 | 1e-6, 1e-6, 1e-6; 61 | ConvexMpc mpc_solver = ConvexMpc(q_weights, r_weights); 62 | mpc_solver.reset(); 63 | 64 | // initialize the mpc state at the first time step 65 | state.mpc_states.resize(13); 66 | state.mpc_states << state.root_euler[0], state.root_euler[1], state.root_euler[2], 67 | state.root_pos[0], state.root_pos[1], state.root_pos[2], 68 | state.root_ang_vel[0], state.root_ang_vel[1], state.root_ang_vel[2], 69 | state.root_lin_vel[0], state.root_lin_vel[1], state.root_lin_vel[2], 70 | -9.8; 71 | 72 | // initialize the desired mpc states trajectory 73 | state.root_lin_vel_d_world = state.root_rot_mat * state.root_lin_vel_d; 74 | state.mpc_states_d.resize(13 * PLAN_HORIZON); 75 | for (int i = 0; i < PLAN_HORIZON; ++i) { 76 | state.mpc_states_d.segment(i * 13, 13) 77 | << 78 | state.root_euler_d[0], 79 | state.root_euler_d[1], 80 | state.root_euler[2] + state.root_ang_vel_d[2] * dt * (i + 1), 81 | state.root_pos[0] + state.root_lin_vel_d_world[0] * dt * (i + 1), 82 | state.root_pos[1] + state.root_lin_vel_d_world[1] * dt * (i + 1), 83 | state.root_pos[2] + state.root_lin_vel_d_world[1] * dt * (i + 1), 84 | state.root_ang_vel_d[0], 85 | state.root_ang_vel_d[1], 86 | state.root_ang_vel_d[2], 87 | state.root_lin_vel_d_world[0], 88 | state.root_lin_vel_d_world[1], 89 | state.root_lin_vel_d_world[2], 90 | -9.8; 91 | } 92 | 93 | // a single A_c is computed for the entire reference trajectory using the average value of each euler angles during the reference trajectory 94 | Eigen::Vector3d avg_root_euler_in_horizon; 95 | avg_root_euler_in_horizon 96 | << 97 | (state.root_euler[0] + state.root_euler[0] + state.root_ang_vel_d[0] * dt * PLAN_HORIZON) / (PLAN_HORIZON + 1), 98 | (state.root_euler[1] + state.root_euler[1] + state.root_ang_vel_d[1] * dt * PLAN_HORIZON) / (PLAN_HORIZON + 1), 99 | (state.root_euler[2] + state.root_euler[2] + state.root_ang_vel_d[2] * dt * PLAN_HORIZON) / (PLAN_HORIZON + 1); 100 | 101 | mpc_solver.calculate_A_mat_c(avg_root_euler_in_horizon); 102 | 103 | // for each point in the reference trajectory, an approximate B_c matrix is computed using desired values of euler angles and feet positions 104 | // from the reference trajectory and foot placement controller 105 | state.foot_pos_abs_mpc = state.foot_pos_rel; 106 | for (int i = 0; i < PLAN_HORIZON; i++) { 107 | // calculate current B_c matrix 108 | mpc_solver.calculate_B_mat_c(state.robot_mass, 109 | state.a1_trunk_inertia, 110 | state.root_rot_mat, 111 | state.foot_pos_abs_mpc); 112 | state.foot_pos_abs_mpc.block<3, 1>(0, 0) = state.foot_pos_abs_mpc.block<3, 1>(0, 0) - state.root_lin_vel_d * dt; 113 | state.foot_pos_abs_mpc.block<3, 1>(0, 1) = state.foot_pos_abs_mpc.block<3, 1>(0, 1) - state.root_lin_vel_d * dt; 114 | state.foot_pos_abs_mpc.block<3, 1>(0, 2) = state.foot_pos_abs_mpc.block<3, 1>(0, 2) - state.root_lin_vel_d * dt; 115 | state.foot_pos_abs_mpc.block<3, 1>(0, 3) = state.foot_pos_abs_mpc.block<3, 1>(0, 3) - state.root_lin_vel_d * dt; 116 | 117 | // state space discretization, calculate A_d and current B_d 118 | mpc_solver.state_space_discretization(dt); 119 | 120 | // store current B_d matrix 121 | mpc_solver.B_mat_d_list.block<13, 12>(i * 13, 0) = mpc_solver.B_mat_d; 122 | } 123 | 124 | // calculate QP matrices 125 | mpc_solver.calculate_qp_mats(state); 126 | 127 | // solve 128 | std::clock_t start; 129 | start = std::clock(); 130 | 131 | OsqpEigen::Solver solver; 132 | solver.settings()->setVerbosity(false); 133 | solver.settings()->setWarmStart(false); 134 | solver.data()->setNumberOfVariables(12 * PLAN_HORIZON); 135 | solver.data()->setNumberOfConstraints(20 * PLAN_HORIZON); 136 | solver.data()->setLinearConstraintsMatrix(mpc_solver.linear_constraints); 137 | solver.data()->setHessianMatrix(mpc_solver.hessian); 138 | solver.data()->setGradient(mpc_solver.gradient); 139 | solver.data()->setLowerBound(mpc_solver.lb); 140 | solver.data()->setUpperBound(mpc_solver.ub); 141 | 142 | // std::cout << "linear_constraints size: " << mpc_solver.linear_constraints.rows() << "X" << mpc_solver.linear_constraints.cols() << std::endl; 143 | // std::cout << "hessian size: " << mpc_solver.hessian.rows() << "X" << mpc_solver.hessian.cols() << std::endl; 144 | // std::cout << "gradient size: " << mpc_solver.gradient.size() << std::endl; 145 | // std::cout << "lb size: " << mpc_solver.lb.size() << std::endl; 146 | // std::cout << "lb size: " << mpc_solver.lb.size() << std::endl; 147 | 148 | solver.initSolver(); 149 | solver.solve(); 150 | 151 | Eigen::VectorXd solution = solver.getSolution(); 152 | 153 | Eigen::Matrix foot_forces_grf; 154 | for (int i = 0; i < 4; ++i) { 155 | foot_forces_grf.block<3, 1>(0, i) = solution.segment<3>(i * 3); 156 | } 157 | std::cout << foot_forces_grf << std::endl; 158 | 159 | std::cout << "Time: " << (std::clock() - start) / (double) (CLOCKS_PER_SEC / 1000) << " ms" << std::endl; 160 | 161 | return 0; 162 | } 163 | -------------------------------------------------------------------------------- /src/a1_cpp/src/test/test_rotation.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 1/25/22. 3 | // 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "../A1CtrlStates.h" 11 | #include "../A1Params.h" 12 | #include "../utils/Utils.h" 13 | 14 | int main(int, char **) { 15 | srand((unsigned int) time(0)); 16 | // random rotation 17 | Eigen::Quaterniond quat = Eigen::Quaterniond::UnitRandom(); 18 | std::cout << "quat " << quat.coeffs().transpose() << std::endl; 19 | // convert to euler using our utility function 20 | Eigen::Vector3d euler = Utils::quat_to_euler(quat); // roll, pitch, yaw 21 | std::cout << "euler using our function " << euler.transpose() << std::endl; 22 | double roll = euler(0), pitch = euler(1), yaw = euler(2); 23 | // convert to euler using Eigen function 24 | Eigen::Vector3d euler2 = quat.toRotationMatrix().eulerAngles(2, 1, 0); // yaw, pitch, roll 25 | std::cout << "euler2 using eigen function" << euler2.transpose() << std::endl; 26 | double roll2 = euler(2), pitch2 = euler(1), yaw2 = euler(0); 27 | // eigen's euler angle is not good because it's range is very strange 28 | 29 | 30 | // next let's see how to assemble roll pitch yaw to rotation matrix 31 | 32 | std::cout << "rotation matrix from quat" << std::endl << quat.toRotationMatrix() << std::endl; 33 | 34 | Eigen::Quaterniond quat2 = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) 35 | * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) 36 | *Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); 37 | std::cout << "rotation matrix from r p y is " << std::endl << quat2.toRotationMatrix() << std::endl; 38 | // quat2 should equals to quat 39 | 40 | Eigen::Quaterniond quat3 = Eigen::AngleAxisd(yaw2, Eigen::Vector3d::UnitZ()) 41 | * Eigen::AngleAxisd(pitch2, Eigen::Vector3d::UnitY()) 42 | *Eigen::AngleAxisd(roll2, Eigen::Vector3d::UnitX()); 43 | std::cout << "rotation matrix from r2 p2 y2 is " << std::endl << quat3.toRotationMatrix() << std::endl; 44 | // again, eigen's euler angle cannot be assembled back to the original matrix 45 | 46 | 47 | double cos_yaw = cos(yaw); 48 | double sin_yaw = sin(yaw); 49 | double cos_pitch = cos(pitch); 50 | double tan_pitch = tan(pitch); 51 | 52 | Eigen::Matrix3d ang_vel_to_rpy_rate; 53 | 54 | ang_vel_to_rpy_rate << cos_yaw / cos_pitch, sin_yaw / cos_pitch, 0, 55 | -sin_yaw, cos_yaw, 0, 56 | cos_yaw * tan_pitch, sin_yaw * tan_pitch, 1; 57 | 58 | std::cout << "ang_vel_to_rpy_rate " << std::endl << ang_vel_to_rpy_rate << std::endl; 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /src/a1_cpp/src/test/test_unitree_msgs.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/1/21. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include "unitree_legged_msgs/MotorCmd.h" 9 | #include "unitree_legged_msgs/MotorState.h" 10 | 11 | int main(int, char**) { 12 | unitree_legged_msgs::MotorCmd lastCmd; 13 | unitree_legged_msgs::MotorState lastState; 14 | lastState.q = 9; 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /src/a1_cpp/src/utils/Utils.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/19/21. 3 | // 4 | 5 | #include "Utils.h" 6 | 7 | Eigen::Vector3d Utils::quat_to_euler(Eigen::Quaterniond quat) { 8 | Eigen::Vector3d rst; 9 | 10 | // order https://github.com/libigl/eigen/blob/master/Eigen/src/Geometry/Quaternion.h 11 | Eigen::Matrix coeff = quat.coeffs(); 12 | double x = coeff(0); 13 | double y = coeff(1); 14 | double z = coeff(2); 15 | double w = coeff(3); 16 | 17 | double y_sqr = y*y; 18 | 19 | double t0 = +2.0 * (w * x + y * z); 20 | double t1 = +1.0 - 2.0 * (x * x + y_sqr); 21 | 22 | rst[0] = atan2(t0, t1); 23 | 24 | double t2 = +2.0 * (w * y - z * x); 25 | t2 = t2 > +1.0 ? +1.0 : t2; 26 | t2 = t2 < -1.0 ? -1.0 : t2; 27 | rst[1] = asin(t2); 28 | 29 | double t3 = +2.0 * (w * z + x * y); 30 | double t4 = +1.0 - 2.0 * (y_sqr + z * z); 31 | rst[2] = atan2(t3, t4); 32 | return rst; 33 | } 34 | 35 | Eigen::Matrix3d Utils::skew(Eigen::Vector3d vec) { 36 | Eigen::Matrix3d rst; rst.setZero(); 37 | rst << 0, -vec(2), vec(1), 38 | vec(2), 0, -vec(0), 39 | -vec(1), vec(0), 0; 40 | return rst; 41 | } 42 | 43 | // https://gist.github.com/pshriwise/67c2ae78e5db3831da38390a8b2a209f 44 | Eigen::Matrix3d Utils::pseudo_inverse(const Eigen::Matrix3d &mat) { 45 | Eigen::JacobiSVD svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV); 46 | double epsilon = std::numeric_limits::epsilon(); 47 | // For a non-square matrix 48 | // Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV); 49 | double tolerance = epsilon * std::max(mat.cols(), mat.rows()) * svd.singularValues().array().abs()(0); 50 | return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * 51 | svd.matrixU().adjoint(); 52 | } 53 | 54 | double Utils::cal_dihedral_angle(Eigen::Vector3d surf_coef_1, Eigen::Vector3d surf_coef_2) { 55 | // surface 1: a1 * x + b1 * y + c1 * z + d1 = 0, coef: [a1, b1, c1] 56 | // surface 2: a2 * x + b2 * y + c2 * z + d2 = 0, coef: [a1, b2, c2] 57 | double angle_cos = 58 | abs(surf_coef_1[0] * surf_coef_2[0] + surf_coef_1[1] * surf_coef_2[1] + surf_coef_1[2] * surf_coef_2[2]) 59 | / (sqrt(surf_coef_1[0] * surf_coef_1[0] + surf_coef_1[1] * surf_coef_1[1] + surf_coef_1[2] * surf_coef_1[2]) * 60 | sqrt(surf_coef_2[0] * surf_coef_2[0] + surf_coef_2[1] * surf_coef_2[1] + surf_coef_2[2] * surf_coef_2[2])); 61 | return acos(angle_cos); 62 | } 63 | 64 | Eigen::Vector3d BezierUtils::get_foot_pos_curve(float t, 65 | Eigen::Vector3d foot_pos_start, 66 | Eigen::Vector3d foot_pos_final, 67 | double terrain_pitch_angle = 0) 68 | { 69 | Eigen::Vector3d foot_pos_target; 70 | // X-axis 71 | std::vector bezierX{foot_pos_start(0), 72 | foot_pos_start(0), 73 | foot_pos_final(0), 74 | foot_pos_final(0), 75 | foot_pos_final(0)}; 76 | foot_pos_target(0) = bezier_curve(t, bezierX); 77 | 78 | // Y-axis 79 | std::vector bezierY{foot_pos_start(1), 80 | foot_pos_start(1), 81 | foot_pos_final(1), 82 | foot_pos_final(1), 83 | foot_pos_final(1)}; 84 | foot_pos_target(1) = bezier_curve(t, bezierY); 85 | 86 | // Z-axis 87 | std::vector bezierZ{foot_pos_start(2), 88 | foot_pos_start(2), 89 | foot_pos_final(2), 90 | foot_pos_final(2), 91 | foot_pos_final(2)}; 92 | bezierZ[1] += FOOT_SWING_CLEARANCE1; 93 | bezierZ[2] += FOOT_SWING_CLEARANCE2 + 0.5*sin(terrain_pitch_angle); 94 | foot_pos_target(2) = bezier_curve(t, bezierZ); 95 | 96 | return foot_pos_target; 97 | } 98 | 99 | 100 | double BezierUtils::bezier_curve(double t, const std::vector &P) { 101 | std::vector coefficients{1, 4, 6, 4, 1}; 102 | double y = 0; 103 | for (int i = 0; i <= bezier_degree; i++) { 104 | y += coefficients[i] * std::pow(t, i) * std::pow(1 - t, bezier_degree - i) * P[i]; 105 | } 106 | return y; 107 | } -------------------------------------------------------------------------------- /src/a1_cpp/src/utils/Utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/19/21. 3 | // 4 | 5 | #ifndef A1_CPP_UTILS_H 6 | #define A1_CPP_UTILS_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "../A1Params.h" 13 | 14 | class Utils { 15 | public: 16 | // compare to Eigen's default eulerAngles 17 | // this function returns yaw angle within -pi to pi 18 | static Eigen::Vector3d quat_to_euler(Eigen::Quaterniond quat); 19 | static Eigen::Matrix3d skew(Eigen::Vector3d vec); 20 | static Eigen::Matrix3d pseudo_inverse(const Eigen::Matrix3d &mat); 21 | static double cal_dihedral_angle(Eigen::Vector3d surf_coef_1, Eigen::Vector3d surf_coef_2); 22 | }; 23 | 24 | class BezierUtils { 25 | // TODO: allow degree change? may not be necessary, we can stick to degree 4 26 | public: 27 | BezierUtils () { 28 | curve_constructed = false; 29 | bezier_degree = 4; 30 | } 31 | // set of functions create bezier curves, get points, reset 32 | Eigen::Vector3d get_foot_pos_curve(float t, 33 | Eigen::Vector3d foot_pos_start, 34 | Eigen::Vector3d foot_pos_final, 35 | double terrain_pitch_angle); 36 | 37 | void reset_foot_pos_curve() {curve_constructed = false;} 38 | private: 39 | double bezier_curve(double t, const std::vector &P); 40 | 41 | bool curve_constructed; 42 | float bezier_degree; 43 | }; 44 | 45 | #endif //A1_CPP_UTILS_H 46 | -------------------------------------------------------------------------------- /src/a1_cpp/src/utils/filter.hpp: -------------------------------------------------------------------------------- 1 | 2 | // This filter comes from 3 | // https://github.com/google-research/tiny-differentiable-simulator/blob/master/examples/whole_body_control/com_velocity_estimator.hpp 4 | // hence we must include Apache License 2.0 too 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | // A stable O(1) moving filter for incoming data streams. Implements the 12 | // Neumaier's algorithm to calculate the moving window average, 13 | // which is numerically stable. 14 | class MovingWindowFilter { 15 | public: 16 | 17 | MovingWindowFilter() {} 18 | 19 | MovingWindowFilter(int window_size) : window_size_(window_size) { 20 | assert(window_size_ > 0); 21 | sum_ = 0.0; 22 | correction_ = 0.0; 23 | } 24 | 25 | // Computes the moving window average. 26 | double CalculateAverage(double new_value) { 27 | if (value_deque_.size() < window_size_) { 28 | // pass 29 | } else { 30 | // The left most value needs to be subtracted from the moving sum first. 31 | UpdateNeumaierSum(-value_deque_.front()); 32 | value_deque_.pop_front(); 33 | } 34 | // Add the new value. 35 | UpdateNeumaierSum(new_value); 36 | value_deque_.push_back(new_value); 37 | 38 | return (sum_ + correction_) / double(window_size_); 39 | } 40 | 41 | std::deque GetValueQueue() { 42 | return value_deque_; 43 | } 44 | private: 45 | int window_size_; 46 | double sum_, correction_; 47 | std::deque value_deque_; 48 | 49 | // Updates the moving window sum using Neumaier's algorithm. 50 | // 51 | // For more details please refer to: 52 | // https://en.wikipedia.org/wiki/Kahan_summation_algorithm#Further_enhancements 53 | void UpdateNeumaierSum(double value) { 54 | double new_sum = sum_ + value; 55 | if (std::abs(sum_) >= std::abs(value)) { 56 | // If previous sum is bigger, low-order digits of value are lost. 57 | correction_ += (sum_ - new_sum) + value; 58 | } else { 59 | correction_ += (value - new_sum) + sum_; 60 | } 61 | sum_ = new_sum; 62 | } 63 | }; -------------------------------------------------------------------------------- /src/data_collection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(data_collection) 3 | 4 | add_definitions(-std=c++17) 5 | set(CMAKE_CXX_FLAGS "-fPIC") 6 | #check environmental variables 7 | message(STATUS "CMAKE_PREFIX_PATH: ${CMAKE_PREFIX_PATH}") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | rospy 12 | std_msgs 13 | geometry_msgs 14 | unitree_legged_msgs 15 | ) 16 | 17 | find_package(Eigen3) 18 | include_directories( 19 | ${EIGEN3_INCLUDE_DIR} 20 | ) 21 | 22 | catkin_package( 23 | # INCLUDE_DIRS include 24 | # LIBRARIES rc_sim_oub 25 | # CATKIN_DEPENDS roscpp rospy std_msgs 26 | # DEPENDS system_lib 27 | ) 28 | 29 | include_directories( 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | # add unitree hardware library, these flags must be correctly set 34 | include_directories( 35 | $ENV{UNITREE_LEGGED_SDK_PATH}/include 36 | ) 37 | 38 | link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib/cpp/amd64) 39 | set(EXTRA_LIBS -pthread libunitree_legged_sdk.so) 40 | 41 | 42 | # hardware (servo_stand) 43 | add_library(lowlevel_lib 44 | src/Lowlevel.hpp 45 | src/Lowlevel.cpp 46 | ) 47 | target_link_libraries(lowlevel_lib ${EXTRA_LIBS}) 48 | 49 | # hardware (sine signal) 50 | add_library(highlevel_lib 51 | src/HighLevel.hpp 52 | src/HighLevel.cpp 53 | ) 54 | target_link_libraries(highlevel_lib ${EXTRA_LIBS}) 55 | 56 | # Declare a cpp executable for hardware robot 57 | add_executable(lowlevel_demo src/MainHardware.cpp) 58 | target_link_libraries(lowlevel_demo 59 | lowlevel_lib 60 | highlevel_lib 61 | ${catkin_LIBRARIES} 62 | ) 63 | 64 | 65 | add_executable(signal_sine src/signal_sine.cpp) 66 | target_link_libraries(signal_sine ${EXTRA_LIBS}) 67 | -------------------------------------------------------------------------------- /src/data_collection/launch/lowlevel_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/data_collection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | data_collection 4 | 0.0.1 5 | data colletion pkg 6 | 7 | 8 | 9 | 10 | shuo 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | unitree_legged_msgs 47 | roscpp 48 | rospy 49 | std_msgs 50 | unitree_legged_msgs 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /src/data_collection/src/HighLevel.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 02.11.22. 3 | // 4 | 5 | #include "HighLevel.hpp" 6 | 7 | 8 | HighLevel::HighLevel(): 9 | safe(UNITREE_LEGGED_SDK::LeggedType::Go1), 10 | udp(UNITREE_LEGGED_SDK::HIGHLEVEL, 8090, "192.168.123.161", 8082){ 11 | udp.InitCmdData(cmd); 12 | cmdVelX_ = 0.; cmdVelY_ = 0.; cmdVelAng_ = 0.; cmdVelMag_ = 0.; 13 | 14 | initMotionScheme(); 15 | 16 | // // start hardware reading thread after everything initialized 17 | // thread_ = std::thread(&HighLevel::UDPRecv, this); 18 | } 19 | 20 | //void HighLevel::UDPRecv() 21 | //{ 22 | // while (destruct == false) { 23 | // 24 | // udp.Recv(); 25 | // udp.GetRecv(state); 26 | // for (int i = 0; i < N_JOINTS; i++) { // first try with front legs 27 | // qSignal_.push_back(state.motorState[i].q); 28 | // } 29 | // 30 | // ros::Duration(HARDWARE_FEEDBACK_FREQUENCY / 1000.0).sleep(); 31 | // } 32 | // 33 | //} 34 | 35 | void HighLevel::UDPRecv() 36 | { 37 | udp.Recv(); 38 | udp.GetRecv(state); 39 | std::cout << "leg height" << state.footRaiseHeight << std::endl; 40 | for (int i = 0; i < N_JOINTS; i++) { // first try with front legs 41 | qSignal_.push_back(state.motorState[i].q); 42 | } 43 | } 44 | 45 | bool HighLevel::UDPSend() 46 | { 47 | udp.SetSend(cmd); 48 | udp.Send(); 49 | return true; 50 | 51 | } 52 | 53 | bool HighLevel::RobotControl() 54 | { 55 | motiontime += 1; 56 | 57 | cmd.mode = 0; // 0:idle, default stand 1:forced stand 2:walk continuously 58 | cmd.gaitType = 0; 59 | cmd.speedLevel = 0; 60 | cmd.footRaiseHeight = 0; 61 | cmd.bodyHeight = 0; 62 | cmd.euler[0] = 0; 63 | cmd.euler[1] = 0; 64 | cmd.euler[2] = 0; 65 | cmd.velocity[0] = 0.0f; 66 | cmd.velocity[1] = 0.0f; 67 | cmd.yawSpeed = 0.0f; 68 | cmd.reserve = 0; 69 | 70 | for (int i = 0; i < int(SIGNAL_LENGTH / MOVEMENT_DURATION); i++) { 71 | if (motiontime >= i * MOVEMENT_DURATION + i * PAUSE_DURATION + 1 && motiontime < (i+1) * MOVEMENT_DURATION + i * PAUSE_DURATION + 1) { 72 | if (motiontime == i * MOVEMENT_DURATION + i * PAUSE_DURATION + 1) { 73 | // when in movement mode, update the scheme first 74 | updateMovementScheme(i); 75 | std::cout << "cmdVelAng: " << cmdVelAng_ << " cmdVelMag: " << cmdVelMag_ << " footHeight: " << footHeight_ << " bodyHeight: " << bodyHeight_ << std::endl; 76 | 77 | } 78 | cmd.mode = 2; 79 | cmd.gaitType = 1; 80 | cmd.velocity[0] = cmdVelMag_ * cmdVelX_; // -1 ~ +1 81 | cmd.velocity[1] = cmdVelMag_ * cmdVelY_; // -1 ~ +1 82 | cmd.footRaiseHeight = footHeight_; 83 | cmd.bodyHeight = bodyHeight_; 84 | } 85 | if (motiontime >= (i+1) * MOVEMENT_DURATION + i * PAUSE_DURATION + 1 && motiontime < (i+1) * MOVEMENT_DURATION + (i+1) * PAUSE_DURATION + 1) { 86 | cmd.mode = 0; 87 | cmd.velocity[0] = 0; 88 | } 89 | } 90 | 91 | if (motiontime >= SIGNAL_LENGTH) { 92 | saveSignalAsFile(); 93 | repeatCheck(); 94 | exit(-1); 95 | } 96 | 97 | return true; 98 | } 99 | 100 | void HighLevel::updateMovementScheme(int i) { 101 | // TODO: add changing body height, leg height, velocity magnitude 102 | cmdVelAng_ = motionParamMat_(i, 0); 103 | cmdVelX_ = sin(cmdVelAng_); 104 | cmdVelY_ = -cos(cmdVelAng_); 105 | 106 | cmdVelMag_ = motionParamMat_(i, 1); 107 | 108 | footHeight_ = motionParamMat_(i, 2); 109 | 110 | bodyHeight_ = motionParamMat_(i, 3); 111 | } 112 | 113 | void HighLevel::initMotionScheme() { 114 | for (int l = 1; l < 2; l++) { 115 | bodyHeight_ = (l - 1) * 0.1; // -1, ..., 1 116 | 117 | for(int i = 4; i < 5; i++) { 118 | footHeight_ = (i - 2) * 0.1; // -2, -1, ..., 1, 2 119 | 120 | for(int j = 0; j < 1; j++) { // change this one-by-one 121 | velMag_ = (j + 1) * 0.2; // 1, 2, ... 122 | 123 | for(int k = 0; k < 8; k++) { 124 | if (k % 2 != 0) { // return trip 125 | velDir_ += M_PI; 126 | } else { 127 | velDir_ = k * M_PI / 8; // 0, 1, ... 128 | } 129 | 130 | motionParam_.push_back(velDir_); 131 | motionParam_.push_back(velMag_); 132 | motionParam_.push_back(footHeight_); 133 | motionParam_.push_back(bodyHeight_); 134 | 135 | } 136 | } 137 | } 138 | } 139 | 140 | // change list into eigen matrix 141 | motionParamMat_ = Eigen::Map>(motionParam_.data()); 142 | } 143 | 144 | void HighLevel::saveSignalAsFile() { 145 | // change list into eigen matrix 146 | std::cout << "qSignal_ length" << qSignal_.size() << std::endl; 147 | qSignalMat_ = Eigen::Map>(qSignal_.data()); 148 | ofstream outFile; 149 | outFile.open("/home/zerenluo/unitree_ros_ws/src/Go1-QP-MPC-Controller/src/data_collection/data/qSineSignal.txt"); 150 | outFile << qSignalMat_; 151 | outFile.close(); 152 | } 153 | 154 | void HighLevel::repeatCheck() { 155 | std::vector vec; 156 | for (int i = 0; i < qSignalMat_.rows(); i++) { 157 | vec.push_back(qSignalMat_.row(i)); 158 | } 159 | 160 | auto it = std::unique(vec.begin(), vec.end()); 161 | vec.erase(it, vec.end()); 162 | // Eigen::Matrix qSignalMatClean; 163 | qSignalMat_.resize(vec.size(), N_JOINTS); 164 | for (int i = 0; i < vec.size(); i++) { 165 | qSignalMat_.row(i) = vec[i]; 166 | } 167 | 168 | // save clean data 169 | ofstream outFile; 170 | outFile.open("/home/zerenluo/unitree_ros_ws/src/Go1-QP-MPC-Controller/src/data_collection/data/qSineSignalClean.txt"); 171 | outFile << qSignalMat_; 172 | outFile.close(); 173 | 174 | } 175 | -------------------------------------------------------------------------------- /src/data_collection/src/HighLevel.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 14.10.22. 3 | // 4 | 5 | // stl 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | 27 | #define ACTION_UPDATE_FREQUENCY 10.0 // ms 28 | #define DEPLOYMENT_FREQUENCY 5.0 // ms 29 | #define HARDWARE_FEEDBACK_FREQUENCY 10.0 // ms 30 | 31 | #define N_JOINTS 12 32 | #define SIGNAL_LENGTH 800 33 | 34 | class HighLevel 35 | { 36 | public: 37 | HighLevel(); 38 | 39 | // ~HighLevel() { 40 | // destruct = true; 41 | // thread_.join(); 42 | // } 43 | 44 | void UDPRecv(); 45 | bool UDPSend(); 46 | bool RobotControl(); 47 | void updateMovementScheme(int i); 48 | void saveSignalAsFile(); 49 | void initMotionScheme(); 50 | 51 | 52 | UNITREE_LEGGED_SDK::Safety safe; 53 | UNITREE_LEGGED_SDK::UDP udp; 54 | UNITREE_LEGGED_SDK::HighCmd cmd = {0}; 55 | UNITREE_LEGGED_SDK::HighState state = {0}; 56 | int motiontime = 0; 57 | 58 | int MOVEMENT_DURATION = 100; 59 | int PAUSE_DURATION = 0; 60 | 61 | private: 62 | float cmdVelX_, cmdVelY_, cmdVelMag_, cmdVelAng_; // change the direction/magnitude of the velocity 63 | vector qSignal_; 64 | 65 | // the list of everything 66 | float velDir_, velMag_, footHeight_, bodyHeight_; 67 | std::vector motionParam_; 68 | Eigen::MatrixXf motionParamMat_; 69 | 70 | Eigen::MatrixXf qSignalMat_; 71 | 72 | void repeatCheck(); 73 | 74 | // a1 hardware reading thread 75 | std::thread thread_; 76 | bool destruct = false; 77 | 78 | 79 | }; -------------------------------------------------------------------------------- /src/data_collection/src/Lowlevel.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 14.10.22. 3 | // 4 | 5 | #include "Lowlevel.hpp" 6 | 7 | Lowlevel::Lowlevel(): 8 | safe(UNITREE_LEGGED_SDK::LeggedType::Go1), udp(UNITREE_LEGGED_SDK::LOWLEVEL, 8090, "192.168.123.10", 8007) { 9 | udp.InitCmdData(cmd); 10 | paramInit(); 11 | } 12 | 13 | bool Lowlevel::UDPRecv() 14 | { 15 | udp.Recv(); 16 | udp.GetRecv(state); 17 | return true; 18 | } 19 | 20 | void Lowlevel::paramInit() 21 | { 22 | for(int i=0; i<2; i++){ 23 | cmd.motorCmd[i*3+0].mode = 0x0A; 24 | cmd.motorCmd[i*3+0].Kp = 10; 25 | cmd.motorCmd[i*3+0].dq = 0; 26 | cmd.motorCmd[i*3+0].Kd = 3; 27 | cmd.motorCmd[i*3+0].tau = 0; 28 | cmd.motorCmd[i*3+1].mode = 0x0A; 29 | cmd.motorCmd[i*3+1].Kp = 40; 30 | cmd.motorCmd[i*3+1].dq = 0; 31 | cmd.motorCmd[i*3+1].Kd = 4; 32 | cmd.motorCmd[i*3+1].tau = 0; 33 | cmd.motorCmd[i*3+2].mode = 0x0A; 34 | cmd.motorCmd[i*3+2].Kp = 20; 35 | cmd.motorCmd[i*3+2].dq = 0; 36 | cmd.motorCmd[i*3+2].Kd = 2; 37 | cmd.motorCmd[i*3+2].tau = 0; 38 | } 39 | 40 | for(int i=2; i<4; i++){ 41 | cmd.motorCmd[i*3+0].mode = 0x0A; 42 | cmd.motorCmd[i*3+0].Kp = 10; 43 | cmd.motorCmd[i*3+0].dq = 0; 44 | cmd.motorCmd[i*3+0].Kd = 3; 45 | cmd.motorCmd[i*3+0].tau = 0; 46 | cmd.motorCmd[i*3+1].mode = 0x0A; 47 | cmd.motorCmd[i*3+1].Kp = 40; 48 | cmd.motorCmd[i*3+1].dq = 0; 49 | cmd.motorCmd[i*3+1].Kd = 4; 50 | cmd.motorCmd[i*3+1].tau = 0; 51 | cmd.motorCmd[i*3+2].mode = 0x0A; 52 | cmd.motorCmd[i*3+2].Kp = 30; 53 | cmd.motorCmd[i*3+2].dq = 0; 54 | cmd.motorCmd[i*3+2].Kd = 2; 55 | cmd.motorCmd[i*3+2].tau = 0; 56 | } 57 | 58 | } 59 | 60 | bool Lowlevel::send_cmd() { 61 | double pos[12] = {0.0, 0.67, -1.6, -0.0, 0.67, -1.6, 62 | 0.0, 0.67, -1.4, -0.0, 0.67, -1.4}; 63 | 64 | // motor q command init 65 | for(int i=0; i<12; i++){ 66 | cmd.motorCmd[i].q = state.motorState[i].q; 67 | } 68 | // change q command overtime 69 | moveAllPosition(pos, 2*30000); 70 | 71 | return true; 72 | } 73 | 74 | void Lowlevel::moveAllPosition(double* targetPos, double duration) { 75 | double pos[12] ,lastPos[12], percent; 76 | for(int j=0; j<12; j++) lastPos[j] = state.motorState[j].q; 77 | for(int i=1; i<=duration; i++){ 78 | percent = (double)i/duration; 79 | // std::cout << "percent" << percent << std::endl; 80 | for(int j=0; j<12; j++){ 81 | cmd.motorCmd[j].q = lastPos[j]*(1-percent) + targetPos[j]*percent; 82 | } 83 | } 84 | 85 | safe.PositionLimit(cmd); 86 | safe.PowerProtect(cmd, state, 5); 87 | // safe.PositionProtect(cmd, state, -0.2); 88 | udp.SetSend(cmd); 89 | udp.Send(); 90 | } -------------------------------------------------------------------------------- /src/data_collection/src/Lowlevel.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 14.10.22. 3 | // 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define ACTION_UPDATE_FREQUENCY 0.5 // ms 12 | #define DEPLOYMENT_FREQUENCY 0.5 // ms 13 | #define HARDWARE_FEEDBACK_FREQUENCY 0.5 // ms 14 | 15 | class Lowlevel 16 | { 17 | public: 18 | Lowlevel(); 19 | bool UDPRecv(); 20 | void RobotControl(); 21 | 22 | void paramInit(); 23 | bool send_cmd(); 24 | void moveAllPosition(double* targetPos, double duration); 25 | 26 | UNITREE_LEGGED_SDK::Safety safe; 27 | UNITREE_LEGGED_SDK::UDP udp; 28 | UNITREE_LEGGED_SDK::LowCmd cmd = {0}; 29 | UNITREE_LEGGED_SDK::LowState state = {0}; 30 | 31 | float dt = 0.002; // 0.001~0.01 32 | }; -------------------------------------------------------------------------------- /src/data_collection/src/MainHardware.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/7/21. 3 | // 4 | // stl 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // ROS 12 | #include 13 | #include 14 | #include 15 | 16 | // servo stand control 17 | #include "Lowlevel.hpp" 18 | #include "HighLevel.hpp" 19 | 20 | int main(int argc, char **argv) { 21 | 22 | std::cout << "Communication level is set to LOW-level." << std::endl 23 | << "Stand" << std::endl 24 | << "Press Enter to continue..." << std::endl; 25 | std::cin.ignore(); 26 | 27 | ros::init(argc, argv, "hardware_go1_rl_ctrl"); 28 | ros::NodeHandle nh; 29 | 30 | // change ros logger 31 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { 32 | ros::console::notifyLoggerLevelsChanged(); 33 | } 34 | 35 | // make sure the ROS infra DO NOT use sim time, otherwise the controller cannot run with correct time steps 36 | std::string use_sim_time; 37 | if (ros::param::get("/use_sim_time", use_sim_time)) { 38 | if (use_sim_time != "false") { 39 | std::cout << "hardware must have real time in order to use this program!" << std::endl; 40 | return -1; 41 | } 42 | } 43 | 44 | // TODO: create RL controller 45 | // create servo stand controller 46 | std::unique_ptr servo = std::make_unique(); 47 | std::unique_ptr walk = std::make_unique(); 48 | 49 | std::atomic control_execute{}; 50 | control_execute.store(true, std::memory_order_release); 51 | 52 | // Thread 1: compute desired ground forces 53 | std::cout << "Enter thread 1: compute desired ground forces" << std::endl; 54 | std::thread compute_foot_forces_grf_thread([&]() { 55 | // prepare variables to monitor time and control the while loop 56 | ros::Time start = ros::Time::now(); 57 | ros::Time prev = ros::Time::now(); 58 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 59 | ros::Duration dt(0); 60 | ros::Duration dt_solver_time(0); 61 | 62 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 63 | // auto t1 = std::chrono::high_resolution_clock::now(); 64 | 65 | // ros::Duration(GRF_UPDATE_FREQUENCY / 1000).sleep(); 66 | 67 | // get t and dt 68 | now = ros::Time::now(); 69 | dt = now - prev; 70 | prev = now; 71 | 72 | // walk->UDPRecv(); 73 | bool running = walk->RobotControl(); 74 | 75 | dt_solver_time = ros::Time::now() - now; 76 | 77 | 78 | 79 | if (!running) { 80 | std::cout << "Thread 1 loop is terminated because of errors." << std::endl; 81 | ros::shutdown(); 82 | std::terminate(); 83 | break; 84 | } 85 | 86 | if (dt_solver_time.toSec() < ACTION_UPDATE_FREQUENCY / 1000) { 87 | ros::Duration( ACTION_UPDATE_FREQUENCY / 1000 - dt_solver_time.toSec() ).sleep(); 88 | } 89 | // auto t2 = std::chrono::high_resolution_clock::now(); 90 | // std::chrono::duration ms_double = t2 - t1; 91 | // std::cout << "foot force is solved in " << dt_solver_time.toSec() << "s" << std::endl; 92 | } 93 | }); 94 | 95 | // Thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands 96 | std::cout << "Enter thread 2: update robot states, compute desired swing legs forces, compute desired joint torques, and send commands" 97 | << std::endl; 98 | std::thread main_thread([&]() { 99 | // prepare variables to monitor time and control the while loop 100 | ros::Time start = ros::Time::now(); 101 | ros::Time prev = ros::Time::now(); 102 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 103 | ros::Duration dt(0); 104 | ros::Duration dt_solver_time(0); 105 | 106 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 107 | // auto t3 = std::chrono::high_resolution_clock::now(); 108 | 109 | 110 | // get t and dt 111 | now = ros::Time::now(); 112 | dt = now - prev; 113 | prev = now; 114 | ros::Duration elapsed = now - start; 115 | 116 | walk->UDPRecv(); 117 | bool send_cmd_running = walk->UDPSend(); 118 | 119 | 120 | if (!send_cmd_running) { 121 | // std::cout << "Thread 2 loop is terminated because of errors." << std::endl; 122 | ros::shutdown(); 123 | std::terminate(); 124 | break; 125 | } 126 | 127 | dt_solver_time = ros::Time::now() - now; 128 | if (dt_solver_time.toSec() < DEPLOYMENT_FREQUENCY / 1000) { // if this f is to high, will read many repetitive data 129 | ros::Duration( DEPLOYMENT_FREQUENCY / 1000 - dt_solver_time.toSec() ).sleep(); 130 | } 131 | } 132 | }); 133 | 134 | ros::AsyncSpinner spinner(12); 135 | spinner.start(); 136 | 137 | compute_foot_forces_grf_thread.join(); 138 | main_thread.join(); 139 | 140 | return 0; 141 | } 142 | -------------------------------------------------------------------------------- /src/data_collection/src/signal_sine.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 24.10.22. 3 | // 4 | 5 | // this file collect the since signal which mimic normal gait 6 | 7 | // stl 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // ROS 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 27 | 28 | 29 | #define N_JOINTS 12 30 | #define SIGNAL_LENGTH 800 31 | 32 | 33 | class Custom 34 | { 35 | public: 36 | Custom(): 37 | safe(UNITREE_LEGGED_SDK::LeggedType::Go1), 38 | udp(UNITREE_LEGGED_SDK::HIGHLEVEL, 8090, "192.168.123.161", 8082){ 39 | udp.InitCmdData(cmd); 40 | cmdVelX_ = 0.; cmdVelY_ = 0.; cmdVelAng_ = 0.; cmdVelMag_ = 0.; 41 | 42 | initMotionScheme(); 43 | 44 | // // start hardware reading thread after everything initialized 45 | // thread_ = std::thread(&Custom::UDPRecv, this); 46 | } 47 | 48 | // ~Custom() { 49 | // destruct = true; 50 | // thread_.join(); 51 | // } 52 | 53 | void UDPRecv(); 54 | bool UDPSend(); 55 | bool RobotControl(); 56 | void updateMovementScheme(int i); 57 | void saveSignalAsFile(); 58 | void initMotionScheme(); 59 | 60 | 61 | UNITREE_LEGGED_SDK::Safety safe; 62 | UNITREE_LEGGED_SDK::UDP udp; 63 | UNITREE_LEGGED_SDK::HighCmd cmd = {0}; 64 | UNITREE_LEGGED_SDK::HighState state = {0}; 65 | int motiontime = 0; 66 | float dt = 0.016; // 0.001~0.01 67 | 68 | int MOVEMENT_DURATION = 100; 69 | int PAUSE_DURATION = 0; 70 | 71 | private: 72 | float cmdVelX_, cmdVelY_, cmdVelMag_, cmdVelAng_; // change the direction/magnitude of the velocity 73 | vector qSignal_; 74 | 75 | // the list of everything 76 | float velDir_, velMag_, footHeight_, bodyHeight_; 77 | std::vector motionParam_; 78 | Eigen::MatrixXf motionParamMat_; 79 | 80 | Eigen::MatrixXf qSignalMat_; 81 | 82 | void repeatCheck(); 83 | 84 | // hardware reading thread 85 | std::thread thread_; 86 | bool destruct = false; 87 | 88 | }; 89 | 90 | 91 | void Custom::UDPRecv() 92 | { 93 | udp.Recv(); 94 | udp.GetRecv(state); 95 | for (int i = 0; i < N_JOINTS; i++) { // first try with front legs 96 | qSignal_.push_back(state.motorState[i].q); 97 | } 98 | 99 | } 100 | 101 | bool Custom::UDPSend() 102 | { 103 | udp.SetSend(cmd); 104 | udp.Send(); 105 | return true; 106 | 107 | } 108 | 109 | bool Custom::RobotControl() 110 | { 111 | motiontime += 1; 112 | 113 | cmd.mode = 0; // 0:idle, default stand 1:forced stand 2:walk continuously 114 | cmd.gaitType = 0; 115 | cmd.speedLevel = 0; 116 | cmd.footRaiseHeight = 0; 117 | cmd.bodyHeight = 0; 118 | cmd.euler[0] = 0; 119 | cmd.euler[1] = 0; 120 | cmd.euler[2] = 0; 121 | cmd.velocity[0] = 0.0f; 122 | cmd.velocity[1] = 0.0f; 123 | cmd.yawSpeed = 0.0f; 124 | cmd.reserve = 0; 125 | 126 | for (int i = 0; i < int(SIGNAL_LENGTH / MOVEMENT_DURATION); i++) { 127 | if (motiontime >= i * MOVEMENT_DURATION + i * PAUSE_DURATION + 1 && motiontime < (i+1) * MOVEMENT_DURATION + i * PAUSE_DURATION + 1) { 128 | if (motiontime == i * MOVEMENT_DURATION + i * PAUSE_DURATION + 1) { 129 | // when in movement mode, update the scheme first 130 | updateMovementScheme(i); 131 | std::cout << "cmdVelAng: " << cmdVelAng_ << " cmdVelMag: " << cmdVelMag_ << " footHeight: " << footHeight_ << " bodyHeight: " << bodyHeight_ << std::endl; 132 | 133 | } 134 | cmd.mode = 2; 135 | cmd.gaitType = 1; 136 | cmd.velocity[0] = cmdVelMag_ * cmdVelX_; // -1 ~ +1 137 | cmd.velocity[1] = cmdVelMag_ * cmdVelY_; // -1 ~ +1 138 | cmd.footRaiseHeight = footHeight_; 139 | cmd.bodyHeight = bodyHeight_; 140 | } 141 | if (motiontime >= (i+1) * MOVEMENT_DURATION + i * PAUSE_DURATION + 1 && motiontime < (i+1) * MOVEMENT_DURATION + (i+1) * PAUSE_DURATION + 1) { 142 | cmd.mode = 0; 143 | cmd.velocity[0] = 0; 144 | } 145 | } 146 | 147 | 148 | // TODO: record and save the actuation signal (with a independent ros thread) 149 | for (int i = 0; i < N_JOINTS; i++) { // first try with front legs 150 | qSignal_.push_back(state.motorState[i].q); 151 | } 152 | 153 | if (motiontime >= SIGNAL_LENGTH) { 154 | saveSignalAsFile(); 155 | repeatCheck(); 156 | exit(-1); 157 | } 158 | 159 | return true; 160 | } 161 | 162 | void Custom::updateMovementScheme(int i) { 163 | // TODO: add changing body height, leg height, velocity magnitude 164 | cmdVelAng_ = motionParamMat_(i, 0); 165 | cmdVelX_ = sin(cmdVelAng_); 166 | cmdVelY_ = -cos(cmdVelAng_); 167 | 168 | cmdVelMag_ = motionParamMat_(i, 1); 169 | 170 | footHeight_ = motionParamMat_(i, 2); 171 | 172 | bodyHeight_ = motionParamMat_(i, 3); 173 | } 174 | 175 | void Custom::initMotionScheme() { 176 | for (int l = 1; l < 2; l++) { 177 | bodyHeight_ = (l - 1) * 0.1; // -1, ..., 1 178 | 179 | for(int i = 0; i < 1; i++) { 180 | footHeight_ = (i - 2) * 0.1; // -2, -1, ..., 1, 2 181 | 182 | for(int j = 0; j < 1; j++) { // change this one-by-one 183 | velMag_ = (j + 1) * 0.2; // 1, 2, ... 184 | 185 | for(int k = 0; k < 8; k++) { 186 | if (k % 2 != 0) { // return trip 187 | velDir_ += M_PI; 188 | } else { 189 | velDir_ = k * M_PI / 8; // 0, 1, ... 190 | } 191 | 192 | motionParam_.push_back(velDir_); 193 | motionParam_.push_back(velMag_); 194 | motionParam_.push_back(footHeight_); 195 | motionParam_.push_back(bodyHeight_); 196 | 197 | } 198 | } 199 | } 200 | } 201 | 202 | // change list into eigen matrix 203 | motionParamMat_ = Eigen::Map>(motionParam_.data()); 204 | } 205 | 206 | void Custom::saveSignalAsFile() { 207 | // change list into eigen matrix 208 | qSignalMat_ = Eigen::Map>(qSignal_.data()); 209 | ofstream outFile; 210 | outFile.open("/home/zerenluo/unitree_ros_ws/src/Go1-QP-MPC-Controller/src/data_collection/data/qSineSignal.txt"); 211 | outFile << qSignalMat_; 212 | outFile.close(); 213 | } 214 | 215 | void Custom::repeatCheck() { 216 | std::vector vec; 217 | for (int i = 0; i < qSignalMat_.rows(); i++) { 218 | vec.push_back(qSignalMat_.row(i)); 219 | } 220 | 221 | auto it = std::unique(vec.begin(), vec.end()); 222 | vec.erase(it, vec.end()); 223 | // Eigen::Matrix qSignalMatClean; 224 | qSignalMat_.resize(vec.size(), N_JOINTS); 225 | for (int i = 0; i < vec.size(); i++) { 226 | qSignalMat_.row(i) = vec[i]; 227 | } 228 | 229 | // save clean data 230 | ofstream outFile; 231 | outFile.open("/home/zerenluo/unitree_ros_ws/src/Go1-QP-MPC-Controller/src/data_collection/data/qSineSignalClean.txt"); 232 | outFile << qSignalMat_; 233 | outFile.close(); 234 | 235 | } 236 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(go1_rl_ctrl_cpp) 3 | 4 | add_definitions(-std=c++17) 5 | set(CMAKE_CXX_FLAGS "-fPIC") 6 | #check environmental variables 7 | message(STATUS "CMAKE_PREFIX_PATH: ${CMAKE_PREFIX_PATH}") 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | rospy 12 | std_msgs 13 | geometry_msgs 14 | unitree_legged_msgs 15 | ) 16 | 17 | find_package(Eigen3) 18 | include_directories( 19 | ${EIGEN3_INCLUDE_DIR} 20 | ) 21 | 22 | catkin_package( 23 | # INCLUDE_DIRS include 24 | # LIBRARIES rc_sim_oub 25 | # CATKIN_DEPENDS roscpp rospy std_msgs 26 | # DEPENDS system_lib 27 | ) 28 | 29 | include_directories( 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | # to find osqp.h 34 | include_directories($ENV{UNITREE_LOCAL_INSTALL}/include/osqp) 35 | find_package(OsqpEigen REQUIRED) 36 | 37 | # add unitree hardware library, these flags must be correctly set 38 | include_directories( 39 | $ENV{UNITREE_LEGGED_SDK_PATH}/include 40 | ) 41 | 42 | link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib/cpp/amd64) 43 | set(EXTRA_LIBS -pthread libunitree_legged_sdk.so) 44 | 45 | # Declare cpp libraries 46 | # common robot controller every type of robots need 47 | add_library(go1_lib 48 | src/legKinematics/Go1Kinematics.hpp 49 | src/legKinematics/Go1Kinematics.cpp 50 | src/Go1Params.hpp 51 | src/Go1CtrlStates.hpp 52 | src/utils/Utils.hpp 53 | src/utils/Utils.cpp 54 | src/EKF/Go1BasicEKF.hpp 55 | src/EKF/Go1BasicEKF.cpp 56 | src/SwitchController.hpp) 57 | target_link_libraries(go1_lib ${catkin_LIBRARIES} OsqpEigen::OsqpEigen) 58 | 59 | #target_link_libraries(${PACKAGE_NAME} /usr/lib/x86_64-linux-gnu/libtinyxml2.so) 60 | 61 | ############################ 62 | ## Link C++ torch library ## 63 | ############################ 64 | list(APPEND CMAKE_PREFIX_PATH "${CMAKE_CURRENT_LIST_DIR}/libtorch/libtorch") 65 | 66 | find_package(Torch REQUIRED) 67 | 68 | add_library(torch_eigen STATIC src/torch_eigen/TorchEigen.cpp) 69 | target_link_libraries(torch_eigen "${TORCH_LIBRARIES}") 70 | set_property(TARGET torch_eigen PROPERTY CXX_STANDARD 14) 71 | 72 | 73 | 74 | 75 | 76 | # lib of different types of robots 77 | # gazebo 78 | add_library(gazebo_go1_lib 79 | src/observation/Go1Observation.hpp 80 | src/Go1RLController.hpp 81 | src/Go1RLController.cpp 82 | ) 83 | target_link_libraries(gazebo_go1_lib 84 | go1_lib 85 | torch_eigen 86 | libyaml-cpp.so 87 | ) 88 | 89 | # hardware 90 | add_library(hardware_go1_lib 91 | src/Go1RLHardwareController.hpp 92 | src/Go1RLHardwareController.cpp 93 | ) 94 | target_link_libraries(hardware_go1_lib 95 | go1_lib 96 | torch_eigen 97 | libyaml-cpp.so 98 | ${EXTRA_LIBS} 99 | ) 100 | 101 | # lib of different types of robots 102 | # gazebo (servo_stand) 103 | add_library(gazebo_servo_lib 104 | src/servo_stand_policy/GazeboServo.hpp 105 | src/servo_stand_policy/GazeboServo.cpp 106 | ) 107 | 108 | # hardware (servo_stand) 109 | add_library(hardware_servo_lib 110 | src/servo_stand_policy/HardwareServo.hpp 111 | src/servo_stand_policy/HardwareServo.cpp 112 | ) 113 | target_link_libraries(hardware_servo_lib ${EXTRA_LIBS}) 114 | 115 | # hardware (servo_stand_switch) for test 116 | add_library(hardware_servo_switch_lib 117 | src/servo_stand_policy/HardwareServoSwitch.hpp 118 | src/servo_stand_policy/HardwareServoSwitch.cpp 119 | ) 120 | target_link_libraries(hardware_servo_switch_lib ${EXTRA_LIBS}) 121 | 122 | # Declare a cpp executable for gazebo robot 123 | # gazebo 124 | add_executable(gazebo_go1_ctrl src/MainGazebo.cpp) 125 | target_link_libraries(gazebo_go1_ctrl 126 | ${catkin_LIBRARIES} 127 | ${TORCH_LIBRARIES} 128 | gazebo_go1_lib 129 | gazebo_servo_lib 130 | ) 131 | 132 | 133 | # ************************** DEBUG ************************** 134 | 135 | # Declare a cpp executable for hardware robot 136 | add_executable(hardware_go1_ctrl src/TestHardware.cpp) 137 | target_link_libraries(hardware_go1_ctrl 138 | hardware_go1_lib 139 | hardware_servo_lib 140 | hardware_servo_switch_lib 141 | ${catkin_LIBRARIES} 142 | ${TORCH_LIBRARIES} 143 | ) 144 | 145 | #add_executable(stand_servo src/servo_stand_policy/servo.cpp) 146 | #target_link_libraries(stand_servo 147 | # gazebo_servo_lib 148 | # ${catkin_LIBRARIES} 149 | # ) 150 | 151 | 152 | 153 | 154 | 155 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/config/hardware_parameters.yaml: -------------------------------------------------------------------------------- 1 | package_dir: /home/zerenluo/unitree_ros_ws/src/Go1-QP-MPC-Controller/src/go1_rl_ctrl_cpp 2 | 3 | use_sim_time: false # gazebo: true; hardware: false 4 | 5 | stand_weights: stand_cpp_model.pt 6 | 7 | weights: mass.pt 8 | 9 | action_update_frequency: 2.5 # ms; 400 Hz 10 | 11 | deployment_frequency: 1.0 # ms; 1000 Hz 12 | 13 | go1_hardware_power_level: 6 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/config/parameters.yaml: -------------------------------------------------------------------------------- 1 | package_dir: /home/zerenluo/unitree_ros_ws/src/Go1-QP-MPC-Controller/src/go1_rl_ctrl_cpp 2 | 3 | use_sim_time: true # gazebo: true; hardware: false 4 | 5 | stand_weights: stand_cpp_model.pt 6 | 7 | weights: mass.pt 8 | 9 | action_update_frequency: 4.0 # ms 10 | 11 | deployment_frequency: 2.0 # ms 12 | 13 | stiffness: 18.0 # 18.0 for 0.0002 and 5000 14 | 15 | damping: 10.0 # 10.0 for 0.0002 and 5000 -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/launch/a1_ctrl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/launch/go1_ctrl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/launch/go1_ctrl_hardware.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | go1_rl_ctrl_cpp 4 | 0.0.1 5 | The A1 Cpp Controller 6 | 7 | 8 | 9 | 10 | shuo 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | unitree_legged_msgs 47 | roscpp 48 | rospy 49 | std_msgs 50 | unitree_legged_msgs 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/resource/cpp_model.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/src/go1_rl_ctrl_cpp/resource/cpp_model.pt -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/resource/mass.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/src/go1_rl_ctrl_cpp/resource/mass.pt -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/resource/position.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/src/go1_rl_ctrl_cpp/resource/position.pt -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/resource/stand_cpp_model.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/src/go1_rl_ctrl_cpp/resource/stand_cpp_model.pt -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/EKF/Go1BasicEKF.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/1/21. 3 | // 4 | 5 | #include "Go1BasicEKF.hpp" 6 | 7 | Go1BasicEKF::Go1BasicEKF () { 8 | // constructor 9 | eye3.setIdentity(); 10 | // C is fixed 11 | C.setZero(); 12 | for (int i=0; i(i*3,0) = -eye3; //-pos 14 | C.block<3,3>(i*3,6+i*3) = eye3; //foot pos 15 | C.block<3,3>(NUM_LEG*3+i*3,3) = eye3; // vel 16 | C(NUM_LEG*6+i,6+i*3+2) = 1; // height z of foot 17 | } 18 | 19 | // Q R are fixed 20 | Q.setIdentity(); 21 | Q.block<3,3>(0,0) = PROCESS_NOISE_PIMU*eye3; // position transition 22 | Q.block<3,3>(3,3) = PROCESS_NOISE_VIMU*eye3; // velocity transition 23 | for (int i=0; i(6+i*3,6+i*3) = PROCESS_NOISE_PFOOT*eye3; // foot position transition 25 | } 26 | 27 | R.setIdentity(); 28 | for (int i=0; i(i*3,i*3) = SENSOR_NOISE_PIMU_REL_FOOT*eye3; // fk estimation 30 | R.block<3,3>(NUM_LEG*3+i*3,NUM_LEG*3+i*3) = SENSOR_NOISE_VIMU_REL_FOOT*eye3; // vel estimation 31 | R(NUM_LEG*6+i,NUM_LEG*6+i) = SENSOR_NOISE_ZFOOT; // height z estimation 32 | } 33 | 34 | // set A to identity 35 | A.setIdentity(); 36 | 37 | // set B to zero 38 | B.setZero(); 39 | 40 | assume_flat_ground = true; 41 | 42 | } 43 | 44 | Go1BasicEKF::Go1BasicEKF (bool assume_flat_ground_):Go1BasicEKF() { 45 | // constructor 46 | assume_flat_ground = assume_flat_ground_; 47 | // change R according to this flag, if we do not assume the robot moves on flat ground, 48 | // then we cannot infer height z using this way 49 | if (assume_flat_ground == false) { 50 | for (int i=0; i(0) = Eigen::Vector3d(0, 0, 0.09); 64 | for (int i = 0; i < NUM_LEG; ++i) { 65 | Eigen::Vector3d fk_pos = state.foot_pos_rel.block<3, 1>(0, i); 66 | x.segment<3>(6 + i * 3) = state.root_rot_mat * fk_pos + x.segment<3>(0); 67 | } 68 | } 69 | 70 | void Go1BasicEKF::update_estimation(Go1CtrlStates& state, double dt) { 71 | // update A B using latest dt 72 | A.block<3, 3>(0, 3) = dt * eye3; 73 | B.block<3, 3>(3, 0) = dt * eye3; 74 | 75 | // control input u is Ra + ag 76 | Eigen::Vector3d u = state.root_rot_mat * state.imu_acc + Eigen::Vector3d(0, 0, -9.81); 77 | 78 | // contact estimation, do something very simple first 79 | if (state.movement_mode == 0) { // stand 80 | for (int i = 0; i < NUM_LEG; ++i) estimated_contacts[i] = 1.0; 81 | } else { // walk 82 | for (int i = 0; i < NUM_LEG; ++i) { 83 | estimated_contacts[i] = std::min(std::max((state.foot_force(i)) / (1000.0 - 0.0), 0.0), 1.0); 84 | // estimated_contacts[i] = 1.0/(1.0+std::exp(-(state.foot_force(i)-100))); 85 | } 86 | } 87 | // update Q 88 | Q.block<3, 3>(0, 0) = PROCESS_NOISE_PIMU * dt / 20.0 * eye3; 89 | Q.block<3, 3>(3, 3) = PROCESS_NOISE_VIMU * dt * 9.8 / 20.0 * eye3; 90 | // update Q R for legs not in contact 91 | for (int i = 0; i < NUM_LEG; ++i) { 92 | Q.block<3, 3>(6 + i * 3, 6 + i * 3) 93 | = 94 | (1 + (1 - estimated_contacts[i]) * 1e3) * dt * PROCESS_NOISE_PFOOT * eye3; // foot position transition 95 | // for estimated_contacts[i] == 1, Q = 0.002 96 | // for estimated_contacts[i] == 0, Q = 1001*Q 97 | 98 | R.block<3, 3>(i * 3, i * 3) 99 | = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_PIMU_REL_FOOT * 100 | eye3; // fk estimation 101 | R.block<3, 3>(NUM_LEG * 3 + i * 3, NUM_LEG * 3 + i * 3) 102 | = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_VIMU_REL_FOOT * eye3; // vel estimation 103 | if (assume_flat_ground) { 104 | R(NUM_LEG * 6 + i, NUM_LEG * 6 + i) 105 | = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_ZFOOT; // height z estimation 106 | } 107 | } 108 | 109 | 110 | // process update 111 | xbar = A*x + B*u ; 112 | Pbar = A * P * A.transpose() + Q; 113 | 114 | // measurement construction 115 | yhat = C*xbar; 116 | // leg_v = (-J_rf*av-skew(omega)*p_rf); 117 | // r((i-1)*3+1:(i-1)*3+3) = body_v - R_er*leg_v; 118 | // actual measurement 119 | for (int i=0; i(0,i); 121 | y.block<3,1>(i*3,0) = state.root_rot_mat*fk_pos; // fk estimation 122 | Eigen::Vector3d leg_v = -state.foot_vel_rel.block<3,1>(0,i) - Utils::skew(state.imu_ang_vel)*fk_pos; 123 | y.block<3,1>(NUM_LEG*3+i*3,0) = 124 | (1.0-estimated_contacts[i])*x.segment<3>(3) + estimated_contacts[i]*state.root_rot_mat*leg_v; // vel estimation 125 | 126 | y(NUM_LEG*6+i) = 127 | (1.0-estimated_contacts[i])*(x(2)+fk_pos(2)) + estimated_contacts[i]*0; // height z estimation 128 | } 129 | 130 | S = C * Pbar *C.transpose() + R; 131 | S = 0.5*(S+S.transpose()); 132 | 133 | error_y = y - yhat; 134 | Serror_y = S.fullPivHouseholderQr().solve(error_y); 135 | 136 | x = xbar + Pbar * C.transpose() * Serror_y; 137 | 138 | SC = S.fullPivHouseholderQr().solve(C); 139 | P = Pbar - Pbar * C.transpose() * SC * Pbar; 140 | P = 0.5 * (P + P.transpose()); 141 | 142 | // reduce position drift 143 | if (P.block<2, 2>(0, 0).determinant() > 1e-6) { 144 | P.block<2, 16>(0, 2).setZero(); 145 | P.block<16, 2>(2, 0).setZero(); 146 | P.block<2, 2>(0, 0) /= 10.0; 147 | } 148 | 149 | // final step 150 | // put estimated values back to Go1CtrlStates& state 151 | for (int i = 0; i < NUM_LEG; ++i) { 152 | if (estimated_contacts[i] < 0.5) { 153 | state.estimated_contacts[i] = false; 154 | } else { 155 | state.estimated_contacts[i] = true; 156 | } 157 | } 158 | // std::cout << x.transpose() <(0); 160 | state.estimated_root_vel = x.segment<3>(3); 161 | 162 | state.root_pos = x.segment<3>(0); 163 | state.root_lin_vel = x.segment<3>(3); 164 | } -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/EKF/Go1BasicEKF.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 11/1/21. 3 | // 4 | 5 | #ifndef GO1BASICEKF_H 6 | #define GO1BASICEKF_H 7 | 8 | #include "../Go1Params.hpp" 9 | #include "../Go1CtrlStates.hpp" 10 | #include "../utils/Utils.hpp" 11 | 12 | 13 | // state estimator parameters 14 | #define STATE_SIZE 18 15 | #define MEAS_SIZE 28 16 | #define PROCESS_NOISE_PIMU 0.01 17 | #define PROCESS_NOISE_VIMU 0.01 18 | #define PROCESS_NOISE_PFOOT 0.01 19 | #define SENSOR_NOISE_PIMU_REL_FOOT 0.001 20 | #define SENSOR_NOISE_VIMU_REL_FOOT 0.1 21 | #define SENSOR_NOISE_ZFOOT 0.001 22 | 23 | // implement a basic error state KF to estimate robot pose 24 | // assume orientation is known from a IMU (state.root_rot_mat) 25 | class Go1BasicEKF { 26 | public: 27 | Go1BasicEKF (); 28 | Go1BasicEKF (bool assume_flat_ground_); 29 | void init_state(Go1CtrlStates& state); 30 | void update_estimation(Go1CtrlStates& state, double dt); 31 | bool is_inited() {return filter_initialized;} 32 | private: 33 | bool filter_initialized = false; 34 | // state 35 | // 0 1 2 pos 3 4 5 vel 6 7 8 foot pos FL 9 10 11 foot pos FR 12 13 14 foot pos RL 15 16 17 foot pos RR 36 | Eigen::Matrix x; // estimation state 37 | Eigen::Matrix xbar; // estimation state after process update 38 | Eigen::Matrix P; // estimation state covariance 39 | Eigen::Matrix Pbar; // estimation state covariance after process update 40 | Eigen::Matrix A; // estimation state transition 41 | Eigen::Matrix B; // estimation state transition 42 | Eigen::Matrix Q; // estimation state transition noise 43 | 44 | // observation 45 | // 0 1 2 FL pos residual 46 | // 3 4 5 FR pos residual 47 | // 6 7 8 RL pos residual 48 | // 9 10 11 RR pos residual 49 | // 12 13 14 vel residual from FL 50 | // 15 16 17 vel residual from FR 51 | // 18 19 20 vel residual from RL 52 | // 21 22 23 vel residual from RR 53 | // 24 25 26 27 foot height 54 | Eigen::Matrix y; // observation 55 | Eigen::Matrix yhat; // estimated observation 56 | Eigen::Matrix error_y; // estimated observation 57 | Eigen::Matrix Serror_y; // S^-1*error_y 58 | Eigen::Matrix C; // estimation state observation 59 | Eigen::Matrix SC; // S^-1*C 60 | Eigen::Matrix R; // estimation state observation noise 61 | // helper matrices 62 | Eigen::Matrix eye3; // 3x3 identity 63 | Eigen::Matrix S; // Innovation (or pre-fit residual) covariance 64 | Eigen::Matrix K; // kalman gain 65 | 66 | 67 | bool assume_flat_ground = false; 68 | 69 | // variables to process foot force 70 | double smooth_foot_force[4]; 71 | double estimated_contacts[4]; 72 | }; 73 | 74 | 75 | #endif //GO1BASICEKF_H 76 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/Go1Params.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/18/21. 3 | // 4 | 5 | #ifndef GO1_CPP_A1PARAMS_H 6 | #define GO1_CPP_A1PARAMS_H 7 | 8 | // control time related 9 | //#define CTRL_FREQUENCY 2.5 // ms 10 | #define ACTION_UPDATE_FREQUENCY 0.5 // ms 11 | #define DEPLOYMENT_FREQUENCY 0.5 // ms 12 | #define HARDWARE_FEEDBACK_FREQUENCY 0.5 // ms 13 | 14 | // constant define 15 | // joy stick command interprate 16 | #define JOY_CMD_BODY_HEIGHT_MAX 0.32 // m 17 | #define JOY_CMD_BODY_HEIGHT_MIN 0.1 // m 18 | #define JOY_CMD_BODY_HEIGHT_VEL 0.04 // m/s 19 | #define JOY_CMD_VELX_MAX 0.6 // m/s 20 | #define JOY_CMD_VELY_MAX 0.6 // m/s 21 | #define JOY_CMD_YAW_MAX 0.7 // rad 22 | #define JOY_CMD_PITCH_MAX 0.4 // rad 23 | #define JOY_CMD_ROLL_MAX 0.4 // rad 24 | 25 | // mpc 26 | #define PLAN_HORIZON 10 27 | #define MPC_STATE_DIM 13 28 | #define MPC_CONSTRAINT_DIM 20 29 | 30 | // robot constant 31 | #define NUM_LEG 4 32 | #define NUM_DOF_PER_LEG 3 33 | #define DIM_GRF 12 34 | #define NUM_DOF 12 35 | 36 | #define LOWER_LEG_LENGTH 0.21 37 | 38 | #define FOOT_FORCE_LOW 30.0 39 | #define FOOT_FORCE_HIGH 80.0 40 | 41 | #define FOOT_SWING_CLEARANCE1 0.0f 42 | #define FOOT_SWING_CLEARANCE2 0.4f 43 | 44 | #define FOOT_DELTA_X_LIMIT 0.1 45 | #define FOOT_DELTA_Y_LIMIT 0.1 46 | 47 | #define ERROR_CURVE_ALREADY_SET 184 48 | #define ERROR_CURVE_NOT_SET 185 49 | 50 | #endif //GO1_CPP_A1PARAMS_H 51 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/Go1RLController.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 30.08.22. 3 | // 4 | 5 | #pragma once 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | // params 14 | #include "yaml-cpp/yaml.h" 15 | 16 | #include 17 | 18 | #include "torch_eigen/TorchEigen.hpp" 19 | 20 | // control parameters 21 | #include "Go1Params.hpp" 22 | #include "observation/Go1Observation.hpp" 23 | #include "Go1CtrlStates.hpp" 24 | 25 | 26 | class Go1RLController { 27 | public: 28 | Go1RLController(ros::NodeHandle &nh); 29 | 30 | // // ! Destructor, used to terminate map thread if there is camera 31 | // ~Go1RLController(); 32 | 33 | bool advance(double dt); 34 | bool advance_servo(double dt); 35 | 36 | bool send_cmd(); 37 | 38 | // QP-based stand policy 39 | bool update_foot_forces_grf(double dt); 40 | bool main_update(double dt); 41 | 42 | // debugging functions 43 | void send_obs(Eigen::VectorXf &obs); 44 | void send_foot_pos(Go1CtrlStates &go1_ctrl_states); 45 | void send_foot_force(Go1CtrlStates &go1_ctrl_states); 46 | 47 | private: 48 | ros::NodeHandle nh_; 49 | std::string pkgDir_; 50 | 51 | // 0, 1, 2: FL_hip, FL_thigh, FL_calf 52 | // 3, 4, 5: FR_hip, FR_thigh, FR_calf 53 | // 6, 7, 8: RL_hip, RL_thigh, RL_calf 54 | // 9, 10, 11: RR_hip, RR_thigh, RR_calf 55 | ros::Publisher pub_joint_cmd_[12]; 56 | ros::Publisher pub_obs_; 57 | ros::Publisher pub_foot_pos_rel_; 58 | ros::Publisher pub_foot_force_; 59 | 60 | //! YAML parsing of parameter file 61 | YAML::Node yamlNode_; 62 | 63 | //! observations & actions 64 | std::unordered_map obsMap_; 65 | Eigen::VectorXd prevActionDouble_, actionDouble_; // double 66 | Eigen::VectorXf action_; // float 67 | Eigen::VectorXd targetPoses_; 68 | 69 | //! controller/policy 70 | TorchEigen standPolicy_; 71 | TorchEigen policy_; 72 | std::string standCtrlWeights_; 73 | std::string ctrlWeights_; 74 | 75 | //! Parameter loading 76 | void loadParameters(); 77 | 78 | //! NN parameter loading 79 | void loadNNparams(); 80 | 81 | //! observation 82 | std::unique_ptr go1Obs_; 83 | 84 | double clipAction_ = 100.; 85 | Eigen::VectorXd clipPoseLower_; 86 | Eigen::VectorXd clipPoseUpper_; 87 | 88 | double actionScale_ = 0.25; 89 | double stiffness_ = 18.; // 17.0 90 | double damping_ = 18.0; // 3.5 91 | double alpha_ = 0.1; 92 | Eigen::VectorXd pGains_; 93 | Eigen::VectorXd dGains_; 94 | 95 | // ! go1 control state 96 | Go1CtrlStates go1_ctrl_states_; 97 | 98 | // ! servo motion time 99 | double servo_motion_time_; 100 | 101 | 102 | }; 103 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/Go1RLHardwareController.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 29/9/22. 3 | // 4 | 5 | #ifndef GO1_CPP_HARDWAREGO1ROS_H 6 | #define GO1_CPP_HARDWAREGO1ROS_H 7 | 8 | // std 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // ROS 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "torch_eigen/TorchEigen.hpp" 32 | 33 | // control parameters 34 | #include "Go1Params.hpp" 35 | //#include "observation/Go1HardwareObservation.hpp" 36 | #include "Go1CtrlStates.hpp" 37 | #include "Go1Params.hpp" 38 | #include "utils/Utils.hpp" 39 | #include "utils/filter.hpp" 40 | #include "EKF/Go1BasicEKF.hpp" 41 | #include "legKinematics/Go1Kinematics.hpp" 42 | 43 | // go1 hardware 44 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 45 | 46 | #define FOOT_FILTER_WINDOW_SIZE 5 47 | 48 | // one of the most important thing: Go1 hardware return info with FR, FL, RR, RL order and receives info in this order 49 | // we need to take of this order in this function 50 | class Go1RLHardwareController { 51 | public: 52 | Go1RLHardwareController(ros::NodeHandle &nh); 53 | 54 | ~Go1RLHardwareController() { 55 | destruct = true; 56 | thread_.join(); 57 | } 58 | 59 | bool advance(); 60 | bool advance_servo(); 61 | 62 | bool send_cmd(); 63 | 64 | void send_obs(Eigen::VectorXf &obs); 65 | 66 | void joy_callback(const sensor_msgs::Joy::ConstPtr &joy_msg); 67 | 68 | void updateObservation(); 69 | 70 | void updateMovementMode(); 71 | 72 | 73 | private: 74 | ros::NodeHandle nh_; 75 | std::string pkgDir_; 76 | 77 | // debug estimated position 78 | ros::Publisher pub_estimated_pose; 79 | 80 | // go1 hardware 81 | UNITREE_LEGGED_SDK::UDP udp; 82 | UNITREE_LEGGED_SDK::Safety safe; 83 | UNITREE_LEGGED_SDK::LowState state = {0}; 84 | UNITREE_LEGGED_SDK::LowCmd cmd = {0}; 85 | // // go1 hardware reading thread 86 | // std::thread thread_; 87 | // bool destruct = false; 88 | 89 | void udp_init_send(); 90 | 91 | void receive_low_state(); 92 | 93 | // go1 hardware switch foot order 94 | Eigen::Matrix swap_joint_indices; 95 | Eigen::Matrix swap_foot_indices; 96 | 97 | ros::Publisher pub_joint_cmd; 98 | ros::Publisher pub_joint_angle; 99 | ros::Publisher pub_imu; 100 | sensor_msgs::JointState joint_foot_msg; 101 | sensor_msgs::Imu imu_msg; 102 | ros::Subscriber sub_joy_msg; 103 | 104 | //! observations & actions for RL controller 105 | // ! observation 106 | size_t obDim_; 107 | Eigen::VectorXd obDouble_, obScaled_; 108 | Eigen::VectorXd actionMean_, actionStd_, obMean_, obStd_; 109 | 110 | Eigen::Vector3d linVelScale_, angVelScale_, gravityScale_, commandScale_; 111 | Eigen::VectorXd dofPosScale_, dofVelScale_; 112 | Eigen::VectorXd scaleFactor_; 113 | double clipObs_ = 100.; 114 | 115 | // ! action 116 | Eigen::VectorXd prevActionDouble_, actionDouble_; // double 117 | Eigen::VectorXf action_; // float 118 | Eigen::VectorXd torques_; 119 | Eigen::VectorXd targetPoses_; 120 | 121 | //! controller/policy 122 | TorchEigen standPolicy_; 123 | TorchEigen policy_; 124 | std::string standCtrlWeights_; 125 | std::string ctrlWeights_; 126 | 127 | //! Parameter loading 128 | void loadParameters(); 129 | 130 | //! NN parameter loading 131 | void loadNNparams(); 132 | 133 | // //! observation 134 | // std::unique_ptr go1Obs_; 135 | 136 | double clipAction_ = 100.; 137 | Eigen::VectorXd clipPoseLower_; 138 | Eigen::VectorXd clipPoseUpper_; 139 | double actionScale_ = 0.25; 140 | double stiffness_ = 20.; 141 | double damping_ = 0.5; 142 | Eigen::VectorXd pGains_; 143 | Eigen::VectorXd dGains_; 144 | 145 | // variables related to control and estimation 146 | Go1Kinematics go1_kin; 147 | Go1CtrlStates go1_ctrl_states; 148 | Go1BasicEKF go1_estimate; 149 | 150 | // joystic command 151 | double joy_cmd_velx = 0.0; 152 | double joy_cmd_vely = 0.0; 153 | double joy_cmd_velz = 0.0; 154 | double joy_cmd_roll_rate = 0.0; 155 | double joy_cmd_pitch_rate = 0.0; 156 | double joy_cmd_yaw_rate = 0.0; 157 | double joy_cmd_pitch_ang = 0.0; 158 | double joy_cmd_roll_ang = 0.0; 159 | double joy_cmd_body_height = 0.17; 160 | 161 | // 0 is standing, 1 is walking 162 | int joy_cmd_ctrl_state = 0; 163 | bool joy_cmd_ctrl_state_change_request = false; 164 | int prev_joy_cmd_ctrl_state = 0; 165 | bool joy_cmd_exit = false; 166 | 167 | // following cade is also in VILEOM 168 | // add leg kinematics 169 | // the leg kinematics is relative to body frame, which is the center of the robot 170 | // following are some parameters that defines the transformation between IMU frame(b) and robot body frame(r) 171 | Eigen::Vector3d p_br; 172 | Eigen::Matrix3d R_br; 173 | // for each leg, there is an offset between the body frame and the hip motor (fx, fy) 174 | double leg_offset_x[4] = {}; 175 | double leg_offset_y[4] = {}; 176 | // for each leg, there is an offset between the body frame and the hip motor (fx, fy) 177 | double motor_offset[4] = {}; 178 | double upper_leg_length[4] = {}; 179 | double lower_leg_length[4] = {}; 180 | std::vector rho_fix_list; 181 | std::vector rho_opt_list; 182 | 183 | // go1 hardware foot force filter 184 | Eigen::Matrix foot_force_filters; 185 | Eigen::Matrix foot_force_filters_idx; 186 | Eigen::Matrix foot_force_filters_sum; 187 | 188 | 189 | // go1 hardware reading thread 190 | std::thread thread_; 191 | bool destruct = false; 192 | 193 | // ! servo motion time 194 | double servo_motion_time_; 195 | 196 | }; 197 | 198 | #endif //GO1_CPP_HARDWAREGO1ROS_H 199 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/MainGazebo.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zixin on 11/1/21. 3 | // 4 | // stl 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // ROS 12 | #include 13 | #include 14 | #include 15 | 16 | //#include "torch_eigen/TorchEigen.hpp" 17 | #include "Go1RLController.hpp" 18 | #include "SwitchController.hpp" 19 | #include "servo_stand_policy/GazeboServo.hpp" 20 | 21 | 22 | int main(int argc, char **argv) { 23 | ros::init(argc, argv, "gazebo_go1_rl_ctrl"); 24 | ros::NodeHandle nh; 25 | 26 | double action_update_frequency, deployment_frequency; 27 | ros::param::get("action_update_frequency", action_update_frequency); 28 | ros::param::get("deployment_frequency", deployment_frequency); 29 | 30 | // change ros logger 31 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { 32 | ros::console::notifyLoggerLevelsChanged(); 33 | } 34 | 35 | // make sure the ROS infra using sim time, otherwise the controller cannot run with correct time steps 36 | bool use_sim_time; 37 | if (ros::param::get("use_sim_time", use_sim_time)) { 38 | if (!use_sim_time) { 39 | ROS_WARN_STREAM(" ROS must set use_sim_time in order to use this program! "); 40 | return -1; 41 | } 42 | } 43 | 44 | // create go1 controllers 45 | std::unique_ptr go1_rl = std::make_unique(nh); 46 | std::unique_ptr switch_ctrl = std::make_unique(nh); 47 | std::unique_ptr servo = std::make_unique(nh); 48 | 49 | 50 | std::atomic control_execute{}; 51 | control_execute.store(true, std::memory_order_release); 52 | 53 | // Thread 1: compute command, forward the NN policy network 54 | std::cout << "Enter thread 1: compute desired action command" << std::endl; 55 | std::thread compute_action_thread([&]() { 56 | // prepare variables to monitor time and control the while loop 57 | ros::Time start = ros::Time::now(); 58 | ros::Time prev = ros::Time::now(); 59 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 60 | ros::Duration dt(0); 61 | 62 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 63 | 64 | ros::Duration(action_update_frequency / 1000).sleep(); 65 | 66 | // get t and dt 67 | now = ros::Time::now(); 68 | dt = now - prev; 69 | prev = now; 70 | 71 | auto t1 = std::chrono::high_resolution_clock::now(); 72 | 73 | bool running; 74 | switch_ctrl->updateMovementMode(); 75 | if (switch_ctrl->movement_mode == 0) { // stand 76 | // compute desired ground forces 77 | // running = go1->update_foot_forces_grf(dt.toSec()); 78 | // running = servo->state_pub(); 79 | running = go1_rl->advance_servo(dt.toSec()); 80 | } else { // walk 81 | // compute actions 82 | running = go1_rl->advance(dt.toSec()); 83 | } 84 | 85 | auto t2 = std::chrono::high_resolution_clock::now(); 86 | std::chrono::duration ms_double = t2 - t1; 87 | // if (switch_ctrl->movement_mode == 1) { // stand 88 | // std::cout << "Controller solution is updated in " << ms_double.count() << "ms" << std::endl; 89 | // } 90 | 91 | if (!running) { 92 | std::cout << "Thread 1 loop is terminated because of errors." << std::endl; 93 | ros::shutdown(); 94 | std::terminate(); 95 | break; 96 | } 97 | } 98 | 99 | }); 100 | 101 | // Thread 2: depolyment on the real system; send commands 102 | std::cout << "Enter thread 2: Deployment on the real robot, sending command" << std::endl; 103 | std::thread deploy_thread([&]() { 104 | // prepare variables to monitor time and control the while loop 105 | ros::Time start = ros::Time::now(); 106 | ros::Time prev = ros::Time::now(); 107 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 108 | ros::Duration dt(0); 109 | 110 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 111 | auto t3 = std::chrono::high_resolution_clock::now(); 112 | 113 | ros::Duration(deployment_frequency / 1000).sleep(); 114 | 115 | // get t and dt 116 | now = ros::Time::now(); 117 | dt = now - prev; 118 | prev = now; 119 | 120 | bool send_cmd_running = go1_rl->send_cmd(); 121 | 122 | auto t4 = std::chrono::high_resolution_clock::now(); 123 | std::chrono::duration ms_double = t4 - t3; 124 | // std::cout << "Thread 2 is updated in " << ms_double.count() << "ms" << std::endl; 125 | 126 | if (!send_cmd_running) { 127 | std::cout << "Thread 2 loop is terminated because of errors." << std::endl; 128 | ros::shutdown(); 129 | std::terminate(); 130 | break; 131 | } 132 | 133 | } 134 | }); 135 | 136 | ros::AsyncSpinner spinner(12); 137 | spinner.start(); 138 | 139 | compute_action_thread.join(); 140 | deploy_thread.join(); 141 | 142 | return 0; 143 | 144 | } 145 | 146 | 147 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/SwitchController.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 12.10.22. 3 | // 4 | 5 | // ROS 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class SwitchController { 12 | public: 13 | SwitchController(ros::NodeHandle &nh) { 14 | sub_joy_msg_ = nh.subscribe("/joy", 1000, &SwitchController::joy_callback, this); 15 | 16 | joy_cmd_ctrl_state_ = 0; 17 | joy_cmd_ctrl_state_change_request_ = false; 18 | prev_joy_cmd_ctrl_state_ = 0; 19 | joy_cmd_exit_ = false; 20 | 21 | } 22 | 23 | void updateMovementMode() { // this is independent with the one in Go1 observation. Here is for changing the outside mode 24 | // update joy cmd 25 | prev_joy_cmd_ctrl_state_ = joy_cmd_ctrl_state_; 26 | 27 | if (joy_cmd_ctrl_state_change_request_) { 28 | // toggle joy_cmd_ctrl_state 29 | joy_cmd_ctrl_state_ = joy_cmd_ctrl_state_ + 1; 30 | joy_cmd_ctrl_state_ = joy_cmd_ctrl_state_ % 2; //TODO: how to toggle more states? 31 | joy_cmd_ctrl_state_change_request_ = false; //erase this change request; 32 | } 33 | 34 | // determine movement mode 35 | if (joy_cmd_ctrl_state_ == 1) { 36 | // walking mode, in this mode the robot should execute gait 37 | movement_mode = 1; 38 | } else if (joy_cmd_ctrl_state_ == 0 && prev_joy_cmd_ctrl_state_ == 1) { 39 | // leave walking mode 40 | // lock current position, should just happen for one instance 41 | movement_mode = 0; 42 | } else { 43 | movement_mode = 0; 44 | } 45 | 46 | 47 | } // updateMovementMode 48 | 49 | void joy_callback(const sensor_msgs::Joy::ConstPtr &joy_msg) { 50 | //A 51 | if (joy_msg->buttons[0] == 1) { 52 | joy_cmd_ctrl_state_change_request_ = true; 53 | } 54 | } 55 | 56 | int movement_mode; 57 | 58 | private: 59 | ros::Subscriber sub_joy_msg_; 60 | 61 | // 0 is standing, 1 is walking 62 | int joy_cmd_ctrl_state_ = 0; 63 | int prev_joy_cmd_ctrl_state_ = 0; 64 | bool joy_cmd_ctrl_state_change_request_ = false; 65 | bool joy_cmd_exit_ = false; 66 | 67 | 68 | 69 | }; 70 | 71 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/TestHardware.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zixin on 11/1/21. 3 | // 4 | // stl 5 | #include "Go1RLHardwareController.hpp" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // ROS 14 | #include 15 | #include 16 | #include 17 | 18 | // control parameters 19 | #include "Go1Params.hpp" 20 | #include "servo_stand_policy/HardwareServo.hpp" 21 | #include "servo_stand_policy/HardwareServoSwitch.hpp" 22 | #include "SwitchController.hpp" 23 | 24 | 25 | 26 | int main(int argc, char **argv) { 27 | std::cout << "Communication level is set to LOW-level." << std::endl 28 | << "Stand" << std::endl 29 | << "Press Enter to continue..." << std::endl; 30 | std::cin.ignore(); 31 | 32 | ros::init(argc, argv, "hardware_go1_rl_ctrl"); 33 | ros::NodeHandle nh; 34 | 35 | double action_update_frequency, deployment_frequency; 36 | ros::param::get("action_update_frequency", action_update_frequency); 37 | ros::param::get("deployment_frequency", deployment_frequency); 38 | 39 | // change ros logger 40 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { 41 | ros::console::notifyLoggerLevelsChanged(); 42 | } 43 | 44 | // make sure the ROS infra DO NOT use sim time, otherwise the controller cannot run with correct time steps 45 | std::string use_sim_time; 46 | if (ros::param::get("/use_sim_time", use_sim_time)) { 47 | if (use_sim_time != "false") { 48 | std::cout << "hardware must have real time in order to use this program!" << std::endl; 49 | return -1; 50 | } 51 | } 52 | 53 | // create go1 controllers 54 | std::unique_ptr go1_rl = std::make_unique(nh); 55 | std::unique_ptr switch_ctrl = std::make_unique(nh); 56 | std::unique_ptr servo = std::make_unique(); 57 | std::unique_ptr servo_switch = std::make_unique(); 58 | 59 | std::atomic control_execute{}; 60 | control_execute.store(true, std::memory_order_release); 61 | 62 | // Thread 1: compute command, forward the NN policy network 63 | std::cout << "Enter thread 1: compute desired action command" << std::endl; 64 | std::thread compute_action_thread([&]() { 65 | // prepare variables to monitor time and control the while loop 66 | ros::Time start = ros::Time::now(); 67 | ros::Time prev = ros::Time::now(); 68 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 69 | ros::Duration dt(0); 70 | ros::Duration dt_solver_time(0); 71 | 72 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 73 | 74 | // ros::Duration(action_update_frequency / 1000).sleep(); 75 | 76 | // get t and dt 77 | now = ros::Time::now(); 78 | dt = now - prev; 79 | prev = now; 80 | 81 | auto t1 = std::chrono::high_resolution_clock::now(); 82 | 83 | bool running; 84 | switch_ctrl->updateMovementMode(); 85 | if (switch_ctrl->movement_mode == 0) { // stand 86 | // servo controller 87 | // running = servo->UDPRecv(); 88 | running = go1_rl->advance_servo(); 89 | } else { // walk 90 | // running = servo_switch->UDPRecv(); 91 | running = go1_rl->advance(); 92 | } 93 | 94 | dt_solver_time = ros::Time::now() - now; 95 | 96 | auto t2 = std::chrono::high_resolution_clock::now(); 97 | std::chrono::duration ms_double = t2 - t1; 98 | if (switch_ctrl->movement_mode == 1) { // stand 99 | std::cout << "thread 1 solution is updated in " << ms_double.count() << "ms" << std::endl; 100 | } 101 | 102 | if (!running) { 103 | std::cout << "Thread 1 loop is terminated because of errors." << std::endl; 104 | ros::shutdown(); 105 | std::terminate(); 106 | break; 107 | } 108 | 109 | if (dt_solver_time.toSec() < action_update_frequency / 1000) { 110 | ros::Duration( action_update_frequency / 1000 - dt_solver_time.toSec() ).sleep(); 111 | } 112 | } 113 | 114 | }); 115 | 116 | // Thread 2: depolyment on the real system; send commands 117 | std::cout << "Enter thread 2: Deployment on the real robot, sending command" << std::endl; 118 | std::thread deploy_thread([&]() { 119 | // prepare variables to monitor time and control the while loop 120 | ros::Time start = ros::Time::now(); 121 | ros::Time prev = ros::Time::now(); 122 | ros::Time now = ros::Time::now(); // bool res = app.exec(); 123 | ros::Duration dt(0); 124 | ros::Duration dt_solver_time(0); 125 | 126 | while (control_execute.load(std::memory_order_acquire) && ros::ok()) { 127 | // auto t3 = std::chrono::high_resolution_clock::now(); 128 | 129 | // get t and dt 130 | now = ros::Time::now(); 131 | dt = now - prev; 132 | prev = now; 133 | 134 | bool send_cmd_running = go1_rl->send_cmd(); 135 | 136 | if (!send_cmd_running) { 137 | std::cout << "Thread 2 loop is terminated because of errors." << std::endl; 138 | ros::shutdown(); 139 | std::terminate(); 140 | break; 141 | } 142 | 143 | dt_solver_time = ros::Time::now() - now; 144 | if (dt_solver_time.toSec() < deployment_frequency / 1000) { 145 | ros::Duration( deployment_frequency / 1000 - dt_solver_time.toSec() ).sleep(); 146 | } 147 | } 148 | }); 149 | 150 | ros::AsyncSpinner spinner(12); 151 | spinner.start(); 152 | 153 | compute_action_thread.join(); 154 | deploy_thread.join(); 155 | 156 | return 0; 157 | 158 | } 159 | 160 | 161 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/legKinematics/Go1Kinematics.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 8/10/21. 3 | // 4 | 5 | #ifndef VILEOM_GO1KINEMATICS_H 6 | #define VILEOM_GO1KINEMATICS_H 7 | 8 | #include 9 | 10 | class Go1Kinematics { 11 | 12 | public: 13 | Go1Kinematics() = default; 14 | ~Go1Kinematics() = default; 15 | 16 | const int RHO_OPT_SIZE = 3; 17 | const int RHO_FIX_SIZE = 5; 18 | // rho opt are contact offset cx cy cz 19 | // rho fix are body offset x& y, thigh offset, upper leg length, lower leg length 20 | // functions with eigen interface 21 | // forward kinematics 3x1 22 | Eigen::Vector3d fk(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 23 | // jacobian 3x3 24 | Eigen::Matrix3d jac(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 25 | // the partial derivative of fk wrt rho opt 3x3 26 | Eigen::Matrix3d dfk_drho(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 27 | // the partial derivative of jacobian wrt q 9x3 28 | Eigen::Matrix dJ_dq(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 29 | // the partial derivative of jacobian wrt rho opt 9x3 30 | Eigen::Matrix dJ_drho(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix); 31 | private: 32 | // functions with basic C++ interface, generated by Matlab 33 | void autoFunc_fk_derive(const double in1[3], const double in2[3], const double in3[5], double p_bf[3]); 34 | void autoFunc_d_fk_dq(const double in1[3], const double in2[3], const double in3[5], double jacobian[9]); 35 | void autoFunc_d_fk_dc(const double in1[3], const double in2[3], const double in3[5], double d_fk_dc[9]); 36 | void autoFunc_dJ_dq(const double in1[3], const double in2[3], const double in3[5], double dJ_dq[27]); 37 | void autoFunc_dJ_dpho(const double in1[3], const double [3], const double [5], double dJ_dpho[27]); 38 | }; 39 | 40 | 41 | #endif //VILEOM_GO1KINEMATICS_H 42 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/legKinematics/README.md: -------------------------------------------------------------------------------- 1 | This folder should contain robot leg kinematics 2 | 3 | Currently we have A1 robot's leg kinematics. 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/observation/Go1HardwareObservation.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 30.09.22. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // ROS 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | // go1 hardware 31 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 32 | 33 | #include "../Go1Params.hpp" 34 | #include "../Go1CtrlStates.hpp" 35 | #include "../utils/Utils.hpp" 36 | #include "../utils/filter.hpp" 37 | #include "../EKF/Go1BasicEKF.hpp" 38 | #include "../legKinematics/Go1Kinematics.hpp" 39 | 40 | #define FOOT_FILTER_WINDOW_SIZE 5 41 | 42 | // TODO: compute the observation vector as the one in issac gym legged_robot.py 43 | class Go1HardwareObservation{ 44 | public: 45 | Go1HardwareObservation(ros::NodeHandle &nh): 46 | udp(UNITREE_LEGGED_SDK::LOWLEVEL, 8090, "192.168.123.10", 8007) { 47 | obDim_ = 36; 48 | obDouble_.setZero(obDim_); obScaled_.setZero(obDim_); 49 | 50 | // scale factor init 51 | linVelScale_.setZero(); 52 | angVelScale_.setZero(); 53 | gravityScale_.setZero(); 54 | commandScale_.setZero(); 55 | linVelScale_ << 2.0, 2.0, 2.0; 56 | angVelScale_ << 0.25, 0.25, 0.25; 57 | gravityScale_ << 1.0, 1.0, 1.0; // projected gravity 58 | commandScale_ << 2.0, 2.0, 0.25; // for cmd_velx, cmd_vely, cmd_ang_vel_yaw 59 | 60 | dofPosScale_.setZero(12); dofVelScale_.setZero(12); 61 | for (int i = 0; i < 12; i++) { 62 | dofPosScale_[i] = 1.0; 63 | dofVelScale_[i] = 0.05; 64 | } 65 | scaleFactor_.setZero(obDim_); 66 | scaleFactor_ << linVelScale_, angVelScale_, gravityScale_, commandScale_, dofPosScale_, dofVelScale_; 67 | 68 | // joy command 69 | sub_joy_msg = nh.subscribe("/joy", 1000, &Go1HardwareObservation::joy_callback, this); 70 | 71 | joy_cmd_ctrl_state_ = 0; 72 | joy_cmd_ctrl_state_change_request_ = false; 73 | prev_joy_cmd_ctrl_state_ = 0; 74 | joy_cmd_exit_ = false; 75 | 76 | go1_ctrl_states_.reset(); 77 | go1_ctrl_states_.resetFromROSParam(nh); 78 | 79 | // start hardware reading thread after everything initialized 80 | thread_ = std::thread(&Go1HardwareObservation::receive_low_state, this); 81 | 82 | } 83 | 84 | ~Go1HardwareObservation() { // move from the controller to here 85 | destruct = true; 86 | thread_.join(); 87 | } 88 | 89 | void updateObservation(double dt) { 90 | // TODO: get all observation from measurement and concat 91 | // assign observation vector 92 | obDouble_.segment(0, 3) = go1_ctrl_states_.root_rot_mat_z.transpose() * go1_ctrl_states_.root_lin_vel; // 1. base linear velocity(robot frame) 93 | obDouble_.segment(3, 3) = go1_ctrl_states_.imu_ang_vel; // 2. base angular velocity(robot frame, [roll, pitch, yaw]) 94 | obDouble_.segment(6, 3) = go1_ctrl_states_.root_rot_mat.transpose() * (-Eigen::Vector3d::UnitZ()); // 3. projected gravity(projected z unit vector) 95 | obDouble_.segment(9, 3) << joy_cmd_velx_, joy_cmd_vely_, joy_cmd_yaw_rate_; // 4. command([cmd_velx, cmd_vely, cmd_ang_vel_yaw]) 96 | obDouble_.segment(12, 12) = go1_ctrl_states_.joint_pos - go1_ctrl_states_.default_joint_pos; // 5. (joint_pos - default_joint_pos) 97 | obDouble_.segment(24, 12) = go1_ctrl_states_.joint_vel; // 6. joint_vel 98 | // obDouble_.segment(36, 12) = go1_ctrl_states_.joint_actions; // 7. actions(clipped NN outputs) 99 | 100 | // scale the observation 101 | for (int i = 0; i < obDouble_.size(); i++) { 102 | obScaled_[i] = obDouble_[i] * scaleFactor_[i]; 103 | } 104 | 105 | // clip the observation 106 | obScaled_ = obScaled_.cwiseMin(clipObs_).cwiseMax(-clipObs_); 107 | 108 | updateMovementMode(); 109 | 110 | } 111 | 112 | 113 | void updateMovementMode() { // keep this: the movement mode in ctrl state is used in the EKF. 114 | // update joy cmd 115 | prev_joy_cmd_ctrl_state_ = joy_cmd_ctrl_state_; 116 | 117 | if (joy_cmd_ctrl_state_change_request_) { 118 | // toggle joy_cmd_ctrl_state 119 | joy_cmd_ctrl_state_ = joy_cmd_ctrl_state_ + 1; 120 | joy_cmd_ctrl_state_ = joy_cmd_ctrl_state_ % 2; //TODO: how to toggle more states? 121 | joy_cmd_ctrl_state_change_request_ = false; //erase this change request; 122 | } 123 | 124 | // determine movement mode 125 | if (joy_cmd_ctrl_state_ == 1) { 126 | // walking mode, in this mode the robot should execute gait 127 | go1_ctrl_states_.movement_mode = 1; 128 | } else if (joy_cmd_ctrl_state_ == 0 && prev_joy_cmd_ctrl_state_ == 1) { 129 | // leave walking mode 130 | go1_ctrl_states_.movement_mode = 0; 131 | } else { 132 | go1_ctrl_states_.movement_mode = 0; 133 | } 134 | 135 | } // updateMovementMode 136 | 137 | Eigen::VectorXd getObservation() { return obScaled_; } 138 | Go1CtrlStates getCtrlState() { return go1_ctrl_states_; } 139 | 140 | 141 | void joy_callback(const sensor_msgs::Joy::ConstPtr &joy_msg) { // This function applies for both gazebo and hardware 142 | // // left updown: change body height, not need now 143 | // joy_cmd_velz = joy_msg->axes[1] * JOY_CMD_BODY_HEIGHT_VEL; 144 | 145 | //A 146 | if (joy_msg->buttons[0] == 1) { 147 | joy_cmd_ctrl_state_change_request_ = true; 148 | } 149 | 150 | // right updown 151 | joy_cmd_velx_ = joy_msg->axes[4] * JOY_CMD_VELX_MAX; 152 | // right horiz 153 | joy_cmd_vely_ = joy_msg->axes[3] * JOY_CMD_VELY_MAX; 154 | // left horiz 155 | joy_cmd_yaw_rate_ = joy_msg->axes[0] * JOY_CMD_YAW_MAX; 156 | 157 | // lb 158 | if (joy_msg->buttons[4] == 1) { 159 | std::cout << "You have pressed the exit button!!!!" << std::endl; 160 | joy_cmd_exit_ = true; 161 | } 162 | } 163 | 164 | 165 | 166 | private: 167 | // 0, 1, 2: FL_hip, FL_thigh, FL_calf 168 | // 3, 4, 5: FR_hip, FR_thigh, FR_calf 169 | // 6, 7, 8: RL_hip, RL_thigh, RL_calf 170 | // 9, 10, 11: RR_hip, RR_thigh, RR_calf 171 | 172 | ros::Subscriber sub_joy_msg; 173 | 174 | // ! observation for RL controller 175 | size_t obDim_; 176 | 177 | Eigen::VectorXd obDouble_, obScaled_; 178 | Eigen::VectorXd actionMean_, actionStd_, obMean_, obStd_; 179 | 180 | Eigen::Vector3d linVelScale_, angVelScale_, gravityScale_, commandScale_; 181 | Eigen::VectorXd dofPosScale_, dofVelScale_; 182 | Eigen::VectorXd scaleFactor_; 183 | double clipObs_ = 100.; 184 | 185 | // joystick command 186 | double joy_cmd_velx_ = 0.0; 187 | double joy_cmd_vely_ = 0.0; 188 | double joy_cmd_velz_ = 0.0; 189 | 190 | double joy_cmd_yaw_rate_ = 0.0; 191 | 192 | double joy_cmd_body_height_ = 0.3; 193 | 194 | // 0 is standing, 1 is walking 195 | int joy_cmd_ctrl_state_ = 0; 196 | int prev_joy_cmd_ctrl_state_ = 0; 197 | bool joy_cmd_ctrl_state_change_request_ = false; 198 | bool joy_cmd_exit_ = false; 199 | 200 | Go1CtrlStates go1_ctrl_states_; // underscore to consistent with gazebo 201 | 202 | 203 | UNITREE_LEGGED_SDK::UDP udp; 204 | UNITREE_LEGGED_SDK::LowState state = {0}; 205 | 206 | 207 | }; 208 | 209 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/servo_stand_policy/GazeboServo.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 14.10.22. 3 | // 4 | 5 | #include "ros/ros.h" 6 | #include 7 | #include 8 | #include "unitree_legged_msgs/LowCmd.h" 9 | #include "unitree_legged_msgs/LowState.h" 10 | #include "unitree_legged_msgs/HighState.h" 11 | #include "unitree_legged_msgs/MotorCmd.h" 12 | #include "unitree_legged_msgs/MotorState.h" 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | class GazeboServo 22 | { 23 | public: 24 | GazeboServo(ros::NodeHandle &nh); 25 | bool state_pub(); 26 | 27 | void paramInit(); 28 | bool send_cmd(); 29 | 30 | void imuCallback(const sensor_msgs::Imu & msg); 31 | 32 | void FRhipCallback(const unitree_legged_msgs::MotorState& msg); 33 | void FRthighCallback(const unitree_legged_msgs::MotorState& msg); 34 | void FRcalfCallback(const unitree_legged_msgs::MotorState& msg); 35 | void FLhipCallback(const unitree_legged_msgs::MotorState& msg); 36 | void FLthighCallback(const unitree_legged_msgs::MotorState& msg); 37 | void FLcalfCallback(const unitree_legged_msgs::MotorState& msg); 38 | void RRhipCallback(const unitree_legged_msgs::MotorState& msg); 39 | void RRthighCallback(const unitree_legged_msgs::MotorState& msg); 40 | void RRcalfCallback(const unitree_legged_msgs::MotorState& msg); 41 | void RLhipCallback(const unitree_legged_msgs::MotorState& msg); 42 | void RLthighCallback(const unitree_legged_msgs::MotorState& msg); 43 | void RLcalfCallback(const unitree_legged_msgs::MotorState& msg); 44 | 45 | void FRfootCallback(const geometry_msgs::WrenchStamped& msg); 46 | void FLfootCallback(const geometry_msgs::WrenchStamped& msg); 47 | void RRfootCallback(const geometry_msgs::WrenchStamped& msg); 48 | void RLfootCallback(const geometry_msgs::WrenchStamped& msg); 49 | 50 | 51 | private: 52 | ros::Subscriber servo_sub[12], footForce_sub[4], imu_sub; 53 | ros::Publisher servo_pub[12]; 54 | ros::Publisher highState_pub; 55 | ros::Publisher lowState_pub; //for rviz visualization 56 | unitree_legged_msgs::LowCmd lowCmd; 57 | unitree_legged_msgs::LowState lowState; 58 | 59 | double motion_time; 60 | double duration = 1000; 61 | double target_pos[12]; 62 | 63 | }; -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/servo_stand_policy/HardwareServo.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 14.10.22. 3 | // 4 | 5 | #include "HardwareServo.hpp" 6 | 7 | HardwareServo::HardwareServo(): 8 | safe(UNITREE_LEGGED_SDK::LeggedType::Go1), udp(UNITREE_LEGGED_SDK::LOWLEVEL, 8090, "192.168.123.10", 8007) { 9 | udp.InitCmdData(cmd); 10 | paramInit(); 11 | } 12 | 13 | bool HardwareServo::UDPRecv() 14 | { 15 | udp.Recv(); 16 | udp.GetRecv(state); 17 | return true; 18 | } 19 | 20 | void HardwareServo::paramInit() 21 | { 22 | for(int i=0; i<2; i++){ 23 | cmd.motorCmd[i*3+0].mode = 0x0A; 24 | cmd.motorCmd[i*3+0].Kp = 10; 25 | cmd.motorCmd[i*3+0].dq = 0; 26 | cmd.motorCmd[i*3+0].Kd = 3; 27 | cmd.motorCmd[i*3+0].tau = 0; 28 | cmd.motorCmd[i*3+1].mode = 0x0A; 29 | cmd.motorCmd[i*3+1].Kp = 40; 30 | cmd.motorCmd[i*3+1].dq = 0; 31 | cmd.motorCmd[i*3+1].Kd = 4; 32 | cmd.motorCmd[i*3+1].tau = 0; 33 | cmd.motorCmd[i*3+2].mode = 0x0A; 34 | cmd.motorCmd[i*3+2].Kp = 20; 35 | cmd.motorCmd[i*3+2].dq = 0; 36 | cmd.motorCmd[i*3+2].Kd = 2; 37 | cmd.motorCmd[i*3+2].tau = 0; 38 | } 39 | 40 | for(int i=2; i<4; i++){ 41 | cmd.motorCmd[i*3+0].mode = 0x0A; 42 | cmd.motorCmd[i*3+0].Kp = 10; 43 | cmd.motorCmd[i*3+0].dq = 0; 44 | cmd.motorCmd[i*3+0].Kd = 3; 45 | cmd.motorCmd[i*3+0].tau = 0; 46 | cmd.motorCmd[i*3+1].mode = 0x0A; 47 | cmd.motorCmd[i*3+1].Kp = 40; 48 | cmd.motorCmd[i*3+1].dq = 0; 49 | cmd.motorCmd[i*3+1].Kd = 4; 50 | cmd.motorCmd[i*3+1].tau = 0; 51 | cmd.motorCmd[i*3+2].mode = 0x0A; 52 | cmd.motorCmd[i*3+2].Kp = 30; 53 | cmd.motorCmd[i*3+2].dq = 0; 54 | cmd.motorCmd[i*3+2].Kd = 2; 55 | cmd.motorCmd[i*3+2].tau = 0; 56 | } 57 | 58 | } 59 | 60 | bool HardwareServo::send_cmd() { 61 | double pos[12] = {0.0, 0.6, -1.3, -0.0, 0.6, -1.3, 62 | 0.0, 0.6, -1.3, -0.0, 0.6, -1.3}; 63 | 64 | // motor q command init 65 | for(int i=0; i<12; i++){ 66 | cmd.motorCmd[i].q = state.motorState[i].q; 67 | } 68 | // change q command overtime 69 | moveAllPosition(pos, 2*30000); 70 | 71 | return true; 72 | } 73 | 74 | void HardwareServo::moveAllPosition(double* targetPos, double duration) { 75 | double pos[12] ,lastPos[12], percent; 76 | for(int j=0; j<12; j++) lastPos[j] = state.motorState[j].q; 77 | for(int i=1; i<=duration; i++){ 78 | percent = (double)i/duration; 79 | // std::cout << "percent" << percent << std::endl; 80 | for(int j=0; j<12; j++){ 81 | cmd.motorCmd[j].q = lastPos[j]*(1-percent) + targetPos[j]*percent; 82 | } 83 | } 84 | 85 | safe.PositionLimit(cmd); 86 | safe.PowerProtect(cmd, state, 5); 87 | // safe.PositionProtect(cmd, state, -0.2); 88 | udp.SetSend(cmd); 89 | udp.Send(); 90 | } -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/servo_stand_policy/HardwareServo.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 14.10.22. 3 | // 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class HardwareServo 12 | { 13 | public: 14 | HardwareServo(); 15 | bool UDPRecv(); 16 | void RobotControl(); 17 | 18 | void paramInit(); 19 | bool send_cmd(); 20 | void moveAllPosition(double* targetPos, double duration); 21 | 22 | UNITREE_LEGGED_SDK::Safety safe; 23 | UNITREE_LEGGED_SDK::UDP udp; 24 | UNITREE_LEGGED_SDK::LowCmd cmd = {0}; 25 | UNITREE_LEGGED_SDK::LowState state = {0}; 26 | 27 | float dt = 0.002; // 0.001~0.01 28 | }; -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/servo_stand_policy/HardwareServoSwitch.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 14.10.22. 3 | // 4 | 5 | #include "HardwareServoSwitch.hpp" 6 | 7 | HardwareServoSwitch::HardwareServoSwitch(): 8 | safe(UNITREE_LEGGED_SDK::LeggedType::Go1), udp(UNITREE_LEGGED_SDK::LOWLEVEL, 8090, "192.168.123.10", 8007) { 9 | udp.InitCmdData(cmd); 10 | paramInit(); 11 | } 12 | 13 | bool HardwareServoSwitch::UDPRecv() 14 | { 15 | udp.Recv(); 16 | udp.GetRecv(state); 17 | return true; 18 | } 19 | 20 | void HardwareServoSwitch::paramInit() 21 | { 22 | for(int i=0; i<2; i++){ 23 | cmd.motorCmd[i*3+0].mode = 0x0A; 24 | cmd.motorCmd[i*3+0].Kp = 10; 25 | cmd.motorCmd[i*3+0].dq = 0; 26 | cmd.motorCmd[i*3+0].Kd = 3; 27 | cmd.motorCmd[i*3+0].tau = 0; 28 | cmd.motorCmd[i*3+1].mode = 0x0A; 29 | cmd.motorCmd[i*3+1].Kp = 40; 30 | cmd.motorCmd[i*3+1].dq = 0; 31 | cmd.motorCmd[i*3+1].Kd = 4; 32 | cmd.motorCmd[i*3+1].tau = 0; 33 | cmd.motorCmd[i*3+2].mode = 0x0A; 34 | cmd.motorCmd[i*3+2].Kp = 20; 35 | cmd.motorCmd[i*3+2].dq = 0; 36 | cmd.motorCmd[i*3+2].Kd = 2; 37 | cmd.motorCmd[i*3+2].tau = 0; 38 | } 39 | 40 | for(int i=2; i<4; i++){ 41 | cmd.motorCmd[i*3+0].mode = 0x0A; 42 | cmd.motorCmd[i*3+0].Kp = 10; 43 | cmd.motorCmd[i*3+0].dq = 0; 44 | cmd.motorCmd[i*3+0].Kd = 3; 45 | cmd.motorCmd[i*3+0].tau = 0; 46 | cmd.motorCmd[i*3+1].mode = 0x0A; 47 | cmd.motorCmd[i*3+1].Kp = 40; 48 | cmd.motorCmd[i*3+1].dq = 0; 49 | cmd.motorCmd[i*3+1].Kd = 4; 50 | cmd.motorCmd[i*3+1].tau = 0; 51 | cmd.motorCmd[i*3+2].mode = 0x0A; 52 | cmd.motorCmd[i*3+2].Kp = 30; 53 | cmd.motorCmd[i*3+2].dq = 0; 54 | cmd.motorCmd[i*3+2].Kd = 2; 55 | cmd.motorCmd[i*3+2].tau = 0; 56 | } 57 | 58 | } 59 | 60 | bool HardwareServoSwitch::send_cmd() { 61 | double pos[12] = {0.0, 0.67, -2.0, -0.0, 0.67, -2.0, 62 | 0.0, 0.67, -1.8, -0.0, 0.67, -1.8}; 63 | 64 | // motor q command init 65 | for(int i=0; i<12; i++){ 66 | cmd.motorCmd[i].q = state.motorState[i].q; 67 | } 68 | // change q command overtime 69 | moveAllPosition(pos, 2*30000); 70 | 71 | return true; 72 | } 73 | 74 | void HardwareServoSwitch::moveAllPosition(double* targetPos, double duration) { 75 | double pos[12] ,lastPos[12], percent; 76 | for(int j=0; j<12; j++) lastPos[j] = state.motorState[j].q; 77 | for(int i=1; i<=duration; i++){ 78 | percent = (double)i/duration; 79 | // std::cout << "percent" << percent << std::endl; 80 | for(int j=0; j<12; j++){ 81 | cmd.motorCmd[j].q = lastPos[j]*(1-percent) + targetPos[j]*percent; 82 | } 83 | } 84 | 85 | safe.PositionLimit(cmd); 86 | safe.PowerProtect(cmd, state, 5); 87 | // safe.PositionProtect(cmd, state, -0.2); 88 | udp.SetSend(cmd); 89 | udp.Send(); 90 | } -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/servo_stand_policy/HardwareServoSwitch.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 14.10.22. 3 | // 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class HardwareServoSwitch 12 | { 13 | public: 14 | HardwareServoSwitch(); 15 | bool UDPRecv(); 16 | void RobotControl(); 17 | 18 | void paramInit(); 19 | bool send_cmd(); 20 | void moveAllPosition(double* targetPos, double duration); 21 | 22 | UNITREE_LEGGED_SDK::Safety safe; 23 | UNITREE_LEGGED_SDK::UDP udp; 24 | UNITREE_LEGGED_SDK::LowCmd cmd = {0}; 25 | UNITREE_LEGGED_SDK::LowState state = {0}; 26 | 27 | float dt = 0.002; // 0.001~0.01 28 | }; -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/servo_stand_policy/body.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #include "body.h" 7 | 8 | namespace unitree_model { 9 | 10 | ros::Publisher servo_pub[12]; 11 | unitree_legged_msgs::LowCmd lowCmd; 12 | unitree_legged_msgs::LowState lowState; 13 | 14 | // These parameters are only for reference. 15 | // Actual patameters need to be debugged if you want to run on real robot. 16 | void paramInit() 17 | { 18 | for(int i=0; i<4; i++){ 19 | lowCmd.motorCmd[i*3+0].mode = 0x0A; 20 | lowCmd.motorCmd[i*3+0].Kp = 70; 21 | lowCmd.motorCmd[i*3+0].dq = 0; 22 | lowCmd.motorCmd[i*3+0].Kd = 3; 23 | lowCmd.motorCmd[i*3+0].tau = 0; 24 | lowCmd.motorCmd[i*3+1].mode = 0x0A; 25 | lowCmd.motorCmd[i*3+1].Kp = 180; 26 | lowCmd.motorCmd[i*3+1].dq = 0; 27 | lowCmd.motorCmd[i*3+1].Kd = 8; 28 | lowCmd.motorCmd[i*3+1].tau = 0; 29 | lowCmd.motorCmd[i*3+2].mode = 0x0A; 30 | lowCmd.motorCmd[i*3+2].Kp = 300; 31 | lowCmd.motorCmd[i*3+2].dq = 0; 32 | lowCmd.motorCmd[i*3+2].Kd = 15; 33 | lowCmd.motorCmd[i*3+2].tau = 0; 34 | } 35 | for(int i=0; i<12; i++){ 36 | lowCmd.motorCmd[i].q = lowState.motorState[i].q; 37 | } 38 | } 39 | 40 | void stand() 41 | { 42 | double pos[12] = {0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 43 | 0.0, 0.67, -1.3, -0.0, 0.67, -1.3}; 44 | moveAllPosition(pos, 2*4000); 45 | } 46 | 47 | void motion_init() 48 | { 49 | paramInit(); 50 | stand(); 51 | } 52 | 53 | void sendServoCmd() 54 | { 55 | for(int m=0; m<12; m++){ 56 | servo_pub[m].publish(lowCmd.motorCmd[m]); 57 | } 58 | ros::spinOnce(); 59 | usleep(1000); 60 | } 61 | 62 | void moveAllPosition(double* targetPos, double duration) 63 | { 64 | double pos[12] ,lastPos[12], percent; 65 | for(int j=0; j<12; j++) lastPos[j] = lowState.motorState[j].q; 66 | for(int i=1; i<=duration; i++){ 67 | if(!ros::ok()) break; 68 | percent = (double)i/duration; 69 | for(int j=0; j<12; j++){ 70 | lowCmd.motorCmd[j].q = lastPos[j]*(1-percent) + targetPos[j]*percent; 71 | } 72 | sendServoCmd(); 73 | } 74 | } 75 | 76 | 77 | } 78 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/servo_stand_policy/body.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef __BODY_H__ 7 | #define __BODY_H__ 8 | 9 | #include "ros/ros.h" 10 | #include "unitree_legged_msgs/LowCmd.h" 11 | #include "unitree_legged_msgs/LowState.h" 12 | #include "unitree_legged_msgs/HighState.h" 13 | #define PosStopF (2.146E+9f) 14 | #define VelStopF (16000.f) 15 | 16 | namespace unitree_model { 17 | 18 | extern ros::Publisher servo_pub[12]; 19 | extern ros::Publisher highState_pub; 20 | extern unitree_legged_msgs::LowCmd lowCmd; 21 | extern unitree_legged_msgs::LowState lowState; 22 | 23 | void stand(); 24 | void motion_init(); 25 | void sendServoCmd(); 26 | void moveAllPosition(double* jointPositions, double duration); 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/torch_eigen/TorchEigen.cpp: -------------------------------------------------------------------------------- 1 | #include "TorchEigen.hpp" 2 | 3 | 4 | void TorchEigen::load(std::string libraryPath) { 5 | // load in the library 6 | model_ = torch::jit::load(libraryPath); 7 | model_.eval(); 8 | } 9 | 10 | void TorchEigen::run(const Eigen::VectorXf &input, Eigen::VectorXf &output) { 11 | torch::NoGradGuard no_grad; 12 | std::vector T; 13 | T.push_back(eigenToTensor(input).reshape({1, input.rows()})); 14 | at::Tensor model_out = model_.forward(T).toTensor(); 15 | output = tensorToEigen(model_out); 16 | } 17 | 18 | 19 | torch::Tensor TorchEigen::eigenToTensor(const Eigen::VectorXf& e) { 20 | auto t = torch::empty({1, e.rows()}); 21 | for (int i=0; i(); 31 | return e; 32 | } 33 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/torch_eigen/TorchEigen.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | class TorchEigen{ 8 | public: 9 | TorchEigen() = default; 10 | 11 | void load(std::string libraryPath); 12 | void run(const Eigen::VectorXf& input, Eigen::VectorXf& output); 13 | 14 | private: 15 | torch::jit::script::Module model_; 16 | torch::Tensor eigenToTensor(const Eigen::VectorXf& e); 17 | Eigen::VectorXf tensorToEigen(const torch::Tensor& t); 18 | }; 19 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/utils/Utils.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/19/21. 3 | // 4 | 5 | #include "Utils.hpp" 6 | 7 | Eigen::Vector3d Utils::quat_to_euler(Eigen::Quaterniond quat) { 8 | Eigen::Vector3d rst; 9 | 10 | // order https://github.com/libigl/eigen/blob/master/Eigen/src/Geometry/Quaternion.h 11 | Eigen::Matrix coeff = quat.coeffs(); 12 | double x = coeff(0); 13 | double y = coeff(1); 14 | double z = coeff(2); 15 | double w = coeff(3); 16 | 17 | double y_sqr = y*y; 18 | 19 | double t0 = +2.0 * (w * x + y * z); 20 | double t1 = +1.0 - 2.0 * (x * x + y_sqr); 21 | 22 | rst[0] = atan2(t0, t1); 23 | 24 | double t2 = +2.0 * (w * y - z * x); 25 | t2 = t2 > +1.0 ? +1.0 : t2; 26 | t2 = t2 < -1.0 ? -1.0 : t2; 27 | rst[1] = asin(t2); 28 | 29 | double t3 = +2.0 * (w * z + x * y); 30 | double t4 = +1.0 - 2.0 * (y_sqr + z * z); 31 | rst[2] = atan2(t3, t4); 32 | return rst; 33 | } 34 | 35 | Eigen::Matrix3d Utils::skew(Eigen::Vector3d vec) { 36 | Eigen::Matrix3d rst; rst.setZero(); 37 | rst << 0, -vec(2), vec(1), 38 | vec(2), 0, -vec(0), 39 | -vec(1), vec(0), 0; 40 | return rst; 41 | } 42 | 43 | // https://gist.github.com/pshriwise/67c2ae78e5db3831da38390a8b2a209f 44 | Eigen::Matrix3d Utils::pseudo_inverse(const Eigen::Matrix3d &mat) { 45 | Eigen::JacobiSVD svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV); 46 | double epsilon = std::numeric_limits::epsilon(); 47 | // For a non-square matrix 48 | // Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV); 49 | double tolerance = epsilon * std::max(mat.cols(), mat.rows()) * svd.singularValues().array().abs()(0); 50 | return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * 51 | svd.matrixU().adjoint(); 52 | } 53 | 54 | double Utils::cal_dihedral_angle(Eigen::Vector3d surf_coef_1, Eigen::Vector3d surf_coef_2) { 55 | // surface 1: a1 * x + b1 * y + c1 * z + d1 = 0, coef: [a1, b1, c1] 56 | // surface 2: a2 * x + b2 * y + c2 * z + d2 = 0, coef: [a1, b2, c2] 57 | double angle_cos = 58 | abs(surf_coef_1[0] * surf_coef_2[0] + surf_coef_1[1] * surf_coef_2[1] + surf_coef_1[2] * surf_coef_2[2]) 59 | / (sqrt(surf_coef_1[0] * surf_coef_1[0] + surf_coef_1[1] * surf_coef_1[1] + surf_coef_1[2] * surf_coef_1[2]) * 60 | sqrt(surf_coef_2[0] * surf_coef_2[0] + surf_coef_2[1] * surf_coef_2[1] + surf_coef_2[2] * surf_coef_2[2])); 61 | return acos(angle_cos); 62 | } 63 | 64 | Eigen::Vector3d BezierUtils::get_foot_pos_curve(float t, 65 | Eigen::Vector3d foot_pos_start, 66 | Eigen::Vector3d foot_pos_final, 67 | double terrain_pitch_angle = 0) 68 | { 69 | Eigen::Vector3d foot_pos_target; 70 | // X-axis 71 | std::vector bezierX{foot_pos_start(0), 72 | foot_pos_start(0), 73 | foot_pos_final(0), 74 | foot_pos_final(0), 75 | foot_pos_final(0)}; 76 | foot_pos_target(0) = bezier_curve(t, bezierX); 77 | 78 | // Y-axis 79 | std::vector bezierY{foot_pos_start(1), 80 | foot_pos_start(1), 81 | foot_pos_final(1), 82 | foot_pos_final(1), 83 | foot_pos_final(1)}; 84 | foot_pos_target(1) = bezier_curve(t, bezierY); 85 | 86 | // Z-axis 87 | std::vector bezierZ{foot_pos_start(2), 88 | foot_pos_start(2), 89 | foot_pos_final(2), 90 | foot_pos_final(2), 91 | foot_pos_final(2)}; 92 | bezierZ[1] += FOOT_SWING_CLEARANCE1; 93 | bezierZ[2] += FOOT_SWING_CLEARANCE2 + 0.5*sin(terrain_pitch_angle); 94 | foot_pos_target(2) = bezier_curve(t, bezierZ); 95 | 96 | return foot_pos_target; 97 | } 98 | 99 | 100 | double BezierUtils::bezier_curve(double t, const std::vector &P) { 101 | std::vector coefficients{1, 4, 6, 4, 1}; 102 | double y = 0; 103 | for (int i = 0; i <= bezier_degree; i++) { 104 | y += coefficients[i] * std::pow(t, i) * std::pow(1 - t, bezier_degree - i) * P[i]; 105 | } 106 | return y; 107 | } 108 | 109 | 110 | // clip function for a vector 111 | void Utils::clip(Eigen::VectorXd& target, const double& lower, const double& upper) { 112 | for (int i = 0; i < target.size(); i++) { 113 | target[i] = std::clamp(target[i], lower, upper); 114 | } 115 | } 116 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/utils/Utils.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shuoy on 10/19/21. 3 | // 4 | 5 | #ifndef UTILS_H 6 | #define UTILS_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "../Go1Params.hpp" 13 | 14 | class Utils { 15 | public: 16 | // compare to Eigen's default eulerAngles 17 | // this function returns yaw angle within -pi to pi 18 | static Eigen::Vector3d quat_to_euler(Eigen::Quaterniond quat); 19 | static Eigen::Matrix3d skew(Eigen::Vector3d vec); 20 | static Eigen::Matrix3d pseudo_inverse(const Eigen::Matrix3d &mat); 21 | static double cal_dihedral_angle(Eigen::Vector3d surf_coef_1, Eigen::Vector3d surf_coef_2); 22 | 23 | static void clip(Eigen::VectorXd& target, const double& lower, const double& upper); 24 | 25 | }; 26 | 27 | class BezierUtils { 28 | // TODO: allow degree change? may not be necessary, we can stick to degree 4 29 | public: 30 | BezierUtils () { 31 | curve_constructed = false; 32 | bezier_degree = 4; 33 | } 34 | // set of functions create bezier curves, get points, reset 35 | Eigen::Vector3d get_foot_pos_curve(float t, 36 | Eigen::Vector3d foot_pos_start, 37 | Eigen::Vector3d foot_pos_final, 38 | double terrain_pitch_angle); 39 | 40 | void reset_foot_pos_curve() {curve_constructed = false;} 41 | private: 42 | double bezier_curve(double t, const std::vector &P); 43 | 44 | bool curve_constructed; 45 | float bezier_degree; 46 | }; 47 | 48 | #endif //UTILS_H 49 | -------------------------------------------------------------------------------- /src/go1_rl_ctrl_cpp/src/utils/filter.hpp: -------------------------------------------------------------------------------- 1 | 2 | // This filter comes from 3 | // https://github.com/google-research/tiny-differentiable-simulator/blob/master/examples/whole_body_control/com_velocity_estimator.hpp 4 | // hence we must include Apache License 2.0 too 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | // A stable O(1) moving filter for incoming data streams. Implements the 12 | // Neumaier's algorithm to calculate the moving window average, 13 | // which is numerically stable. 14 | class MovingWindowFilter { 15 | public: 16 | 17 | MovingWindowFilter() {} 18 | 19 | MovingWindowFilter(int window_size) : window_size_(window_size) { 20 | assert(window_size_ > 0); 21 | sum_ = 0.0; 22 | correction_ = 0.0; 23 | } 24 | 25 | // Computes the moving window average. 26 | double CalculateAverage(double new_value) { 27 | if (value_deque_.size() < window_size_) { 28 | // pass 29 | } else { 30 | // The left most value needs to be subtracted from the moving sum first. 31 | UpdateNeumaierSum(-value_deque_.front()); 32 | value_deque_.pop_front(); 33 | } 34 | // Add the new value. 35 | UpdateNeumaierSum(new_value); 36 | value_deque_.push_back(new_value); 37 | 38 | return (sum_ + correction_) / double(window_size_); 39 | } 40 | 41 | std::deque GetValueQueue() { 42 | return value_deque_; 43 | } 44 | private: 45 | int window_size_; 46 | double sum_, correction_; 47 | std::deque value_deque_; 48 | 49 | // Updates the moving window sum using Neumaier's algorithm. 50 | // 51 | // For more details please refer to: 52 | // https://en.wikipedia.org/wiki/Kahan_summation_algorithm#Further_enhancements 53 | void UpdateNeumaierSum(double value) { 54 | double new_sum = sum_ + value; 55 | if (std::abs(sum_) >= std::abs(value)) { 56 | // If previous sum is bigger, low-order digits of value are lost. 57 | correction_ += (sum_ - new_sum) + value; 58 | } else { 59 | correction_ += (value - new_sum) + sum_; 60 | } 61 | sum_ = new_sum; 62 | } 63 | }; -------------------------------------------------------------------------------- /src/pytorch_debug/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(libtorch_debug) 3 | 4 | list(APPEND CMAKE_PREFIX_PATH "libtorch/libtorch") 5 | 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}") 7 | 8 | find_package(Torch REQUIRED) 9 | 10 | message(STATUS "PROJECT NAME: ${PROJECT_NAME}") 11 | 12 | add_executable(${PROJECT_NAME}_app main.cpp) 13 | target_link_libraries(${PROJECT_NAME}_app "${TORCH_LIBRARIES}") 14 | set_property(TARGET ${PROJECT_NAME}_app PROPERTY CXX_STANDARD 14) -------------------------------------------------------------------------------- /src/pytorch_debug/actor_architecture_default_1.pth: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/src/pytorch_debug/actor_architecture_default_1.pth -------------------------------------------------------------------------------- /src/pytorch_debug/cpp_model.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/src/pytorch_debug/cpp_model.pt -------------------------------------------------------------------------------- /src/pytorch_debug/main.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by zerenluo on 29.08.22. 3 | // 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | /** 12 | * README: 13 | * In order to debug your Python to C++ conversion (i.e. PyTorch to Torchscript), you need to first have the following: 14 | * - A pre-trained PyTorch neural network model (see module.py for an example of how to load a *.pth file in Python) 15 | * - A build folder in python_debug/ 16 | * - An installation of LibTorch (C++11, No CUDA/CPU) in the directory pytorch_debug/libtorch from https://pytorch.org/ 17 | * 18 | * In order to generate the torchscript file, you need to do the following: 19 | * 1) From the pytorch_debug/ folder, run 'python3 module.py' (no apostrophes). This code will generate the Torchscript file. 20 | * 2) Navigate to your pytorch_debug/build/ folder and run: 21 | * a) cmake .. 22 | * b) make 23 | * to build the libtorch_debug_app executable 24 | * 3) Then, run './libtorch_debug_app' from within the build/ folder in order to inference the network that has been serialized for Torchscript 25 | * 26 | * Note that this code and that in module.py is given only as an example of how to do the Python->C++ conversion of your network. You will need to adjust the code parameters and dimensions, etc. to suit your application. 27 | */ 28 | 29 | int main(int argc, const char* argv[]) { 30 | 31 | // int stateDimension = 1221; // module.py 32 | int stateDimension = 48; // rl_policy_module.py 33 | 34 | torch::manual_seed(0); 35 | 36 | torch::jit::script::Module studentModule; 37 | 38 | // studentModule = torch::jit::load("../student_traced_debug.pt"); // module.py 39 | studentModule = torch::jit::load("../cpp_model.pt"); // rl_policy_module.py 40 | 41 | studentModule.to(torch::kCPU); 42 | studentModule.eval(); 43 | torch::NoGradGuard no_grad_; 44 | 45 | std::vector T; 46 | T.push_back(torch::ones({stateDimension})); 47 | 48 | auto actionTensor = studentModule.forward(T).toTensor(); 49 | 50 | std::cout << actionTensor << std::endl; 51 | 52 | return 0; 53 | 54 | } -------------------------------------------------------------------------------- /src/pytorch_debug/py_model.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/src/pytorch_debug/py_model.pt -------------------------------------------------------------------------------- /src/pytorch_debug/rl_policy_module.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from torch import float32 4 | import torch.nn as nn 5 | from torch.nn.utils import weight_norm # https://pytorch.org/docs/stable/generated/torch.nn.utils.weight_norm.html 6 | 7 | import os 8 | 9 | from rsl_rl.modules import ActorCritic 10 | 11 | device = 'cpu' 12 | 13 | torch.manual_seed(0) 14 | 15 | class DummyClass(): 16 | def __init__(self, device): 17 | self.obs_dim = 48 # observation 18 | self.action_dim = 12 # action 19 | 20 | self.policy_cfg = dict() 21 | self.policy_cfg["init_noise_std"] = 1.0 22 | self.policy_cfg["actor_hidden_dims"] = [512, 256, 128] 23 | self.policy_cfg["critic_hidden_dims"] = [512, 256, 128] 24 | self.policy_cfg["activation"] = 'elu' # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid 25 | 26 | self.actor_critic = ActorCritic(self.obs_dim, 27 | self.obs_dim, 28 | self.action_dim, 29 | **self.policy_cfg).to(device) 30 | 31 | def loadtorchmodel(self): 32 | # load rl policy pytorch model 33 | path = "py_model.pt" 34 | loaded_dict = torch.load(path) 35 | self.actor_critic.load_state_dict(loaded_dict['model_state_dict']) 36 | 37 | # follow the 'get_inference_policy' from rsl_rl on_policy_runner.py 38 | self.actor_critic.eval() # switch to evaluation mode (dropout for example) 39 | self.actor_critic.to(device) 40 | self.actor = self.actor_critic.act_inference 41 | 42 | def run(self): 43 | 44 | np.set_printoptions(precision=4) 45 | 46 | dummy_inputs = torch.ones((1, self.obs_dim)) 47 | 48 | outputs = self.actor(dummy_inputs) 49 | 50 | print(outputs.detach().cpu().data.numpy()) 51 | 52 | def main(): 53 | 54 | dummy = DummyClass(device) 55 | 56 | dummy.loadtorchmodel() 57 | 58 | dummy.run() 59 | 60 | if __name__ == "__main__": 61 | main() 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /src/pytorch_debug/student_traced_debug.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zerenluo123/Go1-RL-Controller/eb0a05908f8e5a812b6f089ea3bce1c48c952541/src/pytorch_debug/student_traced_debug.pt --------------------------------------------------------------------------------