def getComConstraint(self, limbsCOMConstraints, exceptList=[]):
return get_com_constraint(self.fullBody,
self.sId,
self.cl.getConfigAtState(self.sId),
limbsCOMConstraints,
interm=self.isIntermediate,
exceptList=exceptList)
def test_ineq(stateid, constraints, n_samples = 10, color=[1,1,1,1.]): Kin = get_com_constraint(fullBody, stateid, configs[stateid], constraints, interm = False) #~ print "kin ", Kin #create box around current com fullBody.setCurrentConfig(configs[stateid]) com = fullBody.getCenterOfMass() bounds_c = flatten([[com[i]-1., com[i]+1.] for i in range(3)]) # arbitrary for i in range(n_samples): c = array([uniform(bounds_c[2*i], bounds_c[2*i+1]) for i in range(3)]) print "c: ", c if(Kin[0].dot(c)<=Kin[1]).all(): print "boundaries satisfied" createPtBox(viewer.client.gui, 0, c, 0.01, color)
def test(stateid=1,
path=False,
use_rand=False,
just_one_curve=False,
num_optim=0,
effector=False,
mu=0.5):
com_1 = __get_com(fullBody, configs[stateid])
com_2 = __get_com(fullBody, configs[stateid + 1])
createPtBox(viewer.client.gui, 0, com_1, 0.01, [0, 1, 1, 1.])
createPtBox(viewer.client.gui, 0, com_2, 0.01, [0, 1, 1, 1.])
data = gen_sequence_data_from_state(fullBody, stateid, configs, mu=mu)
c_bounds_1 = get_com_constraint(fullBody,
stateid,
configs[stateid],
limbsCOMConstraints,
interm=False)
c_bounds_mid = get_com_constraint(fullBody,
stateid,
configs[stateid],
limbsCOMConstraints,
interm=True)
c_bounds_2 = get_com_constraint(fullBody,
stateid,
configs[stateid + 1],
limbsCOMConstraints,
interm=False)
success, c_mid_1, c_mid_2 = solve_quasi_static(
data,
c_bounds=[c_bounds_1, c_bounds_2, c_bounds_mid],
use_rand=use_rand,
mu=mu)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
print "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ calling effector", effector
paths_ids = []
if path and success:
#~ fullBody.straightPath([c_mid_1[0].tolist(),c_mid_2[0].tolist()])
#~ fullBody.straightPath([c_mid_2[0].tolist(),com_2])
if just_one_curve:
print "just one curve"
bezier_0, curve = __Bezier(
[com_1, c_mid_1[0].tolist(), c_mid_2[0].tolist(), com_2])
createPtBox(viewer.client.gui, 0, c_mid_1[0].tolist(), 0.01,
[0, 1, 0, 1.])
createPtBox(viewer.client.gui, 0, c_mid_2[0].tolist(), 0.01,
[0, 1, 0, 1.])
partions = [0., 0.2, 0.8, 1.]
p0 = fullBody.generateCurveTrajParts(bezier_0, partions)
#testing intermediary configurations
print 'wtf', partions[1], " "
com_interm1 = curve(partions[1])
com_interm2 = curve(partions[2])
print "com_1", com_1
success_proj1 = project_com_colfree(
fullBody, stateid,
asarray((com_interm1).transpose()).tolist()[0])
success_proj2 = project_com_colfree(
fullBody, stateid + 1,
asarray((com_interm2).transpose()).tolist()[0])
if not success_proj1:
print "proj 1 failed"
return False, c_mid_1, c_mid_2, paths_ids
if not success_proj2:
print "proj 2 failed"
return False, c_mid_1, c_mid_2, paths_ids
print "p0", p0
#~ pp.displayPath(p0+1)
#~ pp.displayPath(p0+2)
ppl.displayPath(p0)
#~ pp.displayPath(p0+1)
#~ pp.displayPath(p0+2)
#~ pp.displayPath(p0+3)
if (effector):
print "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ calling effector"
paths_ids = [
int(el)
for el in fullBody.effectorRRT(stateid, p0 + 1, p0 +
2, p0 + 3, num_optim)
]
else:
paths_ids = [
int(el)
for el in fullBody.comRRTFromPos(stateid, p0 + 1, p0 +
2, p0 + 3, num_optim)
]
else:
success_proj1 = project_com_colfree(fullBody, stateid,
c_mid_1[0].tolist())
success_proj2 = project_com_colfree(fullBody, stateid + 1,
c_mid_2[0].tolist())
if not success_proj1:
print "proj 1 failed"
return False, c_mid_1, c_mid_2, paths_ids
if not success_proj2:
print "proj 2 failed"
return False, c_mid_1, c_mid_2, paths_ids
print "three curves"
bezier_0, curve = __Bezier([com_1, c_mid_1[0].tolist()],
end_acc=c_mid_1[1].tolist(),
end_vel=[0., 0., 0.])
bezier_1, curve = __Bezier(
[c_mid_1[0].tolist(), c_mid_2[0].tolist()],
end_acc=c_mid_2[1].tolist(),
init_acc=c_mid_1[1].tolist(),
init_vel=[0., 0., 0.],
end_vel=[0., 0., 0.])
bezier_2, curve = __Bezier([c_mid_2[0].tolist(), com_2],
init_acc=c_mid_2[1].tolist(),
init_vel=[0., 0., 0.])
p0 = fullBody.generateCurveTraj(bezier_0)
print "p0", p0
fullBody.generateCurveTraj(bezier_1)
fullBody.generateCurveTraj(bezier_2)
ppl.displayPath(p0)
ppl.displayPath(p0 + 1)
ppl.displayPath(p0 + 2)
paths_ids = [
int(el)
for el in fullBody.comRRTFromPos(stateid, p0, p0 + 1, p0 +
2, num_optim)
]
#~ paths_ids = []
global allpaths
allpaths += paths_ids[:-1]
#~ allpaths += [paths_ids[-1]]
#~ pp(paths_ids[-1])
#~ return success, paths_ids, c_mid_1, c_mid_2
return success, c_mid_1, c_mid_2, paths_ids
def test(stateid=1, path=False, use_rand=False, just_one_curve=False):
com_1 = __get_com(fullBody, configs[stateid])
com_2 = __get_com(fullBody, configs[stateid + 1])
data = gen_sequence_data_from_state(fullBody, stateid, configs)
c_bounds_1 = get_com_constraint(fullBody,
stateid,
configs[stateid],
limbsCOMConstraints,
interm=False)
c_bounds_mid = get_com_constraint(fullBody,
stateid,
configs[stateid],
limbsCOMConstraints,
interm=True)
c_bounds_2 = get_com_constraint(fullBody,
stateid,
configs[stateid + 1],
limbsCOMConstraints,
interm=False)
success, c_mid_1, c_mid_2 = solve_quasi_static(
data,
c_bounds=[c_bounds_1, c_bounds_2, c_bounds_mid],
use_rand=use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
paths_ids = []
if path and success:
#~ fullBody.straightPath([c_mid_1[0].tolist(),c_mid_2[0].tolist()])
#~ fullBody.straightPath([c_mid_2[0].tolist(),com_2])
if just_one_curve:
bezier_0 = __Bezier(
[com_1, c_mid_1[0].tolist(), c_mid_2[0].tolist(), com_2],
init_acc=[0.0, -20., 0.],
end_acc=[0.0, 0., 0.],
init_vel=[1., 0., 0.],
end_vel=[1., 0., 0.])
p0 = fullBody.generateCurveTrajParts(bezier_0, [0., 0.1, 0.9, 1.])
print "p0", p0
#~ pp.displayPath(p0+1)
#~ pp.displayPath(p0+2)
pp.displayPath(p0)
paths_ids = [
int(el)
for el in fullBody.comRRTFromPos(stateid, p0 + 1, p0 + 2, p0 +
3)
]
#~ paths_ids = [int(el) for el in fullBody.effectorRRT(stateid,p0+1,p0+2,p0+3)]
else:
bezier_0 = __Bezier([com_1, c_mid_1[0].tolist()],
end_acc=c_mid_1[1].tolist(),
end_vel=[0., 0., 0.])
bezier_1 = __Bezier([c_mid_1[0].tolist(), c_mid_2[0].tolist()],
end_acc=c_mid_2[1].tolist(),
init_acc=c_mid_1[1].tolist(),
init_vel=[0., 0., 0.],
end_vel=[0., 0., 0.])
bezier_2 = __Bezier([c_mid_2[0].tolist(), com_2],
init_acc=c_mid_2[1].tolist(),
init_vel=[0., 0., 0.])
p0 = fullBody.generateCurveTraj(bezier_0)
print "p0", p0
fullBody.generateCurveTraj(bezier_1)
fullBody.generateCurveTraj(bezier_2)
pp.displayPath(p0)
pp.displayPath(p0 + 1)
pp.displayPath(p0 + 2)
paths_ids = [
int(el)
for el in fullBody.comRRTFromPos(stateid, p0, p0 + 1, p0 + 2)
]
#~ paths_ids = []
global allpaths
allpaths += paths_ids[:-1]
#~ pp(paths_ids[-1])
#~ return success, paths_ids, c_mid_1, c_mid_2
return success, c_mid_1, c_mid_2, paths_ids
