def bases_to_Quat(xBasis, yBasis, zBasis):
""" For new bases expressed in the old, find the quaternion that rotates the old onto the new """
# FIXME , THIS IS WRONG , COMPARE TO 'principal_rot_Quat' !! FIXME , THIS IS WRONG , COMPARE TO 'principal_rot_Quat' !!
Q = [vec_unit(xBasis), vec_unit(yBasis), vec_unit(zBasis)]
[[Qxx, Qxy, Qxz], [Qyx, Qyy, Qyz], [Qzx, Qzy, Qzz]] = Q
t = Qxx + Qyy + Qzz
r = sqrt(1 + t)
w = 0.5 * r
x = copysign(0.5 * sqrt(1 + Qxx - Qyy - Qzz), (Qzy - Qyz))
y = copysign(0.5 * sqrt(1 - Qxx + Qyy - Qzz), (Qxz - Qzx))
z = copysign(0.5 * sqrt(1 - Qxx - Qyy + Qzz), (Qyx - Qxy))
temp = Quaternion(w, x, y, z)
temp.normalize()
return temp
def blend_in_steps(aPose, bPose, linStep=0.02, rotStep=0.087):
""" Blend from 'aPose' to 'bPose' (inclusive) along a straight line and the shortest turn, not exceeding 'linStep' or 'rotStep' """
poseDiff = Pose.diff_from_to(
aPose, bPose) # Obtain the difference between the two poses
dist = vec_mag(poseDiff.position)
k, rot = poseDiff.orientation.get_k_rot(
) # Get the axis-angle of the orientation difference
if dist / linStep >= rot / rotStep: # If more linear steps are needed than rotational steps
distances = incr_max_step(0, dist, linStep)
angles = np.linspace(0, rot, len(distances))
else: # else more rotational steps are needed than linear steps
angles = incr_max_step(0, rot, rotStep)
distances = np.linspace(0, dist, len(angles))
direction = vec_unit(poseDiff.position)
positions = [
np.add(aPose.position, np.multiply(direction, disp))
for disp in distances
]
orientations = [
Quaternion.compose_rots(aPose.orientation,
Quaternion.k_rot_to_Quat(k, blendRot))
for blendRot in angles
]
rtnPoses = []
for index in xrange(
len(positions)): # For each position-orientation pair
rtnPoses.append(
Pose(positions[index],
orientations[index])) # construct a Pose and append
return rtnPoses # All done, return
def __init__(self, pK, pTheta, pMinLimit=None, pMaxLimit=None):
super(Rotation, self).set_k_rot(pK, pTheta)
self.k = vec_unit(pK)
self.theta = pTheta
self.hasLimits = pMinLimit != None or pMaxLimit != None
self.minLimit = pMinLimit
self.maxLimit = pMaxLimit
def shortest_btn_vecs(v1, v2):
""" Return the Quaternion representing the shortest rotation from vector 'v1' to vector 'v2' """
# print "DEBUG , Got vectors: " , v1 , v2
# print "DEBUG , Crossed vectors: " , np.cross( v1 , v2 )
# print "DEBUG , Crossed unit vec:" , vec_unit( np.cross( v1 , v2 ) )
# print "DEBUG , Angle between: " , vec_angle_between( v1 , v2 )
angle = vec_angle_between(v1, v2)
if eq(angle, 0.0):
return Quaternion.no_turn_quat()
elif eq(angle, pi):
# If we are making a pi turn, then just pick any axis perpendicular to the opposing vectors and make the turn
# The axis that is picked will result in different poses
return Quaternion.k_rot_to_Quat(vec_unit(np.cross(v1, [1, 0, 0])),
angle)
else:
return Quaternion.k_rot_to_Quat(vec_unit(np.cross(v1, v2)), angle)
def set_k_rot(self, k, rot):
""" Set the quaternion components based on an axis-angle (radians) specification of rotation """
k = vec_unit(k)
self.sclr = cos(rot / 2)
self._vctr = np.array([ k[0] * sin(rot/2) , \
k[1] * sin(rot/2) , \
k[2] * sin(rot/2) ])
def ray_intersects_triangle(rayOrigin, rayVector, CCWtriCoords):
""" Möller–Trumbore intersection algorithm for whether a ray intersects a triangle in R3 , 'rayVector' can be any length
return: [ bool: Ray intersects triangle , R3: Line intersection point , float: signed distance from ray origin to intersection ] """
# Note: If there is no line intersection , then the second and third elements will be populated with NaN
vertex0 = CCWtriCoords[0]
vertex1 = CCWtriCoords[1]
vertex2 = CCWtriCoords[2]
edge1 = np.subtract(vertex1, vertex0)
edge2 = np.subtract(vertex2, vertex0)
h = np.cross(rayVector, edge2) # rayVector.crossProduct(edge2);
a = np.dot(edge1, h) # edge1.dotProduct(h);
if (a > -EPSILON) and (a < EPSILON):
return [False, vec_NaN(3), float('NaN')
] # No intersection , Return False and in invalid vec
f = 1.0 / a
s = np.subtract(rayOrigin, vertex0)
u = f * np.dot(s, h) # u = f * (s.dotProduct(h));
if (u < 0.0) or (u > 1.0):
return [False, vec_NaN(3), float('NaN')
] # No intersection , Return False and in invalid vec
q = np.cross(s, edge1) # s.crossProduct(edge1);
v = f * np.dot(rayVector, q) # f * rayVector.dotProduct(q);
if (v < 0.0) or (u + v > 1.0):
return [False, vec_NaN(3), float('NaN')
] # No intersection , Return False and in invalid vec
# At this stage we can compute t to find out where the intersection point is on the line.
t = f * np.dot(edge2, q) # f * edge2.dotProduct(q);
outIntersectionPoint = np.add(rayOrigin, np.multiply(rayVector, t))
intersectionMag = np.dot(np.subtract(outIntersectionPoint, rayOrigin),
vec_unit(rayVector))
if t > EPSILON: # ray intersection
return [True, outIntersectionPoint, intersectionMag]
else: # This means that there is a line intersection but not a ray intersection.
return [False, outIntersectionPoint, intersectionMag]
def circle_arc_3D(axis, center, radius, beginMeasureVec, theta, N):
""" Return points on a circular arc about 'axis' at 'radius' , beginning at 'beginMeasureVec' and turning 'theta' through 'N' points """
thetaList = np.linspace(0, theta, N)
k = vec_unit(axis)
rtnList = []
# 1. Project the theta = 0 vector onto the circle plane
measrVec = vec_unit(vec_proj_to_plane(beginMeasureVec, axis))
# 2. For each theta
for theta_i in thetaList:
# 3. Create a rotation matrix for the rotation
R = rot_matx_ang_axs(theta_i, k)
# 4. Apply rotation to the measure vector && 5. Scale the vector to the radius && 6. Add to the center && 7. Append to the list
rtnList.append(np.add(center, np.multiply(np.dot(R, measrVec),
radius)))
# 8. Return
return rtnList
def principal_rot_Quat(basis1, basis2, basis3):
""" For new 'bases' expressed in the old, find the quaternion that rotates the old onto the new """
C = [vec_unit(basis1),
vec_unit(basis2),
vec_unit(basis3)
] # Asssumed normalization enough times that I'll just do it
inside = 0.5 * (C[0][0] + C[1][1] + C[2][2] - 1)
# print "DEBUG , 0.5 * ( C[0][0] + C[1][1] + C[2][2] - 1 ):" , inside
if abs(inside) > 1:
# print "DEBUG , Applying correction!" , inside , "-->" ,
inside = -decimal_part(inside)
# print inside
Phi = acos(inside)
e1 = 0.5 * sin(Phi) * (C[1][2] - C[2][1])
e2 = 0.5 * sin(Phi) * (C[2][0] - C[0][2])
e3 = 0.5 * sin(Phi) * (C[0][1] - C[1][0])
return Quaternion.k_rot_to_Quat([e1, e2, e3], Phi)
def step_from_to(aPose, bPose, linStep, rotStep):
""" Return a linear step and a rotational step from 'aPose' towards 'bPose' for use with Jacobian trajectories """
poseDiff = Pose.diff_from_to(
aPose, bPose) # Obtain the difference between the two poses
dist = vec_mag(poseDiff.position)
vecStep = vec_unit(poseDiff.position
) * linStep if dist > linStep else poseDiff.position
vecStep = np.multiply(vec_unit(poseDiff.position),
linStep) if dist > linStep else poseDiff.position
k, rot = poseDiff.orientation.get_k_rot(
) # Get the axis-angle of the orientation difference
rotStep = rotStep if rotStep > rot else rot
trnStep = np.multiply(k, rotStep)
# [ delta x , delta y , delta z , k0 * d theta , k1 * d theta , k2 * d theta ]
return [
vecStep[0], vecStep[1], vecStep[2], trnStep[0], trnStep[1],
trnStep[2]
]
def k_rot_to_Quat(k, rot):
""" Return a quaternion representing a rotation of 'rot' about 'k'
k : 3D vector that is the axis of rotation
rot : rotation angle expressed in radians """
k = vec_unit(k)
return Quaternion( cos(rot/2), \
k[0] * sin(rot/2), \
k[1] * sin(rot/2), \
k[2] * sin(rot/2) )
def principal_rotation_test():
""" Test of 'Quaternion.principal_rot_Quat' """
newBasisX = vec_unit([0.0, -1.0, 1.0])
newBasisY = vec_unit([1.0, 0.0, 0.0])
newBasisZ = vec_unit([0.0, 1.0, 1.0])
if check_orthonormal(newBasisX, newBasisY, newBasisZ):
print "New bases are orthonormal"
principalQuat = Quaternion.principal_rot_Quat(newBasisX, newBasisY,
newBasisZ)
testBasisX = principalQuat.apply_to([1, 0, 0])
testBasisY = principalQuat.apply_to([0, 1, 0])
testBasisZ = principalQuat.apply_to([0, 0, 1])
print "New basis X", newBasisX, "and transformed basis X", testBasisX, "are equal:", vec_eq(
newBasisX, testBasisX)
print "New basis Y", newBasisY, "and transformed basis Y", testBasisY, "are equal:", vec_eq(
newBasisY, testBasisY)
print "New basis Z", newBasisZ, "and transformed basis Z", testBasisZ, "are equal:", vec_eq(
newBasisZ, testBasisZ)
else:
print "New bases are not orthonormal"
def single_step_from_to(aPose, bPose):
""" Return the linear and rotational difference in the form [ delta x , delta y , delta z , k0 * d theta , k1 * d theta , k2 * d theta ] """
poseDiff = Pose.diff_from_to(
aPose, bPose) # Obtain the difference between the two poses
vecStep = vec_unit(poseDiff.position)
k, rot = poseDiff.orientation.get_k_rot(
) # Get the axis-angle of the orientation difference
trnStep = np.multiply(k, rot)
# [ delta x , delta y , delta z , k0 * d theta , k1 * d theta , k2 * d theta ]
return [
vecStep[0], vecStep[1], vecStep[2], trnStep[0], trnStep[1],
trnStep[2]
]
def vec_proj_to_plane(vec, planeNorm):
""" Return a vector that is the projection of 'vec' onto a plane with the normal vector 'planeNorm', using numpy """
# URL, projection of a vector onto a plane: http://www.euclideanspace.com/maths/geometry/elements/plane/lineOnPlane/
# NOTE: This function assumes 'vec' and 'planeNorm' are expressed in the same bases
if vec_eq(vec, [0, 0, 0]):
return [0, 0, 0]
else:
projDir = vec_unit(np.cross(
np.cross(planeNorm, vec),
planeNorm)) # direction of the projected vector, normalize
projMag = vec_proj(
vec, projDir) # magnitude of the vector in the projected direction
return np.multiply(
projDir, projMag
) # scale the unit direction vector to the calculated magnitude and return
def set_k(self, pK):
""" Update the vector and compute the quaternion
This function assumes that pK is a Vector """
self.k = vec_unit(pK)
self.set_k_rot(self.k, self.theta) # compute the quaternion