Closest Points

Compute the minimum distance and closest points between two convex shapes.

class rigeo.closest.ClosestPointInfo(p1: ndarray, p2: ndarray, dist: float)

Information about a closest point query.

Parameters:

• p1 (np.ndarray) – The closest point on the first shape.

• p2 (np.ndarray) – The closest point on the second shape.

• dist (float, non-negative) – The distance between the two shapes.

dist: float

p1: ndarray

p2: ndarray

rigeo.closest.closest_points(shape1, shape2, solver=None)

Compute the closest points between two shapes.

When the two shapes are in contact or penetrating, the distance will be zero and the points can be anything inside the intersection.

This function is not optimized for speed: a full convex program is solved. Useful for prototyping but not for high-speed queries; in the latter case, use something like hpp-fcl.

Parameters:

• shape1 (ConvexPolyhedron or Ellipsoid or Cylinder) – The first shape to check.

• shape2 (ConvexPolyhedron or Ellipsoid or Cylinder) – The second shape to check.

• solver (str or None) – The solver for cvxpy to use.

Returns:

Information about the closest points.

Return type:

ClosestPointInfo

Constraint

rigeo.constraint.pim_must_equal_param_var(param_var, eps)

rigeo.constraint.pim_must_equal_vec(θ)

Generate a cvxpy expression that converts a parameter vector to pseudo-inertia matrix.

Parameters:

θ (np.ndarray or cp.Expression, shape (10,)) – The inertial parameter vector.

Returns:

The corresponding pseudo-inertia matrix.

Return type:

cp.Expression, shape (4, 4)

rigeo.constraint.pim_psd(J, ε=0)

rigeo.constraint.schur(A, B, C)

Construct the Schur complement matrix

\[\begin{split}\begin{bmatrix} \boldsymbol{A} & \boldsymbol{B} \\ \boldsymbol{B}^T & \boldsymbol{C} \end{bmatrix}\end{split}\]

Parameters:

• A (float or np.ndarray or cp.Expression) –

• B (float or np.ndarray or cp.Expression) –

• C (float or np.ndarray or cp.Expression) –

Returns:

The Schur complement matrix.

Return type:

cp.Expression

Geodesic

rigeo.geodesic.positive_definite_distance(A, B)

Geodesic distance between two symmetric positive definite matrices \(A\) and \(B\).

This metric is coordinate-frame invariant. See Lee et al. [2]. for more details.

Parameters:

• A (np.ndarray, shape (n, n)) – A symmetric positive definite matrix.

• B (np.ndarray, shape (n, n)) – A symmetric positive definite matrix.

Returns:

The geodesic distance between \(A\) and \(B\).

Return type:

float, non-negative

Identification

class rigeo.identify.IdentificationProblem(As, bs, γ=0, ε=0, solver=None)

Inertial parameter identification problem.

The problem is formulated as a convex, constrained least-squares problem and solved via cxvpy.

As

The regressor matrices.

Type:

np.ndarray or Iterable[np.ndarray]

bs

The regressor vectors.

Type:

np.ndarray or Iterable[np.ndarray]

γ

The coefficient of the regularization term.

Type:

float, non-negative

ε

The value such that each body satisfies \(\boldsymbol{J}\succcurlyeq\epsilon\boldsymbol{1}_4\).

Type:

float, non-negative

solver

The underlying solver for cvxpy to use.

Type:

str or None

problem

Once solve has been called, the underlying cvxpy.Problem instance is made available for inspection.

Type:

cxvpy.Problem

solve(bodies, must_realize=True, **kwargs)

Solve the identification problem.

Additional kwargs are passed to the solve method of the cvxpy.Problem instance.

Parameters:

• bodies (Iterable[RigidBody]) – The rigid bodies used to (1) constrain the parameters to be realizable within their shapes and (2) to provide nominal parameters for regularization.

• must_realize (bool) – If True, enforce density realizable constraints. If False, the problem is unconstrained except that each pseudo-inertia matrix must be positive definite.

Returns:

The identified inertial parameters for each body.

Return type:

Iterable[InertialParameters]

rigeo.identify.entropic_regularizer(Js, J0s)

Entropic regularizer for inertial parameter identification.

See Lee et al. [2]. The regularizer is convex in Js.

Parameters:

• Js (cxvpy.Expression or Iterable[cxvpy.Expression]) – The pseudo-inertia matrix variables to regularize.

• J0s (np.ndarray, shape (4, 4), or Iterable[np.ndarray]) – The nominal values for the pseudo-inertia matrices.

Returns:

The cvxpy expression for the regularizer.

Return type:

cxvpy.Expression

rigeo.identify.least_squares_objective(θs, As, bs, W0=None)

Least squares objective function

\[\sum_i \|\boldsymbol{A}_i\boldsymbol{\theta} - \boldsymbol{b}_i\|^2\]

where \(\boldsymbol{\theta}=[\boldsymbol{\theta}_1,\dots,\boldsymbol{\theta}_m]\).

Parameters:

• θs (cxvpy.Expression or Iterable[cxvpy.Expression]) – The regressor variables.

• As (np.ndarray or Iterable[np.ndarray]) – The regressor matrices.

• bs (np.ndarray or Iterable[np.ndarray]) – The regressor vectors.

• W0 (np.ndarray or None) – The inverse measurement covariance matrix. Defaults to identity if not provided.

Returns:

The cxvpy expression for the objective.

Return type:

cxvpy.Expression

Inertial

class rigeo.inertial.InertialParameters(mass, h=None, com=None, H=None, I=None, translate_from_com=False)

Inertial parameters of a rigid body.

Parameters:

• mass (float) – Mass of the body.

• h (np.ndarry, shape (3,)) – First moment of mass.

• H (np.ndarray, shape (3, 3)) – Second moment matrix.

