From 00b3572354c1bea1efb7df6066bae3fbf212f4b3 Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Thu, 27 Jun 2019 10:07:54 +0200
Subject: [PATCH] [tools] add a script to sample random orientation or to
 convert vector director to quaternion

---
 script/tools/sampleRotation.py | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)
 create mode 100644 script/tools/sampleRotation.py

diff --git a/script/tools/sampleRotation.py b/script/tools/sampleRotation.py
new file mode 100644
index 00000000..31d23d4c
--- /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
-- 
GitLab