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
- Paper
- final pdf version of the paper
- 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/velocity/acceleration polytope
- force/velocity manipulability ellipsoid
- quick demo jupyter script
- ROS packages and catkin workspace for FRANKA Emika Panda robot capacity calculation
On-line force capability evaluation based on efficient polytope vertex search (submitted for presentation at ICRA 2021) from Auctus on Vimeo.
In the matlab_octave directory you can find two testing scripts intended for benchmarking of vertex search algorithms
panda_test_polytope_algorithmsand
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 efficient 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);The work described in the paper is fully implemented in python in the frame of the pycapacity pip package. This module you can easily integrate in your python project by isntalling it using pip
pip install pycapacityThe pycapacity module implements multiple task-space capacity metrics for robots and human musculoskeletal models. The robot metrics are contained in the pycapacity.robot module.
import pycapacity.robot as capacityrobot.acceleration_ellipsoid: acceleration ellipsoid calculation (dynamic manipulability ellipsoid)robot.acceleration_polytope: Acceleration polytope calculating functionrobot.acceleration_polytope_withfaces: Acceleration polytope calculating functionrobot.force_ellipsoid: force manipulability ellipsoid calculationrobot.force_polytope: Force polytope representing the capacities of the two robots in a certain configurationrobot.force_polytope_intersection: Force polytope representing the intersection of the capacities of the two robots in certain configurations.robot.force_polytope_intersection_withfaces: Force polytope representing the intersection of the capacities of the two robots in certain configurations.robot.force_polytope_sum_withfaces: Force polytope representing the minkowski sum of the capacities of the two robots in certain configurations.robot.force_polytope_withfaces: Force polytope representing the capacities of the two robots in a certain configuration.robot.velocity_ellipsoid: velocity manipulability ellipsoid calculationrobot.velocity_polytope: Velocity polytope calculating functionrobot.velocity_polytope_withfaces: Velocity polytope calculating function, with faces
See demo_notebook.ipynb for one example use case of the module.
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
To run panda robot capacity calculation nodes first clone the repository and submodules:
git clone --recurse-submodules [email protected]:askuric/polytope_vertex_search.gitpanda_capacity package will calculate is an example implementation of the task space capacity calculation for for Franka Emika Panda robot in a form of a catkin ros package.
It uses the library KDL for reading the robot urdf ad it depends on
- hrl-kdl package (https://gitlab.inria.fr/auctus-team/people/antunskuric/ros_nodes/hrl-kdl)
- franka_description package (https://gitlab.inria.fr/auctus/panda/torque-qp-extended/franka_description)
The three packages are shown in the ROS/packages folder. And you can find an example catkin workspace in the ROS/catkin_ws folder.
Then create new catkin workspace:
mkdir ~/capacity_ws && cd ~/capacity_ws/
mkdir src && cd srcThen you can either copy the folders from ROS/packages into the capacity_ws/src folder for example:
cp -r ~/polytope_vertex_search/ROS/packages/* .or clone the packages directly
git clone [email protected]:auctus-team/people/antunskuric/ros_nodes/panda_capacity.git
git clone [email protected]:auctus/panda/torque-qp-extended/franka_description.git
git clone [email protected]:auctus-team/people/antunskuric/ros_nodes/hrl-kdl.gitFinally you can build the workspace
cd ..
catkin_make # catkin buildAnd you should be ready to go!
For visualizing the polytopes in RVIZ you will need to install the jsk-rviz-plugin
sudo apt install ros-*-jsk-rviz-plugins # melodic/kinetic... your ros versionAnd you'll need to install the pip package pycapacity
pip install pycapacity
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.launchTo 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
Open a new terminal and run the command:
rosrun panda_capacity panda_force_polytope_sum.py
Open a new terminal and run the command:
rosrun panda_capacity panda_force_polytope_intersection.py