Commit 8bf700fc authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[tools] add script sample_root_config

parent 2d34863b
import random
random.seed()
def generate_random_conf_without_orientation(rbprmBuilder,bounds,v=None):
q = rbprmBuilder.getCurrentConfig ();
while True:
pos = [0,0,0]
for i in range(3):
pos[i] = random.uniform(bounds[i*2],bounds[i*2+1])
quat = [0,0,0.7071, 0.7071]
q[0:3] = pos
q[3:7] = quat
q[8] = 0.006761
q[-6:-3] = [0,0,0]
if v :
v(q)
status,message = rbprmBuilder.isConfigValid(q)
if status:
quat = [0,0, 0.382911, 0.923785]
q[3:7] = quat
status,message = rbprmBuilder.isConfigValid(q)
if status:
quat = [0,0, -0.382911, 0.923785]
q[3:7] = quat
status,message = rbprmBuilder.isConfigValid(q)
if status:
quat = [0,0,0, 1.]
q[3:7] = quat
status,message = rbprmBuilder.isConfigValid(q)
if status:
return q
else:
print "Getting invalid config. try again."
print message
def generate_random_conf_with_orientation(rbprmBuilder,bounds,v_init = 0.1,v=None):
q = rbprmBuilder.getCurrentConfig ();
while True:
pos = [0,0,0]
for i in range(3):
pos[i] = random.uniform(bounds[i*2],bounds[i*2+1])
angle = random.uniform(0.,2.*np.pi)
quat = np.array([0,0,np.sin(angle/2), np.cos(angle/2)])
q[0:3] = pos
q[3:7] = quat
q[8] = 0.006761
q[-6:-3] = [v_init*np.cos(angle),v_init*np.sin(angle),0]
if v :
v(q)
status,message = rbprmBuilder.isConfigValid(q)
if status:
return q
else:
print "Getting invalid config. try again."
print message
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