def __init__(self, normal=None, point=None, equation=None):
if equation is None:
self.point = np.array(point)
self.normal = tr.unit_vector(normal)
self.offset = -np.dot(self.normal, self.point)
else:
norm = tr.vector_norm(equation[:3])
self.normal = tr.unit_vector(equation[:3])
self.offset = equation[3] / norm
self.point = -self.offset*self.normal
# Plane origin
self.origin = np.array(self.point)
def look_rotation(forward, up=[0, 0, 1]):
forward = tr.unit_vector(forward)
right = tr.unit_vector(np.cross(up, forward))
up = np.cross(forward, right)
m00 = right[0]
m01 = right[1]
m02 = right[2]
m10 = up[0]
m11 = up[1]
m12 = up[2]
m20 = forward[0]
m21 = forward[1]
m22 = forward[2]
num8 = (m00 + m11) + m22
quaternion = Quaternion()
if (num8 > 0.0):
num = np.sqrt(num8 + 1)
quaternion[0] = num * 0.5
num = 0.5 / num
quaternion[1] = (m12 - m21) * num
quaternion[2] = (m20 - m02) * num
quaternion[3] = (m01 - m10) * num
return quaternion
if ((m00 >= m11) and (m00 >= m22)):
num7 = np.sqrt(((1 + m00) - m11) - m22)
num4 = 0.5 / num7
quaternion[1] = 0.5 * num7
quaternion[2] = (m01 + m10) * num4
quaternion[3] = (m02 + m20) * num4
quaternion[0] = (m12 - m21) * num4
return quaternion
if (m11 > m22):
num6 = np.sqrt(((1 + m11) - m00) - m22)
num3 = 0.5 / num6
quaternion[1] = (m10 + m01) * num3
quaternion[2] = 0.5 * num6
quaternion[3] = (m21 + m12) * num3
quaternion[0] = (m20 - m02) * num3
return quaternion
num5 = np.sqrt(((1 + m22) - m00) - m11)
num2 = 0.5 / num5
quaternion[1] = (m20 + m02) * num2
quaternion[2] = (m21 + m12) * num2
quaternion[3] = 0.5 * num5
quaternion[0] = (m01 - m10) * num2
return quaternion
def fit_plane_optimize(points, seed=None):
"""
Fits a plane to a point cloud using C{scipy.optimize.leastsq}
@type points: list
@param points: list of points
@rtype: np.array
@return: normalized normal vector of the plane
"""
if seed is None:
seed = np.zeros(4)
seed[:3] = tr.unit_vector(tr.random_vector(3))
# Optimization functions
def f_min(X,p):
normal = p[0:3]
d = p[3]
result = ((normal*X.T).sum(axis=1) + d) / np.linalg.norm(normal)
return result
def residuals(params, signal, X):
return f_min(X, params)
# Optimize
XYZ = np.array(points).T
p0 = np.array(seed)
sol = scipy.optimize.leastsq(residuals, p0, args=(None, XYZ))[0]
nn = np.linalg.norm(sol[:3])
sol /= nn
seed_error = (f_min(XYZ, p0)**2).sum()
fit_error = (f_min(XYZ, sol)**2).sum()
return sol, seed_error, fit_error
def rotation_matrix_from_axes(newaxis, oldaxis=Z_AXIS, point=None):
"""
Returns the rotation matrix that aligns two vectors.
@type newaxis: np.array
@param newaxis: The goal axis
@type oldaxis: np.array
@param oldaxis: The initial axis
@rtype: array, shape (4,4)
@return: The resulting rotation matrix that aligns the old to the new axis.
"""
oldaxis = tr.unit_vector(oldaxis)
newaxis = tr.unit_vector(newaxis)
c = np.dot(oldaxis, newaxis)
angle = np.arccos(c)
if np.isclose(c, -1.0) or np.allclose(newaxis, oldaxis):
v = perpendicular_vector(newaxis)
else:
v = tr.unit_vector(np.cross(oldaxis, newaxis))
return tr.rotation_matrix(angle, v, point)
def face_towards(target_position, current_position, up_vector=[0, 0, 1]):
"""
Compute orientation to "face towards" a point in space
given the current position and the initial vector representing "up"
default is z as is the outward direction from the end-effector
"""
direction = tr.unit_vector(target_position - current_position)
cmd_rot = look_rotation(direction, up=up_vector)
target_quat = tr.vector_from_pyquaternion(cmd_rot)
return np.concatenate([current_position, target_quat])
def perpendicular_vector(v):
"""
Finds an arbitrary perpendicular vector to B{v}
@type v: np.array
@param v: The input vector
@rtype: np.array
@return: The perpendicular vector.
"""
v = tr.unit_vector(v)
if np.allclose(v[:2], np.zeros(2)):
if np.isclose(v[2], 0.):
# v is (0, 0, 0)
raise ValueError('zero vector')
# v is (0, 0, Z)
return Y_AXIS
return np.array([-v[1], v[0], 0])