This repo contains the code used to generate the trajectories for large step-ups. It consists of a library which exploits CasADi
to solve an optimal control problem, plus a ROS2
node to communicate with the robot.
This code is related to the paper "Non-Linear Trajectory Optimization for Large Step-Ups: Application to the Humanoid Robot Atlas". Paper: https://ieeexplore.ieee.org/document/9341587 Arxiv: https://arxiv.org/abs/2004.12083
To cite this work, please add the following to your publication
S. Dafarra, S. Bertrand, R. J. Griffin, G. Metta, D. Pucci and J. Pratt, "Non-Linear Trajectory Optimization for Large Step-Ups: Application to the Humanoid Robot Atlas," 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Las Vegas, NV, USA, 2020, pp. 3884-3891, doi: 10.1109/IROS45743.2020.9341587.
Bibtex:
@INPROCEEDINGS{9341587, author={S. {Dafarra} and S. {Bertrand} and R. J. {Griffin} and G. {Metta} and D. {Pucci} and J. {Pratt}}, booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, title={Non-Linear Trajectory Optimization for Large Step-Ups: Application to the Humanoid Robot Atlas}, year={2020}, volume={}, number={}, pages={3884-3891}, doi={10.1109/IROS45743.2020.9341587}}
If the CMake option BUILD_INTERFACE
is ON (it is ON by default), you also need
ihmc-open-robotics-software
ROS2
(Tested with Crystal Clemmys)
The following instructions have been tested on Ubuntu 16.04. It is also assumed that no other ROS2
installation is already present on the system.
Run the following command to install gcc
, cmake
and Ipopt
from apt
.
sudo apt install gcc g++ gfortran git cmake liblapack-dev pkg-config coinor-libipopt-dev --install-recommends
(These instructions have been adapted from https://github.com/casadi/casadi/wiki/InstallationLinux#building-casadi-from-sources)
- Move to the directory where you want to download
CasADi
, e.g.~/dev/large_step_ups
.
cd ~/dev/large_step_ups
- Clone the repository
git clone https://github.com/casadi/casadi.git
- Install
CasADi
. In order to avoid polluting system directories, it is suggested to specify an installation directory different from/usr/local
. In this case we create aninstall
folder inside thebuild
folder.
mkdir build
cd build
mkdir install
export NEW_INSTALL_DIR=$(pwd)/install
cmake -DWITH_IPOPT=ON -DCMAKE_INSTALL_PREFIX=$NEW_INSTALL_DIR ..
make install -j4
- Modify the
.bashrc
exporting thecasadi_DIR
variable. In this wayCasADi
can be easily found bycmake
projects. Add the following line.
casadi_DIR=/path/to/casadi/install
In our case
casadi_DIR=~/dev/large_step_ups/casadi/build/install
It is then necessary to source the modified .bashrc
in order to apply the changes.
(These instructions have been adapted from https://index.ros.org/doc/ros2/Installation/Crystal/Linux-Development-Setup/)
- Install
ROS2
system dependencies
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
sudo apt update && sudo apt install -y \
build-essential \
cmake \
git \
python3-colcon-common-extensions \
python3-lark-parser \
python3-pip \
python-rosdep \
python3-vcstool \
wget
python3 -m pip install -U \
argcomplete \
flake8 \
flake8-blind-except \
flake8-builtins \
flake8-class-newline \
flake8-comprehensions \
flake8-deprecated \
flake8-docstrings \
flake8-import-order \
flake8-quotes \
pytest-repeat \
pytest-rerunfailures \
pytest \
pytest-cov \
pytest-runner \
setuptools
sudo apt install --no-install-recommends -y \
libasio-dev \
libtinyxml2-dev
- Create a
ROS2
workspace and retrieve the code.
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
wget https://raw.githubusercontent.com/ros2/ros2/crystal/ros2.repos
vcs import src < ros2.repos
- Install
ROS2
dependencies viarosdep
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro crystal -y --skip-keys "console_bridge fastcdr fastrtps libopensplice67 libopensplice69 python3-lark-parser rti-connext-dds-5.3.1 urdfdom_headers"
python3 -m pip install -U lark-parser
- Compile
ROS2
cd ~/ros2_ws
colcon build --symlink-install --packages-ignore qt_gui_cpp rqt_gui_cpp
Here we assume that the following messages are available in the ihmc-open-robotics-software/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg
folder:
StepUpPlannerControlElement.msg
StepUpPlannerCostWeights.msg
StepUpPlannerErrorMessage.msg
StepUpPlannerParametersMessage.msg
StepUpPlannerPhase.msg
StepUpPlannerPhaseParameters.msg
StepUpPlannerPhaseResult.msg
StepUpPlannerRequestMessage.msg
StepUpPlannerRespondMessage.msg
StepUpPlannerStepParameters.msg
StepUpPlannerVector2.msg
If not, you may checkout the branch feature/largeStepUps
of ihmc-open-robotics-software
. Now we assume the ihmc-open-robotics-software
to be in the path ~/dev/atlas/ihmc-open-robotics-software/
.
- Create a
controller_msgs
workspace
mkdir -p ~/dev/large_step_ups/controller_msgs_ws/src
- Create a symbolic link to the
controller_msgs
folder
ln -s ~/dev/atlas/ihmc-open-robotics-software/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/ ~/dev/large_step_ups/controller_msgs_ws/src/controller_msgs
Alternatively, it is possible to exploit the ihmc_interfaces
repo. In this case it would be enough to clone the repo in the ~/dev/large_step_ups/controller_msgs_ws/src
folder (make sure that the messages listed above are available).
- Compile the message workspace
cd ~/dev/large_step_ups/controller_msgs_ws
source ~/ros2_ws/install/local_setup.sh
colcon build --symlink-install
- Clone this repo
cd ~/dev/large_step_ups
git clone https://bitbucket.ihmc.us/scm/icsl/large-step-ups-planner.git
- Source the ROS setup files to make sure that all the enviromental variables are set.
source ~/ros2_ws/install/local_setup.sh
source ~/dev/large_step_ups/controller_msgs_ws/install/local_setup.sh
- Compile the code
cd large-step-ups-planner/
mkdir build
cmake ../
make
This node waits for a StepUpPlannerParametersMessage
and a StepUpPlannerRequestMessage
. The first is used to setup the planner. It returns a StepUpPlannerErrorMessage
as an acknowledgement. Depending on the error code, the parameters may be have been successfully set or refused. Some info are provided in the message about what is wrong.
After the parameters are set, it is possible to send requests. The node will respond with a StepUpPlannerRespondMessage
containing the solution.
In order to run, the node needs all the ROS2 variables set up. To this end, it is suggested to add the following alias to the .bashrc
alias LARGE_STEP_UPS_SETUP='export ROS_DOMAIN_ID=8 && source ~/ros2_ws/install/local_setup.sh && source ~/dev/large_step_ups/controller_msgs_ws/install/local_setup.sh && export PATH=$PATH:~/dev/large_step_ups/large-step-ups-planner/build/bin'
In this way, it is possible to run the node with the following commands:
LARGE_STEP_UPS_SETUP
step_up_planner_responder
The output should look like
[INFO] [StepUpPlannerResponder]: Running...
It is necessary to send a StepUpPlannerParametersMessage
the first time a trajectory has to be computed or in case the following values need to change:
- number of phases
- phase settings
- phase type
- foot vertexes
- foot scale
- center offset
- number of instants per phase
- solver verbosity
- max leg length parameter
- min leg length parameter
Ipopt
linear solver- portion of the final phase used to weight the final error
- static friction coefficient
- torsional friction coefficient
- cost weights
- whether to include or not the
CenterOfMassTrajectoryMessage
, thePelvisHeightTrajectoryMessage
and/or theFootstepDataListMessage
into the solution message. - whether or not to send directly to the walking controller he
CenterOfMassTrajectoryMessage
, thePelvisHeightTrajectoryMessage
and/or theFootstepDataListMessage
, and their respective topic name. - delta to be used when creating a
PelvisHeightTrajectoryMessage
from the CoM height profile. - number of data points per message.
In particular, when sending or including the CenterOfMassTrajectoryMessage
and the PelvisHeightTrajectoryMessage
a different message per phase is created. Multiple messages per phase are created if the number of data points per message is lower than the number of instants per phase.
If a StepUpPlannerParametersMessage
has already been sent and successfully set, and none of the above parameters need to change, then it is not necessary to send such message again.
Is the message to be sent every time a new trajectory has to be computed.
Tests on the robot have been performed by launching the node as specified above and by launching the AtlasStepUpPlannerDemo
defined in ihmc-open-robotics-software
.