def genFlat(): q = fullBody.shootRandomConfig() q[0:7] = zeroConf fullBody.setCurrentConfig(q) # v(q) posrf = fullBody.getJointPosition(rfoot)[:3] poslf = fullBody.getJointPosition(lfoot)[:3] s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbIds) s, succ = state_alg.addNewContact(s, rLegId, posrf, [0., 0., 1.], num_max_sample=0) if succ: s, succ = state_alg.addNewContact(s, lLegId, poslf, [0., 0., 1.], num_max_sample=0) if succ: succ = fullBody.isConfigValid( q)[0] and norm(array(posrf[:2]) - array(poslf[:2])) >= 0.3 # print("sid = ", s.sId) # if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1: if succ and norm(array(posrf) - array(poslf)) <= 0.1: v(s.q()) return s.q(), succ, s, [posrf, poslf]
def getContactsFromConfig(q, limbs=[Robot.rLegId, Robot.lLegId]): s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbs) rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId) lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId) return { Robot.rLegId: (rLegPn[0][0], rLegPn[1][0]), Robot.lLegId: (lLegPn[0][0], lLegPn[1][0]) }
def genFlat(init=False): q = fullBody.shootRandomConfig() if init: q = fullBody.referenceConfig[::] q[0:7] = zeroConf fullBody.setCurrentConfig(q) # v(q) positions = [fullBody.getJointPosition(foot)[:3] for foot in effectors] s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbIds) succ = True for effId, pos in zip(limbIds, positions): s, succ = state_alg.addNewContact(s, effId, pos, [0.0, 0.0, 1.0], num_max_sample=0) if not succ: break # posrf = fullBody.getJointPosition(rfoot)[:3] # poslf = fullBody.getJointPosition(lfoot)[:3] # print ("limbsIds ", limbIds) # s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbIds) # s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample= 0) # if succ: # s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample= 0) if succ: # ~ succ = fullBody.isConfigValid(q)[0] # and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3 succ = fullBody.isConfigValid(q)[0] # assert that in static equilibrium if succ: fullBody.setCurrentConfig(q) succ = staticEq(positions, fullBody.getCenterOfMass()) if not succ: v(q) if succ: succ = not pointInsideHull(positions) if not succ: print("************* contacts crossing", not succ) v(q) # if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1: # if succ and norm (array(posrf) - array(poslf) ) <= 0.1: v(s.q()) return s.q(), succ, s, positions
# ~ if rp[1] > -MARGIN_FEET_SIDE: # ~ compoints[j].append(rp) else: global fails fails += 1 # print(fullBody.isConfigValid(q)[1]) # for j in range(0,len(limbIds)): # f1=open('./'+str(limbIds[j])+'_com.erom', 'w+') # for p in points[j]: # f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n") # f1.close() s = rbprmstate.State(fullBody, q=fullBody.getCurrentConfig(), limbsIncontact=[fullBody.limbs_names[0]]) # printRootPosition(rLegId, rfoot, nbSamples) # printRootPosition(lLegId, lfoot, nbSamples) # printRootPosition(rarmId, rHand, nbSamples) # printRootPosition(larmId, lHand, nbSamples) printFootPositionRelativeToOther(6000) print("successes ", success) print("fails ", fails) # ~ for effector, comData, pointsData in zip(effectors, compoints, points): # ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:], # compoints[:1], points[:1]): for effector, limbId, comData, pointsData in zip(effectors, limbIds, compoints, points):
#~ com[2] -= 0.25 #~ while(not succCom and ite < 11): while(not succCom and ite < 17): succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample) com[2] -= 0.05 ite += 1 print("COM?", succCom) v(sres.q()) return sres q = fullBody.getCurrentConfig() q[:2] = allfeetpos[0][:2].tolist() v(q) s = rbprmstate.State(fullBody, q = q, limbsIncontact = []) all_states = [s] sprev = s idfirst = 2 for i in range(1, len(pb["phaseData"])): #~ com = (coms[i-i] ).tolist() com = (coms[i-1] ).tolist() #~ if i > 2: #~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() snew = gen_state(sprev, i , com, num_max_sample = 20, first = i == idfirst ) all_states += [snew] sprev = snew com2 = coms[i].tolist() snew2 = gen_state(snew, i , com2, num_max_sample = 20 ) all_states += [snew2]
if projectCOM: #print "config before projecting to com q1=",sres.q() successCOM = projectCoMInSupportPolygon(sres) if not successCOM: # is it really an issue ? print( "Unable to project CoM in the center of the support polygone") v(sres.q()) return sres q = fullBody.getCurrentConfig() q[:3] = [-2.69, 0.21, 0.649702] v(q) s = rbprmstate.State(fullBody, q=q, limbsIncontact=[fullBody.lLegId, fullBody.rLegId]) idfirst = 2 coms[0] = array(s.getCenterOfMass()) #~ coms[0] = coms[1] #~ coms[1] = array(s.getCenterOfMass()) def normal(phase): s = phase["S"][0] n = cross(s[:, 1] - s[:, 0], s[:, 2] - s[:, 0]) n /= norm(n) if n[2] < 0.: for i in range(3): n[i] = -n[i]