Package: pycapacity
An efficient task-space capacity calculation package for robotics and biomechanics📢 New version of the pycapacity package is out - version v2.1! -see full changelogAboutWhat ispycapacity?Python packagepycapacityprovides a set of tools for evaluating task space physical ability metrics for humans and robots, based on polytopes and ellipsoids.
The aim ofpycapacityis to provide a set of efficient tools for their evaluation in an easy to use framework that can be easily integrated with standard robotics
and biomechanics libraries. The package implements several state of the art algorithms for polytope evaluation that bring many of the
polytope metrics to the few milliseconds evaluation time, making it possible to use them in online and interactive applications.The package can be easily interfaced with standard libraries for robotic manipulator rigid body simulation such asrobotic-toolboxorpinocchio, as well as human musculoskeletal model biomechanics
softwaresopensimandbiorbd. The package can also be used with the Robot Operating System (ROS).The package additionally implements a set of visualization tools for polytopes and ellipsoids based on the
Python packagematplotlibintended for fast prototyping and quick and interactive visualization.Seefull API documentation and docs.Robotic manipulator capacity metricsFor the robotic manipulators the package integrates several velocity, force and acceleration capacity calculation functions based on ellipsoids:Velocity (manipulability) ellipsoidE_vel = {dx | dx = J.dq, ||dq||<1 }Acceleration (dynamic manipulability) ellipsoidE_acc = {ddx | ddx = J.M^(-1).t, ||t||<1 }Force ellipsoidE_for = {f | J^T.f = t, ||t||<1 }And polytopes:Velocity polytopeP_vel = {dx | dx = J.dq,  dq_min < dq < dq_max}Acceleration polytopeP_acc = {ddx | ddx = J.M^(-1).t, t_min < t < t_max}Force polytopeP_for = {f | J^T.f = t, t_min < t < t_max}NEW📢: Reachable space of the robot with the horizonTP_x = {x | x = JM(-1)tT^2/2, t_min < t < t_max,   dq_min < M^(-1)tT < dq_max,   q_min < M^(-1)tT^2/2 < q_max}Force polytopesMinkowski sum and intersectionWhereJis the robot jacobian matrix,fis the vector of cartesian forces,dxandddxare vectors fo cartesian velocities and accretions,dqis the vector of the joint velocities andtis the vector of joint torques.Reachable space polytope approximation is based on this paper:Approximating robot reachable space using convex polytopesby Skuric, Antun, Vincent Padois, and David Daney.In: Human-Friendly Robotics 2022: HFR: 15th International Workshop on Human-Friendly Robotics. Cham: Springer International Publishing, 2023.The force polytope functions have been implemented according to the paper:On-line force capability evaluation based on efficient polytope vertex searchby A.Skuric, V.Padois and D.DaneyPublished on ICRA2021The force polytope functions have been implemented according to the paper:On-line force capability evaluation based on efficient polytope vertex searchby A.Skuric, V.Padois and D.DaneyPublished on ICRA2021And the velocity and acceleration polytopes are resolved using theHyper-plane shifting method:Characterization of Parallel Manipulator Available Wrench Set Facetsby Gouttefarde M., Krut S.In: Lenarcic J., Stanisic M. (eds) Advances in Robot Kinematics: Motion in Man and Machine. Springer, Dordrecht (2010)Human musculoskeletal models capacity metricsFor the human musculoskeletal models this package implements the ellipsoid and polytope evaluation functions. The implemented ellipsoids are:Velocity (manipulability) ellipsoidE_vel = {dx | dx = J.dq, dl = L.dq, ||dl||<1 }Acceleration (dynamic manipulability) ellipsoidE_acc = {ddx | ddx = J.M^(-1).N.F, ||F||<1 }Force ellipsoidE_for = {f | J^T.f = N.F, ||F||<1 }And polytopes:Velocity polytopeP_vel = {dx | dx = J.dq, dl = L.dq  dl_min < dl < dl_max}Acceleration polytopeP_acc = {ddx | ddx = J.M^(-1).N.F, F_min < F < F_max}Force polytopeP_for = {f | J^T.f = N.F, F_min < F < F_max}WhereJis the model's jacobian matrix,Lsi the muscle length jacobian matrix,N= -L^Tis the moment arm matrix,fis the vector of cartesian forces,dxandddxare vectors fo cartesian velocities and accretions,dqis the vector of the joint velocities,tis the vector of joint torques,dlis the vector of the muscle stretching velocities andFis the vector of muscular forces.The force and velocity polytope functions have been implemented according to the paper:On-line feasible wrench polytope evaluation based on human musculoskeletal models: an iterative convex hull methodby A.Skuric, V.Padois, N.Rezzoug and D.DaneySubmitted to RAL & ICRA2022And the acceleration polytopes are resolved using theHyper-plane shifting method:Characterization of Parallel Manipulator Available Wrench Set Facetsby Gouttefarde M., Krut S.In: Lenarcic J., Stanisic M. (eds) Advances in Robot Kinematics: Motion in Man and Machine. Springer, Dordrecht (2010)Polytope evaluation algorithmsThere are three methods implemented in this paper to resolve all the polytope calculations:Hyper-plane shifting method (HPSM)Iterative convex hull method (ICHM)Vertex enumeration algorithm (VEPOLI2)All of the methods are implemented in the modulepycapacity.algorithmsand can be used as standalone functions.  See indocs for more info.Hyper-plane shifting method (HPSM)Characterization of Parallel Manipulator Available Wrench Set Facetsby Gouttefarde M., Krut S.In: Lenarcic J., Stanisic M. (eds) Advances in Robot Kinematics: Motion in Man and Machine. Springer, Dordrecht (2010)This method finds the half-space representation of the polytope of a class:P = {x | x = By, y_min <= y <= y_max }To find the vertices of the polytope after finding the half-space representationHx <= dan convex-hull algorithm is used.The method is a part of thepycapacity.algorithmsmodulehyper_plane_shift_method, See indocs for more info.Iterative convex-hull method (ICHM)On-line feasible wrench polytope evaluation based on human musculoskeletal models: an iterative convex hull methodby A.Skuric, V.Padois, N.Rezzoug and D.DaneySubmitted to RAL & ICRA2022This method finds both vertex and half-space representation of the class of polytopes:P = {x | Ax = By, y_min <= y <= y_max }And it can be additionally extended to the case where there is an additional projection matrixPmaking a class of problems:P = {x | x= Pz, Az = By, y_min <= y <= y_max }The method is a part of thepycapacity.algorithmsmoduleiterative_convex_hull_method. See thedocs for more infoVertex enumeration algorithm (VEPOLI2)On-line force capability evaluation based on efficient polytope vertex searchby A.Skuric, V.Padois and D.DaneyPublished on ICRA2021This method finds vertex representation of the class of polytopes:P = {x | Ax = y, y_min <= y <= y_max }To find the half-space representation (faces) of the polytope after finding the vertex representation  an convex-hull algorithm is used.The method is a part of thepycapacity.algorithmsmodulevertex_enumeration_vepoli2. See thedocs for more infoInstallationAll you need to do to install it is:pip install pycapacityAnd include it to your python projectimportpycapacity.robot# and/orimportpycapacity.human#and/orimportpycapacity.algorithms#and/orimportpycapacity.visualOther way to install the code is by installing it directly from the git repo:pip install git+https://github.com/auctus-team/pycapacity.gitPackage API docsSee full docs at thelinkModuleshumanrobotobjectsalgorithmsvisualFunctionsRobot metricsrobot.acceleration_ellipsoid: acceleration ellipsoid calculation (dynamic manipulability ellipsoid)robot.acceleration_polytope: 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_sum: Force polytope representing the minkowski sum of the capacities of the two robots in certain configurations.robot.velocity_ellipsoid: velocity manipulability ellipsoid calculationrobot.velocity_polytope: Velocity polytope calculating functionHuman metricshuman.acceleration_polytope: A function calculating the polytopes of achievable accelerationshuman.force_polytope: A function calculating the polytopes of achievable foreces basedhuman.joint_torques_polytope: A function calculating the polytopes of achievable joint torqueshuman.torque_to_muscle_force: A function calculating muscle forces needed to create the joint torques tauhuman.velocity_polytope: A function calculating the  polytopes of achievable velocity basedAlgorithmsalgorithms.hyper_plane_shift_method: Hyper plane shifting method implementation used to solve problems of a form:algorithms.iterative_convex_hull_method: A function calculating the polytopes of achievable x for equations form:algorithms.vertex_enumeration_vepoli2: Efficient vertex enumeration algorithm for a problem of a form:Visualisation toolsvisual.plot_polytope: Polytope faces plotting function in 2d and 3dvisual.plot_ellipsoid: Polytope faces plotting function in 2d and 3dvisual.plot_polytope_faces: Polytope faces plotting function in 2d and 3dvisual.plot_polytope_vertex: Polytope vertices plotting function in 2d and 3dCode examplesSeedemo_notebook.ipynbfor more examples of how ot use the module.Randomised serial robot example"""A simple example program for 3d force polytopeevaluation of a randomised 6dof robot"""importpycapacity.robotascapacity# robot capacity moduleimportnumpyasnpm=3# 3d forcesn=6# robot dofJ=np.array(np.random.rand(m,n))# random jacobian matrixt_max=np.ones(n)# joint torque limits max and mint_min=-np.ones(n)f_poly=capacity.force_polytope(J,t_min,t_max)# calculate the polytope vertices and facesprint(f_poly.vertices)# display the vertices# plotting the polytopeimportmatplotlib.pyplotaspltfrompycapacity.visualimport*# pycapacity visualisation toolsfig=plt.figure(4)# draw faces and verticesplot_polytope(polytope=f_poly,plot=plt,label='force polytope',vertex_color='blue',edge_color='blue',alpha=0.2)plt.legend()plt.show()Randomised muslucoskeletal model example"""A simple example program for 3d force polytopeevaluation of a randomised 30 muscle 7dofhuman musculoskeletal model"""importpycapacity.humanascapacity# robot capacity moduleimportnumpyasnpL=30# number of musclesm=3# 3d forcesn=6# number of joints - dofJ=np.array(np.random.rand(m,n))*2-1# random jacobian matrixN=np.array(np.random.rand(n,L))*2-1# random moment arm matrixF_max=100*np.ones(L)# muscle forces limits max and minF_min=np.zeros(L)f_poly=capacity.force_polytope(J,N,F_min,F_max,0.1)# calculate the polytope vertices and facesprint(f_poly.vertices)# display the vertices# plotting the polytopeimportmatplotlib.pyplotaspltfrompycapacity.visualimport*# pycapacity visualisation toolsfig=plt.figure(4)# draw faces and verticesplot_polytope(polytope=f_poly,plot=plt,label='force polytope',vertex_color='blue',edge_color='blue',alpha=0.2)plt.legend()plt.show()