body_wrench(V, A)

Compute the body-frame wrench about the reference point.

consistent(tol=0)

Check if the inertial parameters are fully physically consistent.

This means that there exists a physical rigid body that can have these parameters; a necessary and sufficient condition is that the pseudo-inertia matrix is positive semidefinite.

Parameters:

tol (float, non-negative) – The parameters will be considered consistent if all of the eigenvalues of the pseudo-inertia matrix are greater than or equal to -tol.

Returns:

True if the parameters are consistent, False otherwise.

Return type:

bool

classmethod from_pim(J)

Construct from a pseudo-inertia matrix.

The pseudo-inertia matrix is

\[\begin{split}\boldsymbol{J} = \begin{bmatrix} \boldsymbol{H} & m\boldsymbol{c} \\ m\boldsymbol{c}^T & m \end{bmatrix}\end{split}\]

Parameters:

J (np.ndarray (4, 4)) – The pseudo-inertia matrix.

classmethod from_point_masses(masses, points)

Construct from a system of point masses.

Parameters:

• masses (np.ndarray, shape (n,)) – The masses at each point.

• points (np.ndarray, shape (n, 3)) – The locations of each mass.

classmethod from_vec(vec)

Construct from a parameter vector.

is_same(other)

Check if this set of inertial parameters is the same as another.

Parameters:

other (InertialParameters) – The other set of inertial parameters to check.

Returns:

True if they are the same, False otherwise.

Return type:

bool

classmethod random()

Generate a random set of physically consistent inertial parameters.

Useful for testing purposes.

transform(rotation=None, translation=None)

classmethod zero()

property Hc

H matrix about the CoM.

property I

Inertia matrix.

property Ic

Inertia matrix about the CoM.

property J

Pseudo-inertia matrix.

property M

Spatial mass matrix.

property h

property vec

Inertial parameter vector.

rigeo.inertial.H2I(H)

Convert second moment matrix to inertia matrix.

rigeo.inertial.I2H(I)

Convert inertia matrix to second moment matrix.

Multibody

class rigeo.multibody.MultiBody(model, geom_model, tool_link_name=None, gravity=None)

A connected set of rigid bodies.

compute_forward_kinematics(q, v=None, a=None)

Compute forward kinematics.

This must be called before any calls to obtain task-space quantities, such as get_frame_pose, get_frame_velocity, etc.

Parameters:

• q (np.ndarray, shape (self.nq,)) – The joint positions.

• v (np.ndarray, shape (self.nv,)) – The joint velocities.

• a (np.ndarray, shape (self.nv,)) – The joint accelerations.

compute_frame_jacobian(q, frame=None, expressed_in=pinocchio.pinocchio_pywrap.ReferenceFrame.LOCAL)

Compute the robot geometric Jacobian.

compute_joint_torque_regressor(q, v, a)

Compute the joint torque regressor matrix for the multibody.

The joint torque regressor matrix maps the stacked vector of link inertial parameters to the joint torques.

Parameters:

• q (np.ndarray, shape (self.nq,)) – The joint positions.

• v (np.ndarray, shape (self.nv,)) – The joint velocities.

• a (np.ndarray, shape (self.nv,)) – The joint accelerations.

Returns:

The joint torque regressor matrix.

Return type:

np.ndarray, shape (self.nv, 10 * self.nlinks)

compute_joint_torques(q, v, a)

Compute the joint torques corresponding to a given motion.

This takes model.gravity into account.

Parameters:

• q (np.ndarray, shape (self.nq,)) – The joint positions.

• v (np.ndarray, shape (self.nv,)) – The joint velocities.

• a (np.ndarray, shape (self.nv,)) – The joint accelerations.

Returns:

The corresponding joint torques.

Return type:

np.ndarray, shape (self.nv,)

classmethod from_urdf_file(urdf_file_path, root_joint=None, tool_link_name=None, gravity=None)

Load the model directly from a URDF file.

classmethod from_urdf_string(urdf_str, root_joint=None, tool_link_name=None, gravity=None)

Load the model from a URDF string.

Parameters:

• urdf_str (str) – The string representing the URDF.

• root_joint (pinocchio.JointModel or None) – The root joint of the model (optional).

• tool_name (str or None) – The name of the robot’s tool.

• gravity (np.ndarray, shape (3,) or None) – The gravity vector. If None, defaults to pinocchio’s [0, 0, -9.81].

get_bodies(joints)

Get the rigid bodies corresponding to the given joints.

Parameters:

joints (Iterable[str or int]) – Joints to get the bodies for. Can either be specified by name or index.

Returns:

A list of rigid bodies corresponding to the joints.

Return type:

list[RigidBody]

get_frame_classical_acceleration(frame=None, expressed_in=pinocchio.pinocchio_pywrap.ReferenceFrame.LOCAL)

Get the classical acceleration of a link.

get_frame_index(name)

Get the index of a frame by name.

Parameters:

name (str) – The name of the frame.

Returns:

The index of the frame.

Return type:

int

Raises:

ValueError – If the frame does not exist.

get_frame_pose(frame=None)

Get the pose of a frame.

Note that compute_forward_kinematics(q, ...) must be called first.

Parameters:

frame (int or str or None) – If int, this is interpreted as the frame index. If str, interpreted as the name of a frame. If None, defaults to self.tool_idx.

Returns:

Returns a tuple (position, orientation). position is a np.ndarray of shape (3,) and orientation is a rotation matrix represented by an np.ndarray of shape (3, 3).

Return type:

tuple

get_frame_spatial_acceleration(frame=None, expressed_in=pinocchio.pinocchio_pywrap.ReferenceFrame.LOCAL)

Get the spatial acceleration of a link.

get_frame_velocity(frame=None, expressed_in=pinocchio.pinocchio_pywrap.ReferenceFrame.LOCAL)

