icub-ds-walking: A C++ repository from LASA Laboratory, EPFL

icub-ds-walking

DS-based motion planning for the iCub using the reactive omnidirectional walking controller proposed in [1] guided by a DS learned from demonstrations with the 1) LPV-DS approach [2] and the 2) LAGS-DS approach [3]. The entire motion planning architecure is illustrated in the following figure:

The desired linear velocity of the Center-of-Mass (CoM) of the biped is computed by a DS of the form . In it's current form the angular velocity is defined with the following equation: , where:

: Current Rotation matrix of the robot's CoM in world reference frame
: Desired Rotation matrix of the robot's CoM in world reference frame, computed by aligning with the direction of motion given by the DS
: The skew-symmetric matrix representing the angular velocity vector

TODO: Learn orientation dynamics from demonstration as well.

System Requirements

Ubuntu 16.04, Gazebo7, ROS-Kinetic, YARP

Installation

To avoid any issues, we recommend to start with a clean Ubuntu 16.04 installation.

Install biped-walking-controller package.
- Checkout/clone the nadia-DS branch!
```
   $ git clone -b nadia-DS https://github.com/epfl-lasa/biped-walking-controller.git
```
- Install all dependencies Gazebo7/YARP/iCub/gazebo-yarp-plugin etc (follow README of biped-walking-controller):
  Easiest option (on clean installation):
  - Install latest version of Eigen3: Eigen 3 version >=3.2.9
  - Install Gazebo7 and libgazebo7: installation instructions
  - Install all yarp/iCub/gazebo-plugin libraries with robotology-superbuild
  - Install the Gazebo Plugin GetLinkWorldPose
- Run the tests with different walking commands to check that everything is working properly.
Install ros-kinetic-desktop which includes ROS, rqt, rviz, and robot-generic libraries only.

In your catkin src directory clone the repository

   $ git clone https://github.com/epfl-lasa/icub-ds-walking

wstool gets all other git repository dependencies, after the following steps you should see extra catkin packages in your src directory.

   $  wstool init
   $  wstool merge icub-ds-walking/icub-ds-motion/dependencies.rosinstall && wstool up 
   $  wstool merge ds_motion_generator/dependencies.rosinstall && wstool up

Query and installs all libraries and packages

   $ rosdep install --from-paths . --ignore-src --rosdistro kinetic

Compile all ros-packages

   $ roscd && cd .. && catkin_make

Usage

Terminal 1 Start yarpserver:
```
   $ yarpserver
```
Terminal 2 (simulation) start gazebo simulator and import include the robot model (iCub (no hands))
```
   $ gazebo 
```
Terminal 3 Launch roscore (on iCub-YARP PC)
```
   $ roscore
```
Terminal 4 Load the ROS-to-Yarp Streamer (should be in same PC as yarp-controller):
```
   $ roslaunch ros2yarp_data_streamer ros2yarp_DS_streamer.launch
```

Terminal 5 Run the walking controller as follows:

   $ ./BipedWalkingGrasping_ROS --from ../config/BalanceWalkingController_ROS.ini

   $ ./BipedWalkingGrasping_LPVDS --from ../config/BalanceWalkingController_LPVDS.ini

Terminal 6 Once the ports are open, run the yarp2ros publisher:
```
   $ rosrun yarp2ros_data_publisher yarp2ros_CoM_node --robot icubSim
```
- Name of the robot should be the same as the one defined in ~/biped-walking-controller/config/BalanceWalkingController_ROS.ini

Terminal 7 Load the DS that you want the robot's CoM motion to follow (this could be in another PC):

   $ roslaunch icub-ds-motion icubDS_visualization.launch

To define which DS you want to load you can modify the launch file:

# Load DS Motion Generator
 <include file="$(find icub-ds-motion)/launch/load_lpvDS_motionGenerator.launch">
 	<arg name="DS_name" value="iCub-Line-Loco"/>
 </include>
 # Example Options:
 # - iCub-Line-Loco
 # - iCub-Linear1-Loco
     # - iCub-Linear2-Loco
 # - iCub-Cshape-Loco

These are the names of the .yml files that should be stored in this folder: ~/ds_motion_generator/config/learned_DS/lpvDS/, containing all of the DS parameters.

Testing different walking commands

We currently have 2 different ways of generating desired CoM velocity (v_x, v_y, w_z). These types and their parameters can be defined in the config file: BalanceWalkingController_ROS.ini like so,

# DS Type 0: Simple linear DS, 1: Using a non-linear DS from ROS
DSType		0

Desired Velocity will be generated via a simple linear DS: DSType 0
The implemented DS is of the form whose parameters can be defined as follows:

  # Desired Target with linear DS x [m], y [m], z [m] 
  kappa      0.2
  AttractorX 2.00
  AttractorY -1.00
  AttractorZ 0.541591

where:

: CoM position
: Attractor (target)
: DS gain

Desired Velocity will be generated via a non-linear DS learned from demonstrations: DSType 1. The paramaters of this DS should be defined in.. launch file

References

[1] Bombile, M. and Billard, A. (2017) Capture-Point based Balance and Reactive Omnidirectional Walking Controller. In proceedings of the IEEE-RAS Conference on Humanoid Robots (Humanoids), Birmigham (UK), Nov 15-17, 2017
[2] Figueroa, N. and Billard, A. (2018) A Physically-Consistent Bayesian Non-Parametric Mixture Model for Dynamical System Learning. In Proceedings of the 2nd Conference on Robot Learning (CoRL).
[3] Figueroa, N and Billard, A. (2019) "Locally Active Globally Stable Dynamical Systems: Theory, Learning and Experiments". In Preparation.

Contact: Nadia Figueroa (nadia.figueroafernandez AT epfl dot ch)

Acknowledgments This work was supported by the European Community Horizon 2020 Research and Innovation pro- gramme, grant agreement 644727-Cogimon.