diff --git a/script/tools/sampleRotation.py b/script/tools/sampleRotation.py new file mode 100644 index 0000000000000000000000000000000000000000..31d23d4cd5158767bb703c9e8fe4ebe7df3924b2 --- /dev/null +++ b/script/tools/sampleRotation.py @@ -0,0 +1,22 @@ +import random +import numpy as np +from pinocchio import Quaternion + +def Vector3FromAlpha(alpha): + return np.matrix([np.sin(alpha), -np.cos(alpha), 0.]).T + +def quatFromAlpha(alpha): + vx = np.matrix([1,0,0]).T + v = Vector3FromAlpha(alpha) + return Quaternion.FromTwoVectors(vx,v) + +def sampleRotationForConfig(alphaBounds,q,vPredef): + random.seed() + alpha = random.uniform(alphaBounds[0],alphaBounds[1]) + v = Vector3FromAlpha(alpha) + quat = quatFromAlpha(alpha) + q[3:7] = quat.coeffs().T.tolist()[0] + # set velocity to match this orientation : + q[-6] = v[0,0]*vPredef + q[-5] = v[1,0]*vPredef + return q