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): ...@@ -96,7 +96,7 @@ def getMergedPhases(seqs):
def computeRootYawAngleBetwwenConfigs(q0, q1): def computeRootYawAngleBetwwenConfigs(q0, q1):
quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5]) quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5])
quat1 = Quaternion(q1[6], q1[3], q1[4], q1[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().T.dot(quat1.matrix())))
#print ("q_prev : ",q0) #print ("q_prev : ",q0)
#print ("q : ",q1) #print ("q : ",q1)
#print ("v_angular = ",v_angular) #print ("v_angular = ",v_angular)
......
Markdown is supported
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