This repository includes nodes for generating and visualizing Cartesian Available : (1) Velocity Polytope (2) Force Polytope (3) Desired Polytope (4) Performance Index - Capacity Margin in Rviz.
This repository was tested on Universal Robot UR5. The robot is controllled through an external PC with ROS Noetic and Ubuntu 20.04 LTS.
- External PC. Our specifications are;
Intel® Core™ i7-10700K CPU @ 3.80GHz × 16
Intel® Core™ i9-10700K CPU @ 3.80GHz × 16 (Interactive Inverse Kinematics for the Serial Arm- video for the paper)
RAM: 16 GB
Graphics: NVIDIA Corporation GP106GL [Quadro P2200]
- Ubuntu 20.04 LTS
- ROS Noetic If you are new to ROS, go here to learn how to create a catkin workspace.
- Polytope. This is the source repository for polytope, a toolbox for geometric operations on polytopes in any dimension. Polytope.
- pykdl_utils Higher Level Python wrapper for PyKDL in ROS for Kinematic Solver
- Python native libraries Scipy, Numpy
- Polygon ROS geometry messages for plotting in Rviz jsk-ros-pkg
- Polytope ros message publisher forked and modified from Pycapacity Library capacity_visual_utils
In a Terminal
$ cd ~/catkin_ws/src/
$ git clone https://gitlab.com/KeerthiSagarSN/rospygradientpolytope
In a terminal
$ pip install sympy
$ pip install polytope
Clone Sawyer Robot- Different from the Original Repo due to the offset in the tool center point to avoid Null in Jacobian
$ git clone https://github.com/KeerthiSagarSN/sawyer_robot
Clone UR5 Robot Repository - Different from the Original Repo due to the offset in the tool center point to avoid Null in Jacobian
$ git clone https://github.com/KeerthiSagarSN/universal_robot
$ git clone https://github.com/orocos/orocos_kinematics_dynamics.git
$ git clone https://github.com/KeerthiSagarSN/hrl-kdl.git
$ git checkout Noetic-devel
$ sudo apt-get install ros-noetic-urdf-parser-plugin
$ sudo apt-get install ros-noetic-urdfdom-py
$ git clone https://github.com/jsk-ros-pkg/jsk_visualization.git
$ cd ..
$ catkin build
$ source devel/setup.bash
IF you encounter CMAKE Error : Install all ROS- Dependencies - (Only when CMAKE Error !) Replace underscores "_" with "-" in the package name while typing in the MISSING-PACKAGE below
$ sudo apt install ros-noetic-MISSING-PACKAGE
$ sudo apt-get install ros-$ROS_DISTRO-robot-state-publisher ros-$ROS_DISTRO-joint-state-publisher
$ sudo apt-get install ros-$ROS_DISTRO-joint-state-publisher-gui
$ roslaunch ur_description view_ur5.launch
$ roslaunch sawyer_description test_sawyer_description.launch
$ roslaunch rospygradientpolytope launch_robot_ur.launch
$ roslaunch rospygradientpolytope launch_robot_sawyer.launch
$ git clone https://gitlab.com/KeerthiSagarSN/inverse_kinematics_interactive_rviz.git
$ cd ..
$ catkin build
$ source devel/setup.bash
$ roslaunch inverse_kinematics_interactive_rviz inverse_kinematics_interactive_rviz.launch
- Ensure that a link "world" exists as a base frame in your urdf file.
- Click and drag the interactive sphere with axes in the Rviz window
- Select Run IK button from the Interactive panel
- Click on Polytope:ON for visualizing the estimated capacity margin during optimization
- Polytope:Off To stop updating the polytope (Faster execution option)
- Click on Reset Button : After optimization termination for consecutive IK optimization
Interactive panel is in experimental phase and may lead to occasional crash. Please restart the node, when GUI crashes
from rospygradientpolytope.visual_polytope import *
rospygradientpolytope.visual_polytope.desired_polytope
cartesian_desired_vertices - Desired velocities at the end-effector
polytope_vertices - Convex hull of the desired cartesian vertices
polytope_faces - Simplices of the convex hull mapped to polytope facets
Compute polytope facets and vertices of available/feasible polytope, with the closest (vertex,facet) pair
rospygradientpolytope.visual_polytope.velocity_polytope_with_estimation
[polytope_vertices, polytope_faces, facet_pair_idx, capacity_margin_faces, capacity_proj_vertex, polytope_vertices_est, polytope_faces_est, capacity_margin_faces_est, capacity_proj_vertex_est,Gamma_min_softmax] = velocity_polytope_with_estimation(JE, qdot_max, qdot_min,cartesian_desired_vertices, sigmoid_slope)
JE - Jacobian of the serial manipulator using PyKDL
qdot_max - maximum joint velocity vector- minimum joint velocity vector
qdot_min - minimum joint velocity vector
cartesian_desired_vertices - Desired velocities at the end-effector
sigmoid_slope - Slope parameter for sigmoid activation
polytope_vertices - Convex hull of the available cartesian velocities
polytope_faces - Facets of the available cartesian velocities
facet_pair_idx - Index of the closest (vertex,facet) pair
capacity_margin_faces - Closest facet in the available cartesian velocities contributing to classical capacity margin
capacity_proj_vertex - Closest vertex in the desired cartesian velocities contributing to classical capacity margin
polytope_vertices_est - Convex hull of the available cartesian velocities with estimated hyperplane parameters
polytope_faces_est - Facets of the available cartesian velocities with estimated hyperplane parameters
capacity_margin_faces_est - Closest facet in the available cartesian velocities contributing to estimated capacity margin
capacity_proj_vertex_est - Closest vertex in the desired cartesian velocities contributing to estimated capacity margin
Gamma_min_softmax - Estimated capacity margin (Performance measure)
from rospygradientpolytope.polytope_functions import *
rospygradientpolytope.polytope_functions.get_polytope_hyperplane
[h_plus,h_plus_hat,h_minus,h_minus_hat,p_plus,p_minus,p_plus_hat,p_minus_hat,n_k, Nmatrix, Nnot] = get_polytope_hyperplane(JE,active_joints,cartesian_dof_input,qdot_min,qdot_max,cartesian_desired_vertices,sigmoid_slope)
JE - Jacobian of the serial manipulator using PyKDL
active_joints - Number of joints in the serial manipulator (Eg, for Sawyer,active_joints = 7)
cartesian_dof_input - Cartesian DOF (Eg, Cartesian DOF = 3), if only translational velocities are considered
qdot_min - minimum joint velocity vector
qdot_max - maximum joint velocity vector- minimum joint velocity vector
cartesian_desired_vertices - Desired velocities at the end-effector
sigmoid_slope - Slope parameter for sigmoid activation
h_plus - Maximum distance vector contributing to defining available velocities hyperplane
h_plus_hat - Maximum distance vector contributing to defining available velocities hyperplane (using proposed method)
h_minus - Minimum distance vector contributing to defining available velocities hyperplane
h_minus_hat - Minimum distance vector contributing to defining available velocities hyperplane (using proposed method)
p_plus - Points on the hyperplane defined by h_plus
p_minus - Points on the hyperplane defined by h_minus
p_plus_hat - Points on the hyperplane defined by h_plus_hat
p_minus_hat - Points on the hyperplane defined by h_minus_hat
n_k - Normal vector corresponding to the cross product between two joint twist
Nmatrix - Combinations of twists to define n_k
Nnot - Combinations of twists to define h_plus, h_minus using Nmatrix and n_k
rospygradientpolytope.polytope_functions.get_capacity_margin
[Gamma_minus, Gamma_plus, Gamma_total_hat, Gamma_min, Gamma_min_softmax, Gamma_min_index_hat, facet_pair_idx, hyper_plane_sign] = get_capacity_margin(JE,n_k,h_plus,h_plus_hat,h_minus,h_minus_hat,active_joints,cartesian_dof_input,qdot_min,qdot_max,cartesian_desired_vertices,sigmoid_slope)
JE - Jacobian of the serial manipulator using PyKDL
n_k - Normal vector corresponding to the cross product between two joint twist
h_plus - Maximum distance vector contributing to defining available velocities hyperplane
h_plus_hat - Maximum distance vector contributing to defining available velocities hyperplane (using proposed method)
h_minus - Minimum distance vector contributing to defining available velocities hyperplane
h_minus_hat - Minimum distance vector contributing to defining available velocities hyperplane (using proposed method)
active_joints - Number of joints in the serial manipulator (Eg, for Sawyer,active_joints = 7)
cartesian_dof_input - Cartesian DOF (Eg, Cartesian DOF = 3), if only translational velocities are considered
qdot_min - minimum joint velocity vector
qdot_max - maximum joint velocity vector- minimum joint velocity vector
cartesian_desired_vertices - Desired velocities at the end-effector
sigmoid_slope - Slope parameter for sigmoid activation
Gamma_minus - Distance vector contributing to capacity margin using points p_minus on the hyperplane defined by h_minus
Gamma_plus - Distance vector contributing to capacity margin using points p_plus on the hyperplane defined by h_plus
Gamma_total_hat - Total distance vector contributing to estimated capacity margin
Gamma_min - Classical capacity margin
Gamma_min_softmax - Estimated capacity margin (proposed performance measure)
Gamma_min_index_hat - Index of Gamma_total_hat with the minimum value
facet_pair_idx - Index of the (vertex,facet) pair to plot in rviz
hyper_plane_sign - To identify the hyperplane closer to the vertex to plot in rviz
For computing analytical gradient of hyperplane parameters and estimated capacity margin for serial robot
from rospygradientpolytope.polytope_gradient_functions import *
rospygradientpolytope.polytope_gradient_functions.hyperplane_gradient
[d_h_plus_dq, d_h_minus_dq, dn_dq] = hyperplane_gradient(JE,H,n_k,Nmatrix, Nnot,h_plus_hat,h_minus_hat,p_plus_hat,p_minus_hat,qdot_min,qdot_max,test_joint,sigmoid_slope)
JE - Jacobian of the serial manipulator using PyKDL
H - Hessian of the serial manipulator
n_k - Normal vector corresponding to the cross product between two joint twist
Nmatrix - Combinations of twists to define n_k
Nnot - Combinations of twists to define h_plus, h_minus using Nmatrix and n_k
h_plus_hat - Maximum distance vector contributing to defining available velocities hyperplane (using proposed method)
h_minus_hat - Minimum distance vector contributing to defining available velocities hyperplane (using proposed method)
p_plus_hat - Points on the hyperplane defined by h_plus_hat
p_minus_hat - Points on the hyperplane defined by h_minus_hat
qdot_min - minimum joint velocity vector
qdot_max - maximum joint velocity vector- minimum joint velocity vector
test_joint - Gradient with respect to the corresponding joint
sigmoid_slope - Slope parameter for sigmoid activation
d_h_plus_dq - Gradient of the h_plus_hat vector with respect to the corresponding joint q
d_h_minus_dq - Gradient of the h_minus_hat vector with respect to the corresponding joint q
dn_dq - Gradient of n_k vector with respect to the corresponding joint q
Using multi-processing toolbox with global variable jac_output- representing the analytical gradient of the estimated capacity margin with respect to all active joints in the serial manipulator
rospygradientpolytope.polytope_gradient_functions.Gamma_hat_gradient_dq
[jac_output] = Gamma_hat_gradient_dq(JE,H,n_k,Nmatrix, Nnot,h_plus_hat,h_minus_hat,p_plus_hat,\
p_minus_hat, Gamma_total_hat, Gamma_min_index_hat,\
qdot_min,qdot_max,cartesian_desired_vertices,sigmoid_slope,test_joint,jac_output)
JE - Jacobian of the serial manipulator using PyKDL
H - Hessian of the serial manipulator
n_k - Normal vector corresponding to the cross product between two joint twist
Nmatrix - Combinations of twists to define n_k
Nnot - Combinations of twists to define h_plus, h_minus using Nmatrix and n_k
h_plus_hat - Maximum distance vector contributing to defining available velocities hyperplane (using proposed method)
h_minus_hat - Minimum distance vector contributing to defining available velocities hyperplane (using proposed method)
p_plus_hat - Points on the hyperplane defined by h_plus_hat
p_minus_hat - Points on the hyperplane defined by h_minus_hat
Gamma_total_hat - Total distance vector contributing to estimated capacity margin
qdot_min - minimum joint velocity vector
qdot_max - maximum joint velocity vector- minimum joint velocity vector
cartesian_desired_vertices - Desired velocities at the end-effector
sigmoid_slope - Slope parameter for sigmoid activation
test_joint - Gradient with respect to the corresponding joint
jac_output - Global multiprocessing array for storing analytical gradient of the estimated capacity margin
jac_output- Analytical gradient of the estimated capacity margin with respect to the corresponding test joint test_joint
from rospygradientpolytope.polytope_ros_message import *
Fix all joints and move only one-joint "test_joint" and visualize simultaneously numerical gradient and analytical gradient In launch_robot_sawyer.py --> uncomment self.test_gamma_gradient(sigmoid_slope_test=sigmoid_slope) in init
$ roslaunch rospygradientpolytope launch_robot_sawyer.launch
Fix all joints and move only one-joint "test_joint" and visualize simultaneously numerical gradient and analytical gradient In launch_robot_ur.py --> uncomment self.test_gamma_gradient(sigmoid_slope_test=sigmoid_slope) in init
$ roslaunch rospygradientpolytope launch_robot_ur.launch
Generate Wrench Feasible Workspace (WFW) by identifying where Capacity margin index = 0
$ python3 CDPR_workspace.py
$ python3 CDPR_workspace.py
Plotting with matplotlib is slow in the current implementation, optimized code to plot is under development
$ python3 test_CDPR_Optimization.py
If you found this code useful, we would be grateful if you cite the following reference:
[1] K. Sagar, S. Caro, T. Padır and P. Long, "Polytope-Based Continuous Scalar Performance Measure With Analytical Gradient for Effective Robot Manipulation," in IEEE Robotics and Automation Letters, vol. 8, no. 11, pp. 7289-7296, Nov. 2023, doi: 10.1109/LRA.2023.3313926.
Bibtex:
@ARTICLE{10246372,
author={Sagar, Keerthi and Caro, Stéphane and Padır, Taşkın and Long, Philip},
journal={IEEE Robotics and Automation Letters},
title={Polytope-Based Continuous Scalar Performance Measure With Analytical Gradient for Effective Robot Manipulation},
year={2023},
volume={8},
number={11},
pages={7289-7296},
doi={10.1109/LRA.2023.3313926}}
You can find the video accompanying for the article here
Youtube link for the video can be found here