gpdmink package¶
Submodules¶
gpdmink.CollisionDetection module¶
- class gpdmink.CollisionDetection.CollisionDetection(s_1, s_2)¶
Bases:
object- get_distance()¶
Get the calculated distance between the two shapes.
- Returns:
Distance value (positive if separated, negative if colliding)
- Return type:
float
- get_necessary_condition()¶
Get the necessary condition verification value.
- Returns:
Verification value for collinearity condition
- Return type:
float
- get_witness_points()¶
Get witness points on both shapes and the Minkowski sum.
- Returns:
Tuple containing witness points (x_s_1, x_s_2, x_mink)
- Return type:
tuple(numpy.ndarray, numpy.ndarray, numpy.ndarray)
- is_collide()¶
Check if the two shapes are colliding.
- Returns:
True if colliding, False otherwise
- Return type:
bool
- opt_callback(x)¶
Callback function for optimization process.
- Parameters:
x (numpy.ndarray) – Current optimization variable
- set_transform(linear_1=None, linear_2=None, nonlinear_1=None, nonlinear_2=None, tc_1=None, tc_2=None)¶
Set transformation parameters for both shapes.
- Parameters:
linear_1 (numpy.ndarray, optional) – Linear transformation matrix for the first shape
linear_2 (numpy.ndarray, optional) – Linear transformation matrix for the second shape
nonlinear_1 (dict, optional) – Nonlinear transformation parameters for the first shape
nonlinear_2 (dict, optional) – Nonlinear transformation parameters for the second shape
tc_1 (numpy.ndarray, optional) – Translation vector for the first shape
tc_2 (numpy.ndarray, optional) – Translation vector for the second shape
- solve()¶
Solve the collision detection problem.
Performs the complete optimization process to determine collision status, distance, and witness points between the two shapes.
gpdmink.Ellipsoid module¶
- class gpdmink.Ellipsoid.Ellipsoid(a, quat, tc)¶
Bases:
SuperQuadrics- get_closest_point_to_ellipsoid(eB)¶
Compute closest point to another ellipsoid. E. Rimon and S. P. Boyd, “Obstacle collision detection using best ellipsoid fit,” J. Intell. Robot. Syst., vol. 18, no. 2, pp. 105–126, 1997.
- Parameters:
eB (Ellipsoid) – Target ellipsoid object
- Returns:
Tuple containing closest point and related values
- Return type:
tuple
- get_gradient_from_cartesian(x)¶
Convert surface point to its gradient.
- Parameters:
x (numpy.ndarray) – Cartesian point
- Returns:
Gradient vector
- Return type:
numpy.ndarray
- get_gradient_from_hypersphere(u)¶
Convert hypersphere parameter to gradient.
- Parameters:
u (numpy.ndarray) – Hypersphere parameter
- Returns:
Gradient vector
- Return type:
numpy.ndarray
- get_hypersphere_from_gradient(m)¶
Convert gradient to hypersphere parameter.
- Parameters:
m (numpy.ndarray) – Gradient vector
- Returns:
Hypersphere parameter
- Return type:
numpy.ndarray
- get_implicit_function_canonical(x)¶
Compute implicit function value in canonical frame.
- Parameters:
x (numpy.ndarray) – Point in canonical frame
- Returns:
Implicit function value
- Return type:
float
- get_point_from_gradient(m)¶
Convert gradient to surface point in canonical frame.
- Parameters:
m (numpy.ndarray) – Gradient vector
- Returns:
Surface point
- Return type:
numpy.ndarray
- get_point_from_normal(n)¶
Convert normal vector to surface point.
- Parameters:
n (numpy.ndarray) – Normal vector
- Returns:
Surface point
- Return type:
numpy.ndarray
gpdmink.GPDMinkowskiSum module¶
- gpdmink.GPDMinkowskiSum.compute_phi(s_i, m_j)¶
Compute Phi(m_i) = |m_j| where m_j is the gradient of the other shape.
- Parameters:
s_i (object) – Geometric shape
m_j (numpy.ndarray) – Gradient of the other geometry
- Returns:
Phi(m_i) values
- Return type:
numpy.ndarray
- gpdmink.GPDMinkowskiSum.compute_sj_xform_nl(s_j, m_j, nl_j, deform_type='taper')¶
Compute surface point for s_j using nonlinear least-squares method.
- Parameters:
s_j (object) – Geometric shape object
m_j (numpy.ndarray) – Gradient directions
nl_j (dict) – Nonlinear transformation parameters
deform_type (str, optional) – Type of nonlinear deformation (default: “taper”)
- Returns:
Transformed surface points
- Return type:
numpy.ndarray
- gpdmink.GPDMinkowskiSum.func_lsq(m, s_j, nl_j, m_des, deform_type='taper')¶
Objective function for nonlinear least-squares optimization.
- Parameters:
m (numpy.ndarray) – Current optimization variable
s_j (object) – Geometric shape object
nl_j (dict) – Nonlinear transformation parameters
m_des (numpy.ndarray) – Desired gradient direction
deform_type (str, optional) – Type of nonlinear deformation (default: “taper”)
- Returns:
Error vector for optimization
- Return type:
numpy.ndarray
- gpdmink.GPDMinkowskiSum.get_contact_point(s_i, s_j, m_i, linear_xform_i, linear_xform_j, nl_xform_i, nl_xform_j=None, deform_type='taper')¶
Compute contact point given gradient direction.
- Parameters:
s_i (object) – First geometric shape (primary shape)
s_j (object) – Second geometric shape (secondary shape)
m_i (numpy.ndarray) – Initial gradient direction
linear_xform_i (numpy.ndarray) – Linear transformation for the first shape
linear_xform_j (numpy.ndarray) – Linear transformation for the second shape
nl_xform_i (dict) – Nonlinear transformation for the first shape
nl_xform_j (dict, optional) – Nonlinear transformation for the second shape
deform_type (str, optional) – Type of nonlinear deformation (default: “taper”)
- Returns:
Tuple containing contact points and transformed gradient
- Return type:
tuple(numpy.ndarray, numpy.ndarray, numpy.ndarray)
- gpdmink.GPDMinkowskiSum.gpdmink(s_1, s_2, m, linear_xform_1=None, linear_xform_2=None, nl_xform_1={}, nl_xform_2={}, deform_type='taper')¶
Compute the Minkowski sum of two bodies with given transformations.
- Parameters:
s_1 (object) – First geometric shape
s_2 (object) – Second geometric shape
m (numpy.ndarray) – Direction vector(s)
linear_xform_1 (numpy.ndarray, optional) – Linear transformation matrix for the first shape
linear_xform_2 (numpy.ndarray, optional) – Linear transformation matrix for the second shape
nl_xform_1 (dict, optional) – Nonlinear transformation parameters for the first shape
nl_xform_2 (dict, optional) – Nonlinear transformation parameters for the second shape
deform_type (str, optional) – Type of nonlinear deformation (default: “taper”)
- Returns:
Tuple containing Minkowski sum points and gradient vectors
- Return type:
tuple(numpy.ndarray, numpy.ndarray)
gpdmink.GeometryModel module¶
- class gpdmink.GeometryModel.GeometryModel(quat, tc, num=None)¶
Bases:
object- get_gradient_from_cartesian(x)¶
- get_gradient_from_direction(v)¶
- get_gradient_from_hypersphere(u)¶
- get_gradient_from_param(param, opt)¶
- get_gradient_from_spherical(theta)¶
- get_gradients()¶
- get_gradients_canonical()¶
Compute meshed surface gradients in canonical (local) frame :return: Surface gradients
- get_hypersphere_from_gradient(m)¶
- get_implicit_function(x)¶
Compute implicit function value at given point.
- Parameters:
x (numpy.ndarray) – Point in global coordinate space
- Returns:
Implicit function value
- Return type:
float
- get_implicit_function_canonical(x)¶
- get_point_from_gradient(m)¶
- get_point_from_normal(m)¶
- get_point_from_param(param, opt)¶
Convert parameterization to surface points.
- Parameters:
param (numpy.ndarray) – Parameter values
opt (str) – Parameterization type (“spherical”, “gradient”, “normal”)
- Returns:
Surface points in global coordinates
- Return type:
numpy.ndarray
- get_point_from_spherical(theta)¶
- get_points()¶
Get surface points in global coordinate frame.
- Returns:
Array of surface points
- Return type:
numpy.ndarray
- get_points_canonical()¶
Compute meshed surface points in canonical (local) frame :return: Surface points
- get_surf_gradients_canonical()¶
- get_surf_points()¶
Get surface mesh in global coordinate frame.
- Returns:
Mesh coordinates (xx, yy, zz)
- Return type:
tuple(numpy.ndarray, numpy.ndarray, numpy.ndarray)
- get_surf_points_canonical()¶
- get_volume()¶
- set_num_points(num)¶
Set number of surface points for meshing.
- Parameters:
num (list[int]) – Number of points in each parameter direction
- Returns:
Updated mesh grid parameters
- show(num, ax=None, color=None, alpha=None)¶
Display the geometry :param num: Parameter for surface points :param ax: Axis for the plot :param color: Color of the surface :return: Surface object and plot
gpdmink.NonlinearDeformation module¶
- gpdmink.NonlinearDeformation.get_gradient_parameterized_nonlinear_deformation(s, m, factor, deform_type)¶
Apply nonlinear deformation parameterized by gradients.
- Parameters:
s (object) – Geometric shape object
m (numpy.ndarray) – Gradient vectors
factor (float) – Deformation factor parameter
deform_type (str) – Type of deformation (“taper”, “bend”, “twist”)
- Returns:
Tuple containing deformed points and gradients
- Return type:
tuple(numpy.ndarray, numpy.ndarray)
- gpdmink.NonlinearDeformation.get_nonlinear_deformation(s, x, m, factor, deform_type)¶
Apply nonlinear deformation to surface points and gradients.
- Parameters:
s (object) – Geometric shape object
x (numpy.ndarray) – Surface points in canonical frame
m (numpy.ndarray) – Gradient vectors
factor (float) – Deformation factor parameter
deform_type (str) – Type of deformation (“taper”, “bend”, “twist”)
- Returns:
Tuple containing deformed points and gradients
- Return type:
tuple(numpy.ndarray, numpy.ndarray)
- gpdmink.NonlinearDeformation.get_nonlinear_deformation_reverse(s, x_deform, m_deform, factor, deform_type)¶
Reverse nonlinear deformation to recover original points and gradients.
- Parameters:
s (object) – Geometric shape object
x_deform (numpy.ndarray) – Deformed surface points
m_deform (numpy.ndarray) – Deformed gradient vectors
factor (float) – Deformation factor parameter
deform_type (str) – Type of deformation (“taper”, “bend”, “twist”)
- Returns:
Tuple containing original points and gradients
- Return type:
tuple(numpy.ndarray, numpy.ndarray)
gpdmink.SuperQuadrics module¶
- class gpdmink.SuperQuadrics.SuperQuadrics(a, eps, quat, tc, num=None)¶
Bases:
GeometryModel- get_gradient_from_cartesian(x)¶
Convert cartesian points to gradient vectors.
- Parameters:
x (numpy.ndarray) – Cartesian points
- Returns:
Gradient vectors
- Return type:
numpy.ndarray
- get_gradient_from_direction(v)¶
Convert direction vectors to gradient vectors.
- Parameters:
v (numpy.ndarray) – Direction vectors
- Returns:
Gradient vectors
- Return type:
numpy.ndarray
- get_gradient_from_hypersphere(u)¶
Convert hypersphere parameters to gradient vectors.
- Parameters:
u (numpy.ndarray) – Hypersphere parameters
- Returns:
Gradient vectors
- Return type:
numpy.ndarray
- get_gradient_from_spherical(theta)¶
Convert spherical coordinates to gradient vectors.
- Parameters:
theta (numpy.ndarray) – Spherical angles (eta, omega)
- Returns:
Gradient vectors
- Return type:
numpy.ndarray
- get_hypersphere_from_gradient(m)¶
Convert gradient vectors to hypersphere parameters.
- Parameters:
m (numpy.ndarray) – Gradient vectors
- Returns:
Hypersphere parameters
- Return type:
numpy.ndarray
- get_implicit_function_canonical(x)¶
Compute implicit function value in canonical frame.
- Parameters:
x (numpy.ndarray) – Point in canonical frame
- Returns:
Implicit function value
- Return type:
float
- get_point_from_gradient(m)¶
Convert gradient vectors to surface points.
- Parameters:
m (numpy.ndarray) – Gradient vectors
- Returns:
Surface points in canonical frame
- Return type:
numpy.ndarray
- get_point_from_normal(n)¶
Convert normal vectors to surface points.
- Parameters:
n (numpy.ndarray) – Normal vectors
- Returns:
Surface points in canonical frame
- Return type:
numpy.ndarray
- get_point_from_spherical(theta)¶
Convert spherical coordinates to surface points.
- Parameters:
theta (numpy.ndarray) – Spherical angles (eta, omega)
- Returns:
Surface points in canonical frame
- Return type:
numpy.ndarray
- get_surf_gradients_canonical()¶
Get surface gradients mesh in canonical frame.
- Returns:
Gradient mesh coordinates (m_x, m_y, m_z)
- Return type:
tuple(numpy.ndarray, numpy.ndarray, numpy.ndarray)
- get_surf_points_canonical()¶
Get surface mesh points in canonical frame.
- Returns:
Mesh coordinates (xx, yy, zz)
- Return type:
tuple(numpy.ndarray, numpy.ndarray, numpy.ndarray)
- get_volume()¶
Compute volume of the superquadric.
- Returns:
Volume value
- Return type:
float
gpdmink.util module¶
- gpdmink.util.eps_fun(x, epsilon)¶
Exponentation function with sign preservation.
- Parameters:
x (float or numpy.ndarray) – Input value
epsilon (float) – Exponent value
- Returns:
Signed exponentiation result
- Return type:
float or numpy.ndarray
- gpdmink.util.points2surf(surf_pts, num)¶
Convert point array to mesh grid coordinates.
- Parameters:
surf_pts (numpy.ndarray) – Array of 3D points
num (list[int]) – Grid dimensions [num_x, num_y]
- Returns:
Mesh coordinates (xx, yy, zz)
- Return type:
tuple(numpy.ndarray, numpy.ndarray, numpy.ndarray)
- gpdmink.util.rot2(th)¶
Create 2D rotation matrix.
- Parameters:
th (float) – Rotation angle in radians
- Returns:
2x2 rotation matrix
- Return type:
numpy.ndarray
- gpdmink.util.surf2points(xx, yy, zz)¶
Convert mesh grid coordinates to point array.
- Parameters:
xx (numpy.ndarray) – X coordinate mesh
yy (numpy.ndarray) – Y coordinate mesh
zz (numpy.ndarray) – Z coordinate mesh
- Returns:
Array of 3D points
- Return type:
numpy.ndarray