The package legged_planner
makes use of the OCS2 repository to transform any high level motion commands(e.g. planners from single integrator, double integrator, rigid body model, etc. or in general high velocity commands) into a quadrupedal motion trajectory.
This package differs from others in the sense that it allows for high fidelity motion planning for quadrupeds (and not simply velocity control) with extreme ease.
The legged_planner
architecture of the algorithm consist of two core components:
- Multi Command Interface: Abstract class that handles logistics in interfacing with varying command interface (e.g. velocity command, target pose, or motion planning trajectories). This increases the ease of utilizing different commands for quadrupeds
- Motion Adapter: A filtering layer that transforms these motion trajectories into trajectories appropriate for a legged robot.
Briefly summarizing the rest of the diagram (which are components in OCS2):
- The finite state machine (FSM), handles generation of the discontinuous dynamics due to contact with the terrain
- Accounting for disontinuous dynamics, NMPC generates locally optimal solution while adhering to dynamic constraints
- These are then tracked through a low-level torque control
This framework relies on the OCS2 repository, so the following dependencies needs to be installed. NOTE: OCS2 is a large repository, and so this repository will only build packages pertaining to legged robots. This package assumes the user is using Linux Distro Ubuntu 20.04 (Focal Fossa) and has set up/installed the ros-distro (ros noetic). If ROS Noetic not installed, see how to install ROS Noetic.
mkdir -p ~/<catkin_ws_name>/src
# Go to src directory
cd ~/<catkin_ws_name>/src
# clone the repository
git clone git@github.com:AndrewZheng-1011/legged_planner.git # # If have not setup ssh key, use the web url: https://github.com/AndrewZheng-1011/legged_planner.git
git clone git@github.com:leggedrobotics/ocs2.git # If have not setup ssh key, use the web url: https://github.com/leggedrobotics/ocs2.git
git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git
git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git
git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git
sudo apt install liburdfdom-dev liboctomap-dev libassimp-dev
# Python dependencies
pip3 install sympy # Symbolic python package
pip3 install numpy # Numpy
# ROS Dependencies for catkin tools
sudo apt-get install ros-$ROS_DISTRO-catkin python3-catkin-tools -y
sudo apt-get install ros-$ROS_DISTRO-grid-map
# Eigen library
sudo apt-get install libeigen3-dev
# Doxygen
sudo apt-get install doxygen doxygen-doc doxygen-gui graphviz
Configure the workspace with debug info
cd ~/<catkin_ws_name>
catkin config -DCMAKE_BUILD_TYPE=RelWithDebInfo
catkin config --extend /opt/ros/noetic # Extend only to default ros
Build ocs2 packages
catkin build ocs2_legged_robot_ros ocs2_self_collision_visualization
Install python dependencies
sh ~/<catkin_ws_name>/src/legged_planner/legged_body_planner/setup_deps.sh
Make python script files executable
chmod+x ~/<catkin_ws_name>/src/legged_planner/legged_body_planner/src/scripts/pub_body_plan_demo.py
chmod+x ~/<catkin_ws_name>/src/legged_planner/legged_body_planner/src/scripts/traj_pub_demo.py
Build the source code of legged_body_planner
and source it
catkin build legged_body_planner legged_body_msgs
source ~/<catkin_ws_name>/devel/setup.bash
- Initialize simulation
roslaunch ocs2_legged_robot_ros legged_robot_sqp.launch
- Run one of the 'walking' gait sequences before running next launch file (e.g. trot)
- Initialize plan node
roslaunch legged_body_planner legged_body_plan.launch
It is very intuitive to run this command interface framework with a generic planning algorithms. Simply publish a plan in the form of a Plan
msg, and the legged body planner node will handle the rest. See demo files in the legged_body_planner/src/scripts/pub_body_plan_demo.py
for a python example. Then change the plan node in the legged_body_plan.launch
file.
Example of the fidelity of planning that can be controlled can be seen as well.
By utilizing an abstract class for motion adaptation (e.g. /legged_body_planner/include/legged_body_planner/motion_adapters/MotionAdapter.h
), it is simple to then integrate the your own motion command interface. See the source files for examples.
- Change code s.t. modularize reliance on OCS2
- Add architectural change s.t. compatible with either a server or subcriber/publisher framework
- Add asynchronous threading s.t. different motion command interfaces can operate at different frequencies
- Update README.md to add what the ROS input and outputs are for easier usage