pycapacity.robot module

Overview

This is a python module which implements different robot performance metrics based on polytopes and ellipsoids.

acceleration polytope and ellipsoid
velocity (manipulability) polytope and ellipsoid
force polytope and ellipsoid
force polytope minkowski sum and intersection
reachable space approximation polytope

pycapacity.robot.acceleration_ellipsoid(J, M, t_max)[source]

Acceleration ellipsoid calculation (dynamic manipulability ellipsoid)

\[E_a = \{\ddot{x}~ |~ \ddot{x} = JM^{-1}\tau,\quad ||W^{-1}{\tau}|| \leq 1\}\]

where

\[W=diag(\tau_{max})\]

Parameters:

J – matrix jacobian
M – matrix inertia
t_max – maximal joint torques

Returns:

ellipsoid object with the following attributes radii, axes

Return type:

ellipsoid(Ellipsoid)

pycapacity.robot.acceleration_polytope(J, M, t_max, t_min, t_bias=None, options=None)[source]

Acceleration polytope calculating function

\[P_a = \{\ddot{x}~ |~ \ddot{x} = JM^{-1}\tau,\quad {\tau}_{min} \leq \tau \leq {\tau}_{max}\}\]

Based on the hyper_plane_shifting_method algorithm.

Parameters:

J – position jacobian
M – inertia matrix
t_max – maximal joint torque
t_min – minimal joint torque
t_bias – bias joint torques due to the gravity, robot dynamics and maybe some already appiled forces
options – dictionary of options for the polytope calculation (currently only calculate_faces is supported)

Returns:

polytope object with vertices, half-plane representation with matrices``H`` and d (face_indices and faces if option calculate_faces is set to True)

Return type:

acceleration_polytope(Polytope)

pycapacity.robot.force_ellipsoid(J, t_max)[source]

Force manipulability ellipsoid calculation

\[E_f = \{f~ |~ \tau = J^Tf,\quad ||W^{-1}\tau|| \leq 1\}\]

where

\[W=diag(\tau_{max})\]

Parameters:

J – matrix jacobian
t_max – maximal joint torques

Returns:

ellipsoid object with the following attributes radii, axes

Return type:

ellipsoid(Ellipsoid)

pycapacity.robot.force_polytope(Jacobian, t_max, t_min, t_bias=None, options=None)[source]

Force polytope representing the capacities of the two robots in a certain configuration

\[P_f = \{f~ |~ \tau = J^Tf,\quad {\tau}_{min} \leq \tau \leq {\tau}_{max}\}\]

Based on the vertex_enumeration_vepoli2 algorithm.

Parameters:

Jacobian – position jacobian
t_max – maximal joint torques
t_min – minimal joint torques
t_bias – bias joint torques due to the gravity, robot dynamics and maybe some already appiled forces for robot
options – dictionary of options for the polytope calculation - currently only calculate_faces is supported

Returns:

polytope object with vertices, torque vertices torque_vertices (face_indices and faces if option calculate_faces is set to True)

Return type:

force_polytope(Polytope)

pycapacity.robot.force_polytope_intersection(Jacobian1, Jacobian2, t1_max, t1_min, t2_max, t2_min, t1_bias=None, t2_bias=None, options=None)[source]

Force polytope representing the intersection of the capacities of the two robots in certain configurations.

\[P_f = \{f~ | ~ f_1\! =\! f_2\! =\! f, ~~ \tau_1 = J_1^Tf_1, ~~ \tau_2 = J_2^Tf_2, ~~ {t}_{1,min} \leq \tau_1 \leq {\tau}_{1,max},~~~ {\tau}_{2,min} \leq \tau_1 \leq {\tau}_{2,max}\}\]

Based on the vertex_enumeration_vepoli2 algorithm.

Parameters:

Jacobian1 – position jacobian robot 1
Jacobian2 – Jacobian2 position jacobian robot 2
t_min1 – minimal joint torques robot 1
t_min2 – minimal joint torques robot 2
t_max1 – maximal joint torques robot 1
t_max2 – maximal joint torques robot 2
t1_bias – bias joint torques due to the gravity, robot dynamics and maybe some already applied forces for robot 1
t2_bias – bias joint torques due to the gravity, robot dynamics and maybe some already applied forces for robot 2
options – dictionary of options for the algorithm (currently supported: calculate_faces)

Returns:

force polytope representing the intersection of the capacities of the two robots in certain configurations. Vertex representation vertices and hal-plane representation H and d (face representation faces and face_indices if calculate_faces is set to True in options)

Return type:

polytope(Polytope)

pycapacity.robot.force_polytope_sum(Jacobian1, Jacobian2, t1_max, t1_min, t2_max, t2_min, t1_bias=None, t2_bias=None, options=None)[source]

Force polytope representing the minkowski sum of the capacities of the two robots in certain configurations. With ordered vertices into the faces.

\[P_f = \{f~ | ~ f\! =\! f_1\! +\! f_2, ~~ \tau_1 = J_1^Tf_1, ~~ \tau_2 = J_2^Tf_2, ~~ {\tau}_{1,min} \leq \tau_1 \leq {\tau}_{1,max},~~~ {\tau}_{2,min} \leq \tau_1 \leq {\tau}_{2,max}\}\]

