Commit 00b35723 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[tools] add a script to sample random orientation or to convert vector director to quaternion

parent fa26e41f
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):
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
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