Get velocity of link at index link_idx

get_joint_index(name)

Get the index of a joint by name.

Parameters:

name (str) – The name of the joint.

Returns:

The index of the joint.

Return type:

int

Raises:

ValueError – If the joint does not exist.

is_realizable(joints=None, solver=None)

Check if (a subset of) the multibody is density realizable.

Parameters:

• joints (Iterable[str or int] or None) – If not None, only check density realizability on the bodies corresponding to these joints. index.

• solver (str or None) – If checking realizability requires solving an optimization problem, one can optionally be specified.

Returns:

True if the given joints are realizable, False otherwise.

Return type:

bool

Polyhedron Double Description

Double description of convex polyhedra.

class rigeo.polydd.FaceForm(A_ineq, b_ineq, A_eq=None, b_eq=None)

Face form (H-rep) of a convex polyhedron.

canonical()

Convert to canonical non-redundant representation.

Returns:

A canonicalized version of the face form.

Return type:

FaceForm

classmethod from_cdd_matrix(mat)

Construct from a CDD matrix.

Parameters:

mat (cdd.Matrix) – The CDD matrix representing the polyhedron.

stack(other)

Combine two face forms together.

The corresponds to an intersection of polyhedra.

Parameters:

other (FaceForm) – The other face form to combine with this one.

Returns:

The combined face form representing the intersection.

Return type:

FaceForm

to_cdd_matrix()

Convert to a CDD matrix.

Returns:

A CDD matrix representing the polyhedron.

Return type:

cdd.Matrix

to_span_form()

Convert to span form (V-rep).

Returns:

The equivalent span form of the polyhedron.

Return type:

SpanForm

property dim

The dimension of the ambient space.

property nf

Number of faces.

class rigeo.polydd.SpanForm(vertices=None, rays=None, span=None)

Span form (V-rep) of a convex polyhedron.

vertices

The vertices of the polyhedron.

Type:

np.ndarray, shape (self.nv, self.dim) or None

rays

The rays of the polyhedron.

Type:

np.ndarray, shape (self.nr, self.dim) or None

bounded()

Check if the polyhedron is bounded.

Returns:

True if the polyhedron is bounded, False otherwise.

Return type:

bool

canonical()

Convert to canonical non-redundant representation.

In other words, take the convex hull of the vertices.

Returns:

A canonicalized version of the span form.

Return type:

SpanForm

classmethod from_cdd_matrix(mat)

Construct from a CDD matrix.

Parameters:

mat (cdd.Matrix) – The CDD matrix representing the polyhedron.

is_cone()

Check if the polyhedron is a cone.

This means that for any point \(x\) in the polyhedron, then \(\alpha x\) is also in the polyhedron for any \(\alpha>0\).

Returns:

True if the polyhedron is a cone, False otherwise.

Return type:

bool

to_cdd_matrix()

Convert to a CDD matrix.

Returns:

A CDD matrix representing the polyhedron.

Return type:

cdd.Matrix

to_face_form()

Convert to face form.

Returns:

The equivalent face form of the polyhedron.

Return type:

FaceForm

property dim

Dimension of the ambient space.

property nr

Number of rays.

property nv

Number of vertices.

Random

Generate random values.

rigeo.random.random_points_in_ball(shape=1, dim=3)

Sample random uniform-distributed points in the dim-ball.

See https://compneuro.uwaterloo.ca/files/publications/voelker.2017.pdf

rigeo.random.random_points_on_hypersphere(shape=1, dim=2)

Sample random uniform-distributed points on the dim-sphere.

See https://compneuro.uwaterloo.ca/files/publications/voelker.2017.pdf

rigeo.random.random_psd_matrix(n)

Generate a random symmetric positive semidefinite matrix.

Parameters:

n (int) – Dimension of the matrix.

Returns:

A positive semidefinite matrix.

Return type:

np.ndarray, shape (n, n)

rigeo.random.random_weight_vectors(shape)

Generate a set of random vectors with non-negative entries that sum to one.

Vectors sum to one along the last axis of shape. Entries are uniformly distributed.

Parameters:

shape (int or tuple) – Shape of the weight vector(s).

Returns:

A set of weight vectors.

Return type:

np.ndarray

Rigid Bodies

Three-dimensional rigid bodies.

class rigeo.rigidbody.RigidBody(shapes, params=None)

A rigid body in three dimensions.

The rigid body is defined by a list of shapes and a set of inertial parameters.

shapes

A list of shapes, the union of which defines the shape of the body.

Type:

list

params

The inertial parameters of the body. If none are provided, then they default to zero and the body acts like an “empty” (massless) shape.

Type:

InertialParameters

can_realize(params, solver=None)

Check if the rigid body can realize a set of inertial parameters.

Parameters:

• params (InertialParameters) – The inertial parameters to check.

• solver (str or None) – If checking realizability requires solving an optimization problem, one can optionally be specified.

Returns:

True if the parameters are realizable, False otherwise.

Return type:

bool

is_realizable(solver=None)

Check if the rigid body is density realizable.

Parameters:

solver (str or None) – If checking realizability requires solving an optimization problem, one can optionally be specified.

Returns:

True if self.params is realizable on self.shapes, False otherwise.

Return type:

bool

mbes(sphere=False, solver=None)

Generate a new rigid body with each shape replaced with its bounding ellipsoid.

Parameters:

• sphere (bool) – If True, use bounding spheres rather than ellipsoids.

• solver (str or None) – If generating the minimum bounding ellipsoid requires solving an optimization problem, a solver can optionally be specified.

Returns:

The new body with the same inertial parameters but each shapes replaced by its minimum-volume bounding ellipsoid.

Return type:

RigidBody

must_realize(param_var, eps=0)