Based on the vertex_enumeration_vepoli2 algorithm.

Parameters:

Jacobian1 – position jacobian robot 1
Jacobian2 – Jacobian2 position jacobian robot 2
t_min1 – minimal joint torques robot 1
t_min2 – minimal joint torques robot 2
t_max1 – maximal joint torques robot 1
t_max2 – maximal joint torques robot 2
t1_bias – bias joint torques due to the gravity, robot dynamics and maybe some already applied forces for robot 1
t2_bias – bias joint torques due to the gravity, robot dynamics and maybe some already applied forces for robot 2
options – dictionary of additional options (currently supported only calculate_faces option)

Returns:

polytope object with vertices and half-plane representation H and d, ( faces and face_indices if calculate_faces option is set to True)

Return type:

polytope(Polytope)

pycapacity.robot.reachable_space_approximation(M, J, q0, horizon, t_max, t_min, t_bias=None, q_max=None, q_min=None, dq_max=None, dq_min=None, x0=None, A_x=None, b_x=None, options=None)[source]

Reachable space aproximation function based on convex polytopes. For a given time horizon, it calculates the reachable space of the robot. It evaluates the polytope of a form:

\[P_x = \{\Delta x~ |~ \Delta{x} = JM^{-1}\tau \Delta t_{h}^2/2,\]

\[{\tau}_{min} - \tau_{bias} \leq \tau \leq {\tau}_{max} - \tau_{bias}\]

\[\dot{q}_{min} \leq M^{-1}\tau \Delta t_{h} \leq \dot{q}_{max}\]

\[{q}_{min} \leq M^{-1}\tau \Delta t_{h}^2/2 \leq {q}_{max}\]

\[A_x \Delta{x} \leq b_x - A_x x_0\}\]

where \(\tau_{bias}\) is the bias joint torques due to the gravity, robot dynamics and maybe some already appiled forces which is optional. and \(\Delta t_{h}\) is the time horizon. If limits on joint velocity \(\dot{q}_{min}\) and \(\dot{q}_{max}\) or joint postion limits \({q}_{min}\) and \({q}_{max}\) are not given, the function calculates the ploytope without them. A_x and b_x are additional inequality covex constraints in the Cartesian position space. x0 is the initial Cartesian position corresponding to the initial joint position q0.

Based on the iterative_convex_hull algorithm.

Parameters:

M – inertia matrix
J – position jacobian
q0 – initial joint position
horizon – time horizon
t_max – maximal joint torque
t_min – minimal joint torque
x0 – initial Cartesian position (optional)
t_bias – bias joint torques due to the gravity, robot dynamics and maybe some already appiled forces (optional)
q_max – maximal joint position (optional)
q_min – minimal joint position (optional)
dq_max – maximal joint velocities (optional)
dq_min – minimal joint velocities (optional)
A_x – additional inequality constraints matrices in Cartesian position space (optional)
b_x – additional inequality constraints matrices in Cartesian position space (optional)
options – dictionary of options for the polytope calculation - currently supported tolerance, max_iteration and verbose (default: tolerance=1e-3, max_iteration=500, verbose=False)

Returns:

polytope object with vertices, torque vertices torque_vertices, half-plane representation with matrices H and d, and faces defiend by face_indices and faces

Return type:

polytope(Polytope)

Raises:

ValueError – if the mass and jacobian matrices are not appropriate size
ValueError – if any of the provided joint limits sizes are not equal to the number of joints
ValueError – if bias joint torques are given and their size is not equal to the number of joints

Note

Skuric, Antun, Vincent Padois, and David Daney. “Approximating robot reachable space using convex polytopes.” Human-Friendly Robotics 2022: HFR: 15th International Workshop on Human-Friendly Robotics. Cham: Springer International Publishing, 2023.

pycapacity.robot.velocity_ellipsoid(J, dq_max)[source]

Velocity manipulability ellipsoid calculation

\[E_f = \{\dot{x}~ |~ J\dot{q} = \dot{x},\quad ||W^{-1}\dot{q}|| \leq 1\}\]

where

\[W=diag(\dot{q}_{max})\]

Parameters:

J – position jacobian
dq_max – maximal joint velocities

Returns:

ellipsoid object with the following attributes radii, axes

Return type:

ellipsoid(Ellipsoid)

pycapacity.robot.velocity_polytope(Jacobian, dq_max, dq_min, options=None)[source]

Velocity polytope calculating function

\[P_f = \{\dot{x}~ |~ J\dot{q} = \dot{x},\quad {\dot{q}}_{min} \leq \dot{q} \leq {\dot{q}}_{max}\}\]

Based on the hyper_plane_shifting_method algorithm.

Parameters:

Jacobian – position jacobian
dq_max – maximal joint velocities
dq_min – minimal joint velocities
options – dictionary of options for the polytope calculation - currently only calculate_faces is supported

Returns:

polytope object with vertices, halfspaces H and d (face_indices and faces if option calculate_faces is set to True)

Return type:

velocity_polytope(Polytope)