primp.util package
Submodules
primp.util.benchmark_util module
Benchmark for ProMP in learning trajectory distribution from demonstration
@author: Sipu Ruan
- primp.util.benchmark_util.evaluate_desired_position(traj_res, x_desired, t_via)
Evaluate similarity between result and desired pose.
- Parameters:
traj_res – Resulting (Generated) trajectory
x_desired – The desired position
t_via – The time step of via point
- Returns:
The distance from the desired position
- primp.util.benchmark_util.evaluate_traj_distribution(traj_res, traj_init)
Evaluate the generated trajectory distribution
- Parameters:
traj_res – The resulting (generated) trajectory
traj_init – The initial trajectory
- Returns:
The distance between the generated and initial trajectory mean
- primp.util.benchmark_util.load_dataset_param(dataset_name)
Load the parameters for datasets
- Parameters:
dataset_name – The name of the dataset: “panda_arm”, “lasa_handwriting/pose_data”
- Returns:
The type list of the demonstration
primp.util.data_parser module
primp.util.finite_difference module
- primp.util.finite_difference.df_vect(x, f, k, n_r, dim)
Uses high order finite difference method to compute numerical derivatives along arbitrary dimension of an array. Uses Frohnberg’s method to compute coefficents.
- Parameters:
x – Elements in domain of known function values, should be a 1-D array
f – Known function values, f(x)
k – Order of the derivative
n_r – About half of the window size, i.e. how many points in front and behind to be considered when computing the derivaitve. Practically, n_r = 3 or n_r = 4 works well
dim – Dimension to compute the derivative along (usually temporal or spatial dimension)
- Return df:
Computed finite difference vector
- primp.util.finite_difference.fd_coefficient(k, xbar, x)
Compute coefficients for finite difference approximation for the derivative of order k at xbar based on grid values at points in x.
This function returns a row vector c of dimension 1 by n, where n=length(x), containing coefficients to approximate u^{(k)}(xbar), the k’th derivative of u evaluated at xbar, based on n values of u at x(1), x(2), … x(n).
If U is a column vector containing u(x) at these n points, then c*U will give the approximation to u^{(k)}(xbar).
Note for k=0 this can be used to evaluate the interpolating polynomial itself.
Requires length(x) > k. Usually the elements x(i) are monotonically increasing and x(1) <= xbar <= x(n), but neither condition is required. The x values need not be equally spaced but must be distinct.
This program should give the same results as fdcoeffV.m, but for large values of n is much more stable numerically.
Based on the program “weights” in B. Fornberg, “Calculation of weights in finite difference formulas”, SIAM Review 40 (1998), pp. 685-691. From http://www.amath.washington.edu/~rjl/fdmbook/ (2007)
- Parameters:
k – Order of derivative
xbar – Element in domain of known function values
x – The rest elements in domain of known function values
- Returns:
Coefficient
primp.util.interp_se3_trajectory module
- primp.util.interp_se3_trajectory.interp_se3_trajectory_svd(t0, x_traj, t)
Compute interpolation along an SE(3) trajectory using SVD
Reference: “An SVD-Based Projection Method for Interpolation on SE(3). Calin Belta, Vijay Kumar. 2002.”
- Parameters:
t0 – Original time sequence
x_traj – Original SE(3) trajectory
t – Desired time sequence
- Returns:
Interpolated SE(3) trajectory
@author: Thomas Mitchel, 2018 @maintainer: Sipu Ruan, 2022
primp.util.plot_util module
primp.util.se3_distribution module
- primp.util.se3_distribution.get_covariance(g, mu=None, group_name='SE')
Compute covariance of a set of SE(3)
- Parameters:
g – The set of SE(3) elements
mu – Computed mean or None
group_name – Name of the group
- Returns:
Covariance of the set
- primp.util.se3_distribution.get_mean(g, group_name='SE')
Compute mean of a set of SE(3)
- Parameters:
g – The set of SE(3) elements
group_name – Name of the group
- Returns:
Mean of the set
primp.util.se3_util module
- primp.util.se3_util.adjoint_group(g=array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), group_name='SE')
Adjoint operator for groups SE(3) or PCG(3)
- Parameters:
g – Group element in homogeneous matrix form
group_name – “SE” (default), “PCG”
- Returns:
Adjoint operator
- primp.util.se3_util.expm_se3(xi=array([0., 0., 0., 0., 0., 0.]))
Closed-form exponential from se(3) to SE(3)
- Parameters:
xi – Element in se(3) of the form [omega, v]
- Returns:
Homogeneous transformation matrix for SE(3)
- primp.util.se3_util.expm_so3(w=array([0., 0., 0.]))
Closed-form exponential for so(3) into SO(3)
- Parameters:
w – Vector for rotation
- Returns:
Rotation matrix
- primp.util.se3_util.get_exp_coord(g=array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), group_name='SE')
Get exponential coordinates from Lie group to Lie algebra
- Parameters:
g – Homogeneous transformation matrix for poses
group_name – “SE” (default), “PCG”
- Returns:
Exponential coordinates of the form [omega, v]
See also: logm_se3, logm_so3
- primp.util.se3_util.get_exp_mapping(exp_coord=array([0., 0., 0., 0., 0., 0.]), group_name='SE')
Get the exponential mapping from Lie algebra (exponential coordinates) to Lie group
- Parameters:
exp_coord – Exponential coordinates of the form [omega, v]
group_name – “SE” (default), “PCG”
- Returns:
Homogeneous transformation matrix
See also: expm_se3, expm_so3
- primp.util.se3_util.homo2pose_axang(g=array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]))
Convert SE(3) homogeneous matrix to pose vector, rotation is represented by axis-angle
- Parameters:
g – SE(3) element
- Returns:
Pose vector, axis-angle order: [axis-x, axis-y, axis-z, angle]
- primp.util.se3_util.homo2pose_quat(g=array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]))
Convert SE(3) homogeneous matrix to pose vector, rotation is represented by unit Quaternion
- Parameters:
g – SE(3) element
- Returns:
Pose vector, quaternion order: [x, y, z, w]
- primp.util.se3_util.jacobian_inv_so3_l(w=array([0., 0., 0.]))
Closed-form left inverse Jacobian of SO(3)
- Parameters:
w – Vector for the rotation
- Returns:
Closed-form left inverse Jacobian of SO(3)
- primp.util.se3_util.jacobian_so3_l(w=array([0., 0., 0.]))
Closed-form left Jacobian of SO(3)
- Parameters:
w – Vector for the rotation
- Returns:
Closed-form left Jacobian of SO(3)
- primp.util.se3_util.logm_se3(g=array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]))
Closed-form logarithm from SE(3) to se(3)
- Parameters:
g – Homogeneous transformation matrix for SE(3)
- Returns:
Element in se(3) in the matrix form
- primp.util.se3_util.logm_so3(rot=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]))
Closed-form logarithm from SO(3) to so(3)
- Parameters:
rot – Rotation matrix as element in SO(3)
- Returns:
Element in so(3) in the matrix form
- primp.util.se3_util.norm_se3(g)
Weighted norm of SE(3)
- Parameters:
g – an SE(3) element
- Returns:
Weighted norm
- primp.util.se3_util.skew(w=array([0., 0., 0.]))
Construct skew-symmetric matrix from vector
- Parameters:
w – Vector of size 3
- Returns:
Skew-symmetric matrix
- primp.util.se3_util.vex(w_hat=array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]]))
Extract vector form from skew-symmetric matrix
- Parameters:
w_hat – Skew-symmetric matrix
- Returns:
Vector form