Generate cvxpy constraints for inertial parameters to be realizable on this body.

Parameters:

• param_var (cp.Expression, shape (4, 4) or shape (10,)) – The cvxpy inertial parameter variable. If shape is (4, 4), this is interpreted as the pseudo-inertia matrix. If shape is (10,), this is interpreted as the inertial parameter vector.

• eps (float, non-negative) – Pseudo-inertia matrix J is constrained such that J - eps * np.eye(4) is positive semidefinite and J is symmetric.

Returns:

List of cvxpy constraints.

Return type:

list

static regressor(V, A)

Compute regressor matrix Y for the body.

The regressor maps the inertial parameters to the body inertial wrench: w = Yθ.

Parameters:

• V (np.ndarray, shape (6,)) – Body-frame velocity.

• A (np.ndarray, shape (6,)) – Body-frame acceleration.

Returns:

The regressor matrix.

Return type:

np.ndarray, shape (6, 10)

transform(rotation=None, translation=None)

Apply a rigid transform to the body.

Parameters:

• rotation (np.ndarray, shape (d, d)) – Rotation matrix.

• translation (np.ndarray, shape (d,)) – Translation vector.

Returns:

A new rigid body that has been rigidly transformed.

Return type:

RigidBody

Shape

Polyhedral and ellipsoidal geometry.

class rigeo.shape.Box(half_extents, center=None, rotation=None)

A box aligned with the x, y, z axes.

Parameters:

• half_extents – The (x, y, z) half extents of the box. The half extents are each half the length of the corresponding side lengths.

• center – The center of the box. Defaults to the origin.

half_extents

The (x, y, z) half extents of the box.

center

The center of the box.

rotation

The orientation of the box. If rotation=np.eye(3), then the box is axis-aligned.

Type:

np.ndarray, shape (3, 3)

can_realize(params, tol=0, solver=None)

Check if the shape can realize the inertial parameters.

Parameters:

• params (InertialParameters) – The inertial parameters to check.

• tol (float) – Numerical tolerance for realization. This is applied in a shape-dependent manner.

• solver (str or None) – If checking realizability requires solving an optimization problem, a solver can optionally be specified.

Returns:

True if the parameters are realizable, False otherwise.

Return type:

bool

classmethod cube(half_extent, center=None, rotation=None)

Construct a cube.

classmethod from_points_to_bound(points)

Construct the smallest axis-aligned box that contains all of the points.

classmethod from_side_lengths(side_lengths, center=None, rotation=None)

Construct a box with given side lengths.

classmethod from_two_vertices(v1, v2)

Construct an axis-aligned box from two opposed vertices.

grid(n)

Generate a set of points evenly spaced in the box.

Parameters:

n (int) – The number of points in each of the three dimensions.

Returns:

An array of points with shape (n**3, 3).

mbe(rcond=None, sphere=False, solver=None)

Construct the minimum-volume bounding ellipsoid for this polyhedron.

mie(rcond=None, sphere=False, solver=None)

Construct the maximum-volume ellipsoid inscribed in this polyhedron.

must_realize(param_var, eps=0)

Generate cvxpy constraints for inertial parameters to be realizable on this shape.

Parameters:

• param_var (cp.Expression, shape (4, 4) or shape (10,)) – The cvxpy inertial parameter variable. If shape is (4, 4), this is interpreted as the pseudo-inertia matrix. If shape is (10,), this is interpreted as the inertial parameter vector.

• eps (float, non-negative) – Pseudo-inertia matrix J is constrained such that J - eps * np.eye(4) is positive semidefinite and J is symmetric.

Returns:

List of cvxpy constraints.

Return type:

list

rotate_about_center(rotation)

Rotate the box about its center point.

Parameters:

rotation (np.ndarray, shape (3, 3)) – Rotation matrix.

Returns:

A new box that has been rotated about its center point.

Return type:

Box

transform(rotation=None, translation=None)

Apply a rigid transform to the shape.

Parameters:

• rotation (np.ndarray, shape (d, d)) – Rotation matrix.

• translation (np.ndarray, shape (d,)) – Translation vector.

Returns:

A new shape that has been rigidly transformed.

Return type:

Shape

uniform_density_params(mass)

Generate the inertial parameters corresponding to a uniform mass density.

The inertial parameters are generated with respect to the origin.

Parameters:

mass (float, non-negative) – The mass of the body.

Returns:

The inertial parameters.

Return type:

InertialParameters

property side_lengths

The side lengths of the box.

property volume

The volume of the box.

class rigeo.shape.Capsule(cylinder)

A capsule in three dimensions.

Parameters:

cylinder (Cylinder) – The cylinder to build the capsule from.

aabb()

Generate the minimum-volume axis-aligned box that bounds the shape.

Returns:

The axis-aligned bounding box.

Return type:

Box

can_realize(params, tol=0, solver=None)

Check if the shape can realize the inertial parameters.

Parameters:

• params (InertialParameters) – The inertial parameters to check.

• tol (float) – Numerical tolerance for realization. This is applied in a shape-dependent manner.

• solver (str or None) – If checking realizability requires solving an optimization problem, a solver can optionally be specified.

Returns:

True if the parameters are realizable, False otherwise.

Return type:

bool

contains(points, tol=1e-08)

Test if the shape contains a set of points.

Parameters:

• points (np.ndarray, shape (n, self.dim)) – The points to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

Boolean array where each entry is True if the shape contains the corresponding point and False otherwise.

Return type:

bool or np.ndarray of bool, shape (n,)

is_same(other, tol=1e-08)

Check if this shape is the same as another one.

Parameters:

• other (Shape) – The other shape to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

True if the polyhedra are the same, False otherwise.

Return type:

bool

mbb()

mbe(rcond=None, sphere=False, solver=None)

