Revision history [back]

def  pose_from_vector3D(waypoint):
#http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
pose= Pose()
pose.position.x = waypoint[0]
pose.position.y = waypoint[1]
pose.position.z = waypoint[2] 
#calculating the half-way vector.
u = [1,0,0]
norm = linalg.norm(waypoint[3:])
v = asarray(waypoint[3:])/norm 
if (array_equal(u, v)):
    pose.orientation.w = 1
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 0
elif (array_equal(u, negative(v))):
    pose.orientation.w = 0
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 1
else:
    half = [u[0]+v[0], u[1]+v[1], u[2]+v[2]]
    pose.orientation.w = dot(u, half)
    temp = cross(u, half)
    pose.orientation.x = temp[0]
    pose.orientation.y = temp[1]
    pose.orientation.z = temp[2]
norm = math.sqrt(pose.orientation.x*pose.orientation.x + pose.orientation.y*pose.orientation.y + 
    pose.orientation.z*pose.orientation.z + pose.orientation.w*pose.orientation.w)
if norm == 0:
    norm = 1
pose.orientation.x /= norm
pose.orientation.y /= norm
pose.orientation.z /= norm
pose.orientation.w /= norm
return pose

I realized my mistake, I didn't normalize the quaternion. This code works!