pycapacity.robot module

Overview

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

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.reachable_space_nonlinear(forward_func, q0, time_horizon, q_max, q_min, dq_max, dq_min, options=None)[source]

Compute the reachable set of the robot for the given joint configuration. The algorithm calculates the reachable set of cartesian position of the desired frame of the robot given the robots joint position and joint velocity limits. The output of the algorithm is the reachable space that the robot is able to reach within the horizon time, while guaranteeing that the joint position and velocity limits are not violated.

If you are interested in the complete workspace of the robot, you can set a large time horizon (>1 second)

\[C_x = \{ x~ |~ x = f_{fk}(q_0 + \dot{q}\Delta t),\]
\[\dot{q}_{min} \leq \dot{q} \leq \dot{q}_{max},\quad {q}_{min} \leq q_0 + \dot{q}\Delta t \leq {q}_{max} \}\]

The parameters of the algorithm are set using the options dictionary. The following options are available:

  • n_samples: The number of samples to use for the discretization of the joint velocity space. The higher the number of samples, the more accurate the reachable set will be, however the longer the computation time will be

  • facet_dim: The dimension of the facet that will be sampled. Between 0 and the number of DOF of the robot. The higher the number of samples, the more accurate the reachable set will be, however the longer the computation time will be

  • convex_hull: Approximate the reachable set with a convex hull (True) or with a non-convex shape (False) - if False, CGAL must be installed

Parameters:

  • forward_func – The forward kinematic function, taking in the current joint position and ouputting the Cartesian space position (no orientation)

  • q0 – Current joint configuration

  • time_horizon – The time horizon for which to compute the reachable set

  • q_max – maximal joint position

  • q_min – minimal joint position

  • dq_max – maximal joint velocities

  • dq_min – minimal joint velocities

  • options – dictionary of options for the polytope calculation - currently supported calculate_faces, n_samples, facet_dim

Returns:

polytope object with vertices and faces if option calculate_faces is set to True in options

Return type:

polytope(Polytope)

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)