Generate the minimum-volume bounding ellipsoid for the shape.

Parameters:

• sphere (bool) – If True, force the ellipsoid to be a sphere.

• solver (str or None) – If generating the minimum bounding ellipsoid requires solving an optimization problem, a solver can optionally be specified.

Returns:

The minimum bounding ellipsoid (or sphere, if sphere=True).

Return type:

Ellipsoid

must_contain(points, scale=1.0)

Generate cvxpy constraints to keep the points inside the shape.

Parameters:

• points (cp.Variable, shape (self.dim,) or (n, self.dim)) – A point or set of points to constrain to lie inside the shape.

• scale (float, positive) – Scale for points. The main idea is that one may wish to check that the CoM belongs to the shape, but using the quantity \(h=mc\). Then must_contain(c) is equivalent to must_contain(h, scale=m).

Returns:

A list of cxvpy constraints that keep the points inside the shape.

Return type:

list

must_realize(param_var, eps=0)

Generate cvxpy constraints for inertial parameters to be realizable on this shape.

Parameters:

• param_var (cp.Expression, shape (4, 4) or shape (10,)) – The cvxpy inertial parameter variable. If shape is (4, 4), this is interpreted as the pseudo-inertia matrix. If shape is (10,), this is interpreted as the inertial parameter vector.

• eps (float, non-negative) – Pseudo-inertia matrix J is constrained such that J - eps * np.eye(4) is positive semidefinite and J is symmetric.

Returns:

List of cvxpy constraints.

Return type:

list

random_points(shape=1)

Generate random points contained in the shape.

Parameters:

shape (int or tuple) – The shape of the set of points to be returned.

Returns:

The random points.

Return type:

np.ndarray, shape shape + (self.dim,)

transform(rotation=None, translation=None)

Apply a rigid transform to the shape.

Parameters:

• rotation (np.ndarray, shape (d, d)) – Rotation matrix.

• translation (np.ndarray, shape (d,)) – Translation vector.

Returns:

A new shape that has been rigidly transformed.

Return type:

Shape

property center

property full_length

property inner_length

property radius

property rotation

class rigeo.shape.ConvexPolyhedron(face_form=None, span_form=None)

A convex polyhedron in dim dimensions.

The __init__ method accepts either or both of the span (V-rep) and face (H-rep) forms of the polyhedron. If neither is provided, an error is raised. It is typically more convenient to construct the polyhedron using from_vertices or from_halfspaces.

Parameters:

• face_form (FaceForm or None) – The face form of the polyhedron.

• span_form (SpanForm or None) – The span form of the polyhedron.

aabb()

Generate the minimum-volume axis-aligned box that bounds the shape.

Returns:

The axis-aligned bounding box.

Return type:

Box

can_realize(params, tol=0, solver=None)

Check if the shape can realize the inertial parameters.

Parameters:

• params (InertialParameters) – The inertial parameters to check.

• tol (float) – Numerical tolerance for realization. This is applied in a shape-dependent manner.

• solver (str or None) – If checking realizability requires solving an optimization problem, a solver can optionally be specified.

Returns:

True if the parameters are realizable, False otherwise.

Return type:

bool

contains(points, tol=1e-08)

Test if the shape contains a set of points.

Parameters:

• points (np.ndarray, shape (n, self.dim)) – The points to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

Boolean array where each entry is True if the shape contains the corresponding point and False otherwise.

Return type:

bool or np.ndarray of bool, shape (n,)

classmethod from_halfspaces(A, b, prune=False)

Construct the polyhedron from a set of halfspaces.

The polyhedron is the set {x | Ax <= b}. For degenerate cases with linear equality constraints, use the __init__ method to pass a face form directly.

Parameters:

• A (np.ndarray) – The matrix of halfspace normals.

• b (np.ndarray) – The vector of halfspace offsets.

• prune (bool) – If True, the halfspaces will be pruned to eliminate any redundancies.

classmethod from_vertices(vertices, prune=False)

Construct the polyhedron from a set of vertices.

Parameters:

• vertices (np.ndarray, shape (nv, dim)) – The extremal points of the polyhedron.

• prune (bool) – If True, the vertices will be pruned to eliminate any non-extremal points.

intersect(other)

Intersect this polyhedron with another one.

Parameters:

other (ConvexPolyhedron) – The other polyhedron.

Returns:

The intersection, which is another ConvexPolyhedron, or None if the two polyhedra do not intersect.

Return type:

ConvexPolyhedron or None

is_same(other, tol=1e-08)

Check if this shape is the same as another one.

Parameters:

• other (Shape) – The other shape to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

True if the polyhedra are the same, False otherwise.

Return type:

bool

mbe(rcond=None, sphere=False, solver=None)

Construct the minimum-volume bounding ellipsoid for this polyhedron.

mie(rcond=None, sphere=False, solver=None)

Construct the maximum-volume ellipsoid inscribed in this polyhedron.

must_contain(points, scale=1.0)

Generate cvxpy constraints to keep the points inside the shape.

Parameters:

• points (cp.Variable, shape (self.dim,) or (n, self.dim)) – A point or set of points to constrain to lie inside the shape.

• scale (float, positive) – Scale for points. The main idea is that one may wish to check that the CoM belongs to the shape, but using the quantity \(h=mc\). Then must_contain(c) is equivalent to must_contain(h, scale=m).

Returns:

A list of cxvpy constraints that keep the points inside the shape.

Return type:

list

must_realize(param_var, eps=0)

Generate cvxpy constraints for inertial parameters to be realizable on this shape.

Parameters:

• param_var (cp.Expression, shape (4, 4) or shape (10,)) – The cvxpy inertial parameter variable. If shape is (4, 4), this is interpreted as the pseudo-inertia matrix. If shape is (10,), this is interpreted as the inertial parameter vector.

