Commit 3877a6ea authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Tools] surface_from_path: fix computation of angle difference

parent 58c0e1a9
......@@ -96,7 +96,7 @@ def getMergedPhases(seqs):
def computeRootYawAngleBetwwenConfigs(q0, q1):
quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5])
quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5])
v_angular = np.array(log3(quat0.matrix().dot(quat1.matrix())))
v_angular = np.array(log3(quat0.matrix()
#print ("q_prev : ",q0)
#print ("q : ",q1)
#print ("v_angular = ",v_angular)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment