Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
P : array-like, shape (n_steps, 7)
Sequence of poses represented by positions and quaternions in the
order (x, y, z, w, vx, vy, vz) for each step
Returns
-------
H : array-like, shape (n_steps, 4, 4)
Sequence of poses represented by homogeneous matrices
"""
n_steps = len(P)
H = np.empty((n_steps, 4, 4))
H[:, :3, 3] = P[:, :3]
H[:, 3, :3] = 0.0
H[:, 3, 3] = 1.0
for t in range(n_steps):
H[t, :3, :3] = matrix_from_quaternion(P[t, 3:])
return H
def transform_from_pq(pq):
"""Conversion from position and quaternion to homogeneous matrix.
Parameters
----------
pq : array-like, shape (7,)
Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)
Returns
-------
A2B : array-like, shape (4, 4)
Transform from frame A to frame B
"""
pq = check_pq(pq)
return transform_from(matrix_from_quaternion(pq[3:]), pq[:3])
Each component of the translation will be sampled from
:math:`\mathcal{N}(\mu=0, \sigma=1)`.
Parameters
----------
random_state : np.random.RandomState, optional (default: random seed 0)
Random number generator
Returns
-------
A2B : array-like, shape (4, 4)
Random transform from frame A to frame B
"""
q = random_quaternion(random_state)
R = matrix_from_quaternion(q)
p = random_vector(random_state, n=3)
return transform_from(R=R, p=p)