• eps (float, non-negative) – Pseudo-inertia matrix J is constrained such that J - eps * np.eye(4) is positive semidefinite and J is symmetric.

Returns:

List of cvxpy constraints.

Return type:

list

random_points(shape=1)

Generate random points contained in the shape.

Parameters:

shape (int or tuple) – The shape of the set of points to be returned.

Returns:

The random points.

Return type:

np.ndarray, shape shape + (self.dim,)

transform(rotation=None, translation=None)

Apply a rigid transform to the shape.

Parameters:

• rotation (np.ndarray, shape (d, d)) – Rotation matrix.

• translation (np.ndarray, shape (d,)) – Translation vector.

Returns:

A new shape that has been rigidly transformed.

Return type:

Shape

vertex_point_mass_params(mass)

Compute the inertial parameters corresponding to a system of point masses located at the vertices.

Parameters:

mass (float or np.ndarray, shape (self.nv,)) – A single scalar represents the total mass which is uniformly distributed among the vertices. Otherwise represents the mass for each individual vertex.

Returns:

The parameters representing the point mass system.

Return type:

InertialParameters

property A

Matrix part of the face form (normals).

property b

Vector part of the face form (offsets).

property dim

The dimension of the ambient space.

property nf

Number of faces.

property nv

Number of vertices.

property vertices

The extremal points of the polyhedron.

class rigeo.shape.Cylinder(length, radius, rotation=None, center=None)

A cylinder in three dimensions.

Parameters:

• length (float, non-negative) – The length along the longitudinal axis.

• radius (float, non-negative) – The radius of the transverse cross-section.

• rotation (np.ndarray, shape (3, 3)) – Rotation matrix, where identity means the z-axis is the longitudinal axis.

• center (np.ndarray, shape (3,)) – The center of the cylinder. If not provided, defaults to the origin.

aabb()

Generate the minimum-volume axis-aligned box that bounds the shape.

Returns:

The axis-aligned bounding box.

Return type:

Box

can_realize(params, tol=0, solver=None)

Check if the shape can realize the inertial parameters.

Parameters:

• params (InertialParameters) – The inertial parameters to check.

• tol (float) – Numerical tolerance for realization. This is applied in a shape-dependent manner.

• solver (str or None) – If checking realizability requires solving an optimization problem, a solver can optionally be specified.

Returns:

True if the parameters are realizable, False otherwise.

Return type:

bool

capsule()

Generate a capsule from this cylinder.

Returns:

The capsule built from this cylinder. That is, this cylinder with two semispheres on the ends.

Return type:

Capsule

contains(points, tol=1e-08)

Test if the shape contains a set of points.

Parameters:

• points (np.ndarray, shape (n, self.dim)) – The points to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

Boolean array where each entry is True if the shape contains the corresponding point and False otherwise.

Return type:

bool or np.ndarray of bool, shape (n,)

endpoints()

Get the two points at the ends of the longitudinal axis.

is_same(other)

Check if this shape is the same as another one.

Parameters:

• other (Shape) – The other shape to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

True if the polyhedra are the same, False otherwise.

Return type:

bool

mbb()

mbe(rcond=None, sphere=False, solver=None)

Generate the minimum-volume bounding ellipsoid for the shape.

Parameters:

• sphere (bool) – If True, force the ellipsoid to be a sphere.

• solver (str or None) – If generating the minimum bounding ellipsoid requires solving an optimization problem, a solver can optionally be specified.

Returns:

The minimum bounding ellipsoid (or sphere, if sphere=True).

Return type:

Ellipsoid

mib()

must_contain(points, scale=1.0)

Generate cvxpy constraints to keep the points inside the shape.

Parameters:

• points (cp.Variable, shape (self.dim,) or (n, self.dim)) – A point or set of points to constrain to lie inside the shape.

• scale (float, positive) – Scale for points. The main idea is that one may wish to check that the CoM belongs to the shape, but using the quantity \(h=mc\). Then must_contain(c) is equivalent to must_contain(h, scale=m).

Returns:

A list of cxvpy constraints that keep the points inside the shape.

Return type:

list

must_realize(param_var, eps=0)

Generate cvxpy constraints for inertial parameters to be realizable on this shape.

Parameters:

• param_var (cp.Expression, shape (4, 4) or shape (10,)) – The cvxpy inertial parameter variable. If shape is (4, 4), this is interpreted as the pseudo-inertia matrix. If shape is (10,), this is interpreted as the inertial parameter vector.

• eps (float, non-negative) – Pseudo-inertia matrix J is constrained such that J - eps * np.eye(4) is positive semidefinite and J is symmetric.

Returns:

List of cvxpy constraints.

Return type:

list

random_points(shape=1)

Generate random points contained in the shape.

Parameters:

shape (int or tuple) – The shape of the set of points to be returned.

Returns:

The random points.

Return type:

np.ndarray, shape shape + (self.dim,)

transform(rotation=None, translation=None)

Apply a rigid transform to the shape.

Parameters:

• rotation (np.ndarray, shape (d, d)) – Rotation matrix.

• translation (np.ndarray, shape (d,)) – Translation vector.

Returns:

A new shape that has been rigidly transformed.

Return type:

Shape

uniform_density_params(mass)

property longitudinal_axis

property transverse_axes

property volume

The volume of the cylinder.

class rigeo.shape.Ellipsoid(half_extents, rotation=None, center=None)

Ellipsoid in dim dimensions.

The ellipsoid may be degenerate, which means that one or more of the half extents is zero and it has no volume.

aabb()

Generate the minimum-volume axis-aligned box that bounds the shape.

Returns:

The axis-aligned bounding box.

Return type:

Box

can_realize(params, tol=0, solver=None)

Check if the shape can realize the inertial parameters.

