Commit 88412508 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

update eroms for com

parent 2c7c1b96
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
......@@ -71,10 +71,10 @@ def printRootPosition(limbId, effectorName, nbSamples):
q = fullBody.getSample(limbId, i)
fullBody.setCurrentConfig(q) #setConfiguration matching sample
qEffector = fullBody.getJointPosition(effectorName)
q0 = quat.Quaternion(qEffector[3:7])
rot = q0.toRotationMatrix().transpose() #compute rotation matrix world -> local
p = rot.dot(qEffector[0:3]) #(0,0,0) coordinate expressed in effector frame
f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")
#~ q0 = quat.Quaternion(qEffector[3:7])
#~ rot = q0.toRotationMatrix().transpose() #compute rotation matrix world -> local
p = qEffector[0:3] #(0,0,0) coordinate expressed in effector frame
f1.write(str(-p[0]) + "," + str(-p[1]) + "," + str(-p[2]) + "\n")
f1.close()
printRootPosition(rLegId, rfoot, nbSamples)
......
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