On-line force capability evaluation based on efficient polytope vertex search

by Antun Skuric, Vincent Padois, David Daney
Published at ICRA2021

New on-line polytope vertex search algorithm optimised for force and velocity polytope evaluation of serial robots.

This repository consists of

Matlab/octave implementation
- full modular algorithm implementation
- benchmarking comparison of three vertex search algorithms for FRANKA Emika Panda and UR5 robot
Python module for calculating robot capacity pycapacity
- force polytope
- force/velocity manipulability ellipsoid
- quick demo jupyter script
ROS node for FRANKA Emika Panda robot

Matlab / Octave testing scripts

In the matlab_octave directory you can find two testing scripts intended for benchmarking of vertex search algorithms

panda_test_polytope_algorithms

and

ur5_test_polytope_algorithms

They are both testing the performance of three vertex search algorithms on Franka Emika Panda robot and UR5 robot:

On-line force capability evaluation based on eﬀicient polytope vertex search
by Antun Skuric, Vincent Padois, David Daney

[f_vert, matrix_inverse_count] = polytope_auctus(Jacobian_mat,tau_min,tau_max);

Evaluation of Force Capabilities for Redundant manipulatiors
by P.Chiacchio, Pierrot et al.

[f_vert, matrix_inverse_count] = polytope_pierrot(Jacobian_mat,tau_min,tau_max);

Vertex search algorithm of convex polyhedron representing upper limb manipulation ability
by Sasaki et al.

[f_vert, matrix_inverse_count] = polytope_sasaki(Jacobian_mat,tau_min,tau_max);

Python capacity module

In this directory you can find the generic robot capacity calculation module called pycapacity which you can easily integrate in your python project, for example

import pycapacity.pycapacity as capacity

Module functions

force_polytope: Force polytope representing the capacities of the two robots in a certain configuration
force_polytope_intersection: Force polytope representing the intersection of the capacities of the two robots in certain configurations.
force_polytope_intersection_withfaces: Force polytope representing the intersection of the capacities of the two robots in certain configurations.
force_polytope_sum_withfaces: Force polytope representing the minkowski sum of the capacities of the two robots in certain configurations.
force_polytope_withfaces: Force polytope representing the capacities of the two robots in a certain configuration.
manipulability_force: force manipulability calculation
manipulability_velocity: velocity manipulability calculation

See demo_notebook.ipynb for one example use case of the module.

ROS panda capacity package

In the ROS_nodes directory you can find the implementation of the python capacity module for a specific use case of Panda robot. The directory consists of two ros packages:

hkl-kdl: a fork of http://wiki.ros.org/hrl-kdl
franka_description: Panda robot definitions from Franka Emika http://wiki.ros.org/franka_description
panda_capacity: the capacity solver for Panda robot

Install the ros packages - using catkin

To run panda robot capacity calculation nodes first clone the repository and submodules:

git clone --recurse-submodules git@gitlab.inria.fr:askuric/polytope_vertex_search.git

Then create new catkin workspace:

mkdir ~/capacity_ws && cd ~/capacity_ws/
mkdir src && cd src

Then you can copy the folders from ROS_nodes into the capacity_ws/src folder for example:

cp -r ~/polytope_vertex_search/ROS_nodes/* .

Finally you can build the workspace

cd ..
catkin_make

And you should be ready to go!

Visualisation dependancies

For visualizing the polytopes in RVZ you will need to install the jsk-rviz-plugin

sudo apt install ros-*-jsk-rviz-plugins # melodic/kinetic... your ros version

One panda simulation

Once when you have everything installed you will be able to run the interactive simulations and see the polytope being visualised in real-time.

To see a simulation with one panda robot and its force and velocity manipulatibility ellipsoid and polytope run the command in the terminal.

source ~/capacity_ws/devel/setup.bash 
roslaunch panda_capacity one_panda.launch

Two panda simulation

To demonstrate the collaborative robotics applications of our algorithm we have provided an interactive simulation of two panda robots where use can visualise their separate and joint force capacities.

For the interactive simulation of two panda robots with their own capacity measures you can simply run the commands:

source ~/capacity_ws/devel/setup.bash 
roslaunch panda_capacity two_panda.launch

Minkowski sum of polytopes

Open a new terminal and run the command:

rosrun panda_capacity panda_force_polytope_sum.py

Intersection of polytopes