Parameters:

• params (InertialParameters) – The inertial parameters to check.

• tol (float) – Numerical tolerance for realization. This is applied in a shape-dependent manner.

• solver (str or None) – If checking realizability requires solving an optimization problem, a solver can optionally be specified.

Returns:

True if the parameters are realizable, False otherwise.

Return type:

bool

contains(points, tol=1e-08)

Check if points are contained in the ellipsoid.

Parameters:

• points (iterable) – Points to check. May be a single point or a list or array of points.

• tol (float, non-negative) – Numerical tolerance for qualifying as inside the ellipsoid.

Returns:

Given a single point, return True if the point is contained in the ellipsoid, or False if not. For multiple points, return a boolean array with one value per point.

contains_ellipsoid(other)

classmethod from_Einv(Einv, center=None)

classmethod from_Q(Q)

classmethod from_affine(A, b, rcond=None)

Construct an ellipsoid from an affine transformation of the unit ball.

hollow_density_params(mass)

is_degenerate()

Check if the ellipsoid is degenerate.

This means that it has zero volume, and lives in a lower dimension than the ambient one.

Returns:

Returns True if the ellipsoid is degenerate, False otherwise.

Return type:

bool

is_infinite()

is_same(other)

Check if this ellipsoid is the same as another.

lower()

Project onto a rank-dimensional subspace.

mbb()

Minimum-volume bounding box.

mbe(rcond=None, sphere=False)

Generate the minimum-volume bounding ellipsoid for the shape.

Parameters:

• sphere (bool) – If True, force the ellipsoid to be a sphere.

• solver (str or None) – If generating the minimum bounding ellipsoid requires solving an optimization problem, a solver can optionally be specified.

Returns:

The minimum bounding ellipsoid (or sphere, if sphere=True).

Return type:

Ellipsoid

mib()

Maximum-volume inscribed box.

must_contain(points, scale=1.0)

Generate cvxpy constraints to keep the points inside the shape.

Parameters:

• points (cp.Variable, shape (self.dim,) or (n, self.dim)) – A point or set of points to constrain to lie inside the shape.

• scale (float, positive) – Scale for points. The main idea is that one may wish to check that the CoM belongs to the shape, but using the quantity \(h=mc\). Then must_contain(c) is equivalent to must_contain(h, scale=m).

Returns:

A list of cxvpy constraints that keep the points inside the shape.

Return type:

list

must_realize(param_var, eps=0)

Generate cvxpy constraints for inertial parameters to be realizable on this shape.

Parameters:

• param_var (cp.Expression, shape (4, 4) or shape (10,)) – The cvxpy inertial parameter variable. If shape is (4, 4), this is interpreted as the pseudo-inertia matrix. If shape is (10,), this is interpreted as the inertial parameter vector.

• eps (float, non-negative) – Pseudo-inertia matrix J is constrained such that J - eps * np.eye(4) is positive semidefinite and J is symmetric.

Returns:

List of cvxpy constraints.

Return type:

list

random_points(shape=1)

Generate random points contained in the shape.

Parameters:

shape (int or tuple) – The shape of the set of points to be returned.

Returns:

The random points.

Return type:

np.ndarray, shape shape + (self.dim,)

rotate_about_center(rotation)

Rotate the ellipsoid about its center point.

Parameters:

rotation (np.ndarray, shape (d, d)) – Rotation matrix.

Returns:

A new ellipsoid that has been rigidly transformed.

Return type:

Ellipsoid

classmethod sphere(radius, center=None)

Construct a sphere.

Parameters:

• radius (float) – Radius of the sphere.

• center (np.ndarray, shape (dim,)) – Optional center point of the sphere.

transform(rotation=None, translation=None)

Apply a rigid transform to the shape.

Parameters:

• rotation (np.ndarray, shape (d, d)) – Rotation matrix.

• translation (np.ndarray, shape (d,)) – Translation vector.

Returns:

A new shape that has been rigidly transformed.

Return type:

Shape

uniform_density_params(mass)

property E

property Einv

property Q

Q representation of the ellipsoid.

\[\mathcal{E} = \{x\in\mathbb{R}^d \mid \tilde{x}^TQ\tilde{q}\geq 0\}\]

property affine_matrix

The matrix \(\boldsymbol{A}\) from when the ellipsoid is represented as an affine transformation of the unit ball.

\[\mathcal{E} = \{x\in\mathbb{R}^d \mid \|Ax+b\|^2\leq 1\}\]

property affine_vector

The vector \(\boldsymbol{b}\) from when the ellipsoid is represented as an affine transformation of the unit ball.

\[\mathcal{E} = \{x\in\mathbb{R}^d \mid \|Ax+b\|^2\leq 1\}\]

property dim

property rank

property volume

The volume of the ellipsoid.

class rigeo.shape.Shape

abstract aabb()

Generate the minimum-volume axis-aligned box that bounds the shape.

Returns:

The axis-aligned bounding box.

Return type:

Box

abstract can_realize(params, tol=0, solver=None)

Check if the shape can realize the inertial parameters.

Parameters:

• params (InertialParameters) – The inertial parameters to check.

• tol (float) – Numerical tolerance for realization. This is applied in a shape-dependent manner.

• solver (str or None) – If checking realizability requires solving an optimization problem, a solver can optionally be specified.

Returns:

True if the parameters are realizable, False otherwise.

Return type:

bool

abstract contains(points, tol=1e-08)

Test if the shape contains a set of points.

Parameters:

• points (np.ndarray, shape (n, self.dim)) – The points to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

Boolean array where each entry is True if the shape contains the corresponding point and False otherwise.

Return type:

bool or np.ndarray of bool, shape (n,)

contains_polyhedron(poly, tol=1e-08)

Check if this shape contains a polyhedron.

Parameters:

• poly (ConvexPolyhedron) – The polyhedron to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

True if this shapes contains the polyhedron, False otherwise.

Return type:

bool

grid(n)

Generate a regular grid inside the shape.

The approach is to compute a bounding box, generate a grid for that, and then discard any points not inside the actual polyhedron.

Parameters:

n (int) – The maximum number of points along each dimension.

Returns:

The points contained in the grid.

Return type:

np.ndarray, shape (N, self.dim)

abstract is_same(other, tol=1e-08)

Check if this shape is the same as another one.

Parameters:

• other (Shape) – The other shape to check.

• tol (float, non-negative) – The numerical tolerance for membership.

Returns:

True if the polyhedra are the same, False otherwise.

Return type:

bool

abstract mbe(rcond=None, sphere=False, solver=None)

Generate the minimum-volume bounding ellipsoid for the shape.

Parameters:

• sphere (bool) – If True, force the ellipsoid to be a sphere.

• solver (str or None) – If generating the minimum bounding ellipsoid requires solving an optimization problem, a solver can optionally be specified.

Returns:

The minimum bounding ellipsoid (or sphere, if sphere=True).

Return type:

Ellipsoid

abstract must_contain(points, scale=1.0)

Generate cvxpy constraints to keep the points inside the shape.

Parameters:

• points (cp.Variable, shape (self.dim,) or (n, self.dim)) – A point or set of points to constrain to lie inside the shape.

• scale (float, positive) – Scale for points. The main idea is that one may wish to check that the CoM belongs to the shape, but using the quantity \(h=mc\). Then must_contain(c) is equivalent to must_contain(h, scale=m).

Returns:

A list of cxvpy constraints that keep the points inside the shape.

Return type:

list

abstract must_realize(param_var, eps=0)

Generate cvxpy constraints for inertial parameters to be realizable on this shape.

Parameters:

• param_var (cp.Expression, shape (4, 4) or shape (10,)) – The cvxpy inertial parameter variable. If shape is (4, 4), this is interpreted as the pseudo-inertia matrix. If shape is (10,), this is interpreted as the inertial parameter vector.

• eps (float, non-negative) – Pseudo-inertia matrix J is constrained such that J - eps * np.eye(4) is positive semidefinite and J is symmetric.

Returns:

List of cvxpy constraints.

Return type:

list

abstract random_points(shape=1)

Generate random points contained in the shape.

Parameters:

shape (int or tuple) – The shape of the set of points to be returned.

Returns:

The random points.

Return type:

np.ndarray, shape shape + (self.dim,)

abstract transform(rotation=None, translation=None)

Apply a rigid transform to the shape.

Parameters:

• rotation (np.ndarray, shape (d, d)) – Rotation matrix.

• translation (np.ndarray, shape (d,)) – Translation vector.

Returns:

A new shape that has been rigidly transformed.

Return type:

Shape

rigeo.shape.convex_hull(points, rcond=None)

Get the vertices of the convex hull of a set of points.

Parameters:

• points (np.ndarray, shape (n, d)) – A set of n points in d dimensions for which to compute the convex hull. The points do not need to be full rank; that is, they may span a lower-dimensional space than \(\mathbb{R}^d\).

• rcond (float, optional) – Conditioning number used for internal routines.

Returns:

The vertices of the convex hull that fully contains the set of points.

Return type:

np.ndarray, shape (m, d)

rigeo.shape.mbe_of_ellipsoids(ellipsoids, sphere=False, solver=None)

Compute the minimum-volume bounding ellipsoid for a set of ellipsoids.

See Boyd and Vandenberghe [1], Section 8.4.1.

Parameters:

• ellipsoids (iterable of Ellipsoids) – The union of ellipsoids to bound.

• sphere (bool) – If True, compute the minimum bounding sphere. Defaults to False.

• solver (str or None) – The solver for cvxpy to use.

Returns:

The minimum-volume bounding ellipsoid.

Return type:

Ellipsoid

rigeo.shape.mbe_of_points(points, rcond=None, sphere=False, solver=None)

Compute the minimum-volume bounding ellipsoid for a set of points.

See Boyd and Vandenberghe [1], Section 8.4.1.

Parameters:

• points (np.ndarray, shape (n, d)) – The points to bound. There are n points in d dimensions.

• rcond (float, optional) – Conditioning number used for internal routines.

• sphere (bool) – If True, compute the minimum bounding sphere. Defaults to False.

• solver (str or None,) – The solver for cvxpy to use.

Returns:

The minimum-volume bounding ellipsoid.

Return type:

Ellipsoid

rigeo.shape.mie(vertices, rcond=None, sphere=False, solver=None)

Compute the maximum inscribed ellipsoid for a polyhedron represented by a set of vertices.

Returns the ellipsoid.

Utilities

rigeo.util.clean_transform(rotation, translation, dim)

rigeo.util.compute_evaluation_times(duration, step=0.1)

Compute times spaced at a fixed step across an interval.

Times start at zero.

Parameters:

• duration (float, positive) – Duration of the interval.

• step (float, positive) – Duration of each step.

Returns:

A tuple (n, times) representing the number of times and the time values themselves.

Return type:

tuple

rigeo.util.lift3(x)

Lift a 3-vector x such that A @ x = lift(x) @ vech(A) for symmetric A.

rigeo.util.lift6(x)

Lift a 6-vector V such that A @ V = lift(V) @ vech(A) for symmetric A.

rigeo.util.skew3(v)

Form a skew-symmetric matrix out of 3-dimensional vector v.

rigeo.util.skew6(V)

6D cross product matrix

rigeo.util.validation_rmse(Ys, ws, θ)

Compute root mean square wrench error on a validation set.

rigeo.util.vech(J)

Half-vectorize the inertia matrix