def setupHrp2(): switch_context(0) q_init = [ 1.465951314830228, -0.9176187491454572, 0.556996332024304, 0.8949373222781749, 0.004991393277543945, 0.09328595707567328, 0.43630265344046976, -0.03547067445226664, 0.6709217094864276, 0.2612513403690041, 0.018790826902884042, 1.0472, -0.12716727062306263, 0.014121758070711707, 0.0349066, -0.005336299638361007, 0.09555844824139348, -0.15100592820137693, 1.0472, -0.6564341232652954, -0.14124478581057368, 0.0349066, 0.006442792051943824, 0.0976416359755596, 0.048557325328327426, 0.025298798438044116, 0.5088603837394758, 0.09277155854142789, 0.06056704285485918, -0.40397983120647746, -0.3442592958713712, -0.5709648671805531, -0.5813136986745363, -0.8406045693532259, 0.5508411880503161, 0.06566162355557537, 0.5817029063967701 ] r(q_init) #~ q_init[1] = -1.05 s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId]) #~ s1 = State(fullBody,q=q_init, limbsIncontact = []) q0 = s1.q()[:] r(q0) fullBody.setRefConfig(q0) return s1
def setupSpidey(): switch_context(1) q_0 = fullBody.getCurrentConfig() r(q_0) q_init = [ 1.8644820749752133, -0.05907835662004786, 0.36244227429714687, 0.796622257271051, -0.01123057286487888, -0.04124810131291858, 0.6029638858104035, -0.03547067445226691, 0.6709217094864277, 0.26125134036900377, 0.018790826902884056, 1.0472, -0.12716727062306263, -0.4194108294469009, -0.5282737768672232, 0.6483375025026266, -0.36743071075802436, -1.3354344044074755, 0.8731280507393882, -0.6564341232652954, -0.14124478581057368, 0.0349066, 1.0782285414916801, -1.2054219129402555, 0.6375403038065977 ] #~ q_init[0:7] = tp.q_init[0:7] #~ q_init[1] += 1.05 #~ q_0[2] = 1.1 s1 = State(fullBody, q=q_init, limbsIncontact=['r1', 'r3', 'l3']) #~ q_goal = fullBody.generateContacts(s1.q(), [0,0,1]) #~ s1.setQ(q_goal) q0 = s1.q()[:] r(q0) return s1
def setupHrp2(): switch_context(0) q_init = [ 0.8728886120451924, -0.8101285717720692, 0.9831109373101212, 0.9907480839443233, 0.04566101822960869, 6.080932806073498e-05, -0.12780180701818292, 0.0, 0.0, 0.700898687394426, 0.2643416012373336, 1.0472, -0.04797431922382627, -0.013276945338995495, -0.1474914458541713, -0.002729397895803521, 0.0417439495415805, -0.0959524864077709, 1.0472, -0.7688894346658527, -0.12266497430814384, -0.12242722827560858, 0.0031594766930237773, 0.0443584738891719, 0.015217658052039982, 0.6567108816211705, 0.3519940697164601, -0.3096847583571286, 0.01661099687285411, 0.6843606576274044, -0.33788734650437907, 0.3649203666768749, -0.3603776588496829, 0.1551833621146152, -0.001045294421723865, 0.23087743645539907, 0.28930745214380776 ] r(q_init) #~ q_init[1] = -1.05 s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId]) #~ s1 = State(fullBody,q=q_init, limbsIncontact = []) q0 = s1.q()[:] r(q0) fullBody.setRefConfig(q0) return s1
def setupSpidey(): switch_context(1) q_0 = fullBody.getCurrentConfig() r(q_0) q_init = [ 1.5118243482119733, 0.1515578829873285, 0.48976931477040697, 0.9915264175846067, -0.018363722321990766, -0.068500987098437, -0.10883819045195667, 0.5302771431871597, 0.5441203380575321, 0.2782134865083353, -0.06010765345021368, -0.8900451738361533, 0.276589550853067, -0.34163470046205424, -0.2710174858398316, 0.06526434276718414, -0.43672572993852576, -0.9562437680837305, 0.9084315210259988, -0.6088478466994447, -0.13563212555983598, 0.0349066, 0.9658270566778959, -0.6362233126122913, 1.5 ] #~ q_init[0:7] = tp.q_init[0:7] #~ q_init[1] += 1.05 #~ q_0[2] = 1.1 s1 = State(fullBody, q=q_init, limbsIncontact=['r1', 'r3', 'l3', 'l2']) #~ q_goal = fullBody.generateContacts(s1.q(), [0,0,1]) #~ s1.setQ(q_goal) q0 = s1.q()[:] r(q0) return s1
def setupSpidey(): switch_context(1) q_0 = fullBody.getCurrentConfig(); r(q_0) q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7] q_init[1] += 1.05 #~ q_0[2] = 1.1 s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId]) q0 = s1.q()[:] r(q0) return s1
def setupHrp2(): switch_context(0) q_init = fullBody.getCurrentConfig() r(q_init) #~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] #~ q_init [0:3] = [-0.05, -0.82, 0.50]; q_init = [ -0.05, -0.82, 0.65, 1.0, 0.0, 0.0, 0.0, # Free flyer 0-6 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 ] r(q_init) #~ q_init[1] = -1.05 s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId]) #~ s1 = State(fullBody,q=q_init, limbsIncontact = []) q0 = s1.q()[:] r(q0) fullBody.setRefConfig(q0) return s1
def projectMidFeet(fullBody, q, limbs): fullBody.setCurrentConfig(q) s = State(fullBody, q=q, limbsIncontact=limbs) com = np.array(fullBody.getJointPosition(rfoot)[0:3]) com += np.array(fullBody.getJointPosition(lfoot)[0:3]) com /= 2. com[2] = fullBody.getCenterOfMass()[2] successProj = s.projectToCOM(com.tolist()) if successProj and fullBody.isConfigValid( s.q())[0] and fullBody.isConfigBalanced(s.q(), limbs, 5): return s else: return None
def setupHrp2(): switch_context(0) q_init = [ 0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 ]; r (q_init) #~ q_init[1] = -1.05 s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId]) q0 = s1.q()[:] r(q0) return s1
def _interpolateState(ps, fullBody, stepsize, pathId, robustnessTreshold=0, filterStates=False, testReachability=True, quasiStatic=False, erasePreviousStates=True): if (filterStates): filt = 1 else: filt = 0 #discretize path totalLength = ps.pathLength(pathId) configsPlan = [] step = 0. configSize = fullBody.getConfigSize() - len(ps.configAtParam(pathId, 0.)) z = [0. for _ in range(configSize)] while step < totalLength: configsPlan += [ps.configAtParam(pathId, step) + z[:]] step += stepsize configsPlan += [ps.configAtParam(pathId, totalLength) + z[:]] configs = fullBody.clientRbprm.rbprm.interpolateConfigs( configsPlan, robustnessTreshold, filt, testReachability, quasiStatic, erasePreviousStates) firstStateId = fullBody.clientRbprm.rbprm.getNumStates() - len(configs) return [ State(fullBody, i) for i in range(firstStateId, firstStateId + len(configs)) ], configs
def generateConfigFromPhase(fb, phase, projectCOM=False): fb.usePosturalTaskContactCreation(False) effectorsInContact = phase.effectorsInContact() contacts = [ ] # contacts should contains the limb names, not the effector names list_effector = list(fb.dict_limb_joint.values()) for eeName in effectorsInContact: contacts += [ list(fb.dict_limb_joint.keys())[list_effector.index(eeName)] ] #q = phase.q_init.tolist() # should be the correct config for the previous phase, if used only from high level helper methods q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic ! root = computeCenterOfSupportPolygonFromPhase( phase, fb.DEFAULT_COM_HEIGHT).tolist() q[0:2] = root[0:2] q[2] += root[2] - fb.DEFAULT_COM_HEIGHT quat = Quaternion(phase.root_t.evaluateAsSE3(phase.timeInitial).rotation) q[3:7] = [quat.x, quat.y, quat.z, quat.w] # create state in fullBody : state = State(fb, q=q, limbsIncontact=contacts) # check if q is consistent with the contact placement in the phase : fb.setCurrentConfig(q) for limbId in contacts: eeName = fb.dict_limb_joint[limbId] placement_fb = SE3FromConfig(fb.getJointPosition(eeName)) placement_phase = phase.contactPatch(eeName).placement if placement_fb != placement_phase: # add a threshold instead of 0 ? how ? # need to project the new contact : placement = phase.contactPatch(eeName).placement p = placement.translation.tolist() n = computeContactNormal(placement).tolist() state, success = StateHelper.addNewContact(state, limbId, p, n, 1000) if not success: print( "Cannot project the configuration to contact, for effector : ", eeName) return state.q() if projectCOM: success = projectCoMInSupportPolygon(state) if not success: print( "cannot project com to the middle of the support polygon." ) phase.q_init = np.array(state.q()) return state.q()
def perturbateContactNormal(fb, state_id, epsilon=1e-2): """ Add a small variation (+- epsilon) to the contact normals of the given state :param fb: :param state_id: :param epsilon: :return: the new state ID, -1 if fail """ state = State(fb, state_id) for name in state.getLimbsInContact(): p, n = state.getCenterOfContactForLimb(name) n[2] += uniform(-epsilon, epsilon) n = np.array(n) state, success = StateHelper.addNewContact(state, name, p, n.tolist()) if not success: return -1 return state.sId
def sampleRandomTransitionFlatFloor(fullBody, limbsInContact, movingLimb, z=0): random.seed() success = False it_tot = 0 while not success and it_tot < 1000: it_tot += 1 s0 = sampleRandomStateFlatFloor(fullBody, limbsInContact, z) success, s1 = sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb, z) if not success: print "Timeout for generation of feasible transition" sys.exit(1) # recreate the states to assure the continuity of the index in fullBody : state0 = State(fullBody, q=s0.q(), limbsIncontact=s0.getLimbsInContact()) state1 = State(fullBody, q=s1.q(), limbsIncontact=s1.getLimbsInContact()) return state0, state1
def check_projection_path(s0, s1, c, dc, ddc, t_per_phase): kin_valid = True stab_valid = True moving_limb = s0.contactsVariations(s1) s0_orig = State(s0.fullBody, q=s0.q(), limbsIncontact=s0.getLimbsInContact()) s1_orig = State(s1.fullBody, q=s1.q(), limbsIncontact=s1.getLimbsInContact()) if len(moving_limb) > 1: print("Too many contact variation between adjacent states") return False, False if len(moving_limb) == 0: print("No contact between adjacent states") return True, True #print "test for phase 0 :" for t in t_per_phase[0]: if kin_valid and not check_projection(s0, c, t): kin_valid = False if stab_valid and not check_stability(s0_orig, c, dc, ddc, t): stab_valid = False smid, success = StateHelper.removeContact(s0, moving_limb[0]) smid_orig, success_orig = StateHelper.removeContact( s0_orig, moving_limb[0]) if not (success and success_orig): print("Error in creation of intermediate state") return False, False #print "test for phase 1 :" for t in t_per_phase[1]: if kin_valid and not check_projection(smid, c, t): kin_valid = False if stab_valid and not check_stability(smid_orig, c, dc, ddc, t): stab_valid = False #print "test for phase 2" # go in reverse order for p2, it's easier for the projection : for i in range(1, len(t_per_phase[2]) + 1): t = t_per_phase[2][-i] if kin_valid and not check_projection(s1, c, t): kin_valid = False if stab_valid and not check_stability(s1_orig, c, dc, ddc, t): stab_valid = False return kin_valid, stab_valid
def sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb, z): it = 0 success = False n = [0, 0, 1] vz = np.matrix(n).T while not success and it < 10000: it += 1 # sample a random position for movingLimb and try to project s0 to this position qr = fullBody.shootRandomConfig() q1 = s0.q()[::] q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[ limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact) fullBody.setCurrentConfig(s1.q()) p = fullBody.getJointPosition( fullBody.dict_limb_joint[movingLimb])[0:3] p[0] = random.uniform(eff_x_range[0], eff_x_range[1]) p[1] = random.uniform(eff_y_range[0], eff_y_range[1]) p[2] = z s1, success = StateHelper.addNewContact(s1, movingLimb, p, n) if success: """ # force root orientation : (align current z axis with vertical) quat_1 = Quaternion(s1.q()[6],s1.q()[3],s1.q()[4],s1.q()[5]) v_1 = quat_1.matrix() * vz align = Quaternion.FromTwoVectors(v_1,vz) rot = align * quat_1 q_root = s1.q()[0:7] q_root[3:7] = rot.coeffs().T.tolist()[0] """ root = SE3.Identity() root.translation = np.matrix(s1.q()[0:3]).T root = sampleRotationAlongZ(root) success = s1.projectToRoot(se3ToXYZQUATtuple(root)) # check if new state is in static equilibrium if success: # check stability success = fullBody.isStateBalanced(s1.sId, 3) if success: success = projectInKinConstraints(fullBody, s1) # check if transition is feasible according to CROC if success: #success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0) success = fullBody.isReachableFromState(s0.sId, s1.sId) return success, s1
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 = State(fullBody, q=q, limbsIncontact=limbIds) succ = True for effId, pos in zip(limbIds, positions): s, succ = StateHelper.addNewContact(s, effId, pos, [0., 0., 1.], num_max_sample=0) if not succ: break # ~ posrf = fullBody.getJointPosition(rfoot)[:3] # ~ poslf = fullBody.getJointPosition(lfoot)[:3] # ~ print ("limbsIds ", limbIds) # ~ s = State(fullBody, q = q, limbsIncontact = limbIds) # ~ s, succ = StateHelper.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0) # ~ if succ: # ~ s, succ = StateHelper.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
def check_traj_valid(ps, fullBody, s0_, s1_, pIds, dt=0.001): # create copy of original states : s0 = State(fullBody, q=s0_.q(), limbsIncontact=s0_.getLimbsInContact()) s1 = State(fullBody, q=s1_.q(), limbsIncontact=s1_.getLimbsInContact()) fullBody.setStaticStability(False) Ts = [ ps.pathLength(int(pIds[1])), ps.pathLength(int(pIds[2])), ps.pathLength(int(pIds[3])) ] t_array = genTimeArray(Ts, dt) #print "Time array : ",t_array c_qp = fullBody.getPathAsBezier(int(pIds[0])) dc_qp = c_qp.compute_derivate(1) ddc_qp = dc_qp.compute_derivate(1) return check_projection_path(s0, s1, c_qp, dc_qp, ddc_qp, t_array)
def removeContact(state, limbName, projectToCOM = False, friction = 0.6): sId = state.cl.removeContact(state.sId, limbName) if(sId != -1): s = State(state.fullBody, sId = sId) if projectToCOM: return s, projectToFeasibleCom(s, ddc =[0.,0.,0.], max_num_samples = 10, friction = friction) else: return s, True return state, False
def check_muscod_traj(fullBody, cs, s0_, s1_): if cs.size() != 3: print("Contact sequence must be of size 3 (one step)") return False, False kin_valid = True stab_valid = True stab_valid_discretized = True # create copy of original states : s0 = State(fullBody, q=s0_.q(), limbsIncontact=s0_.getLimbsInContact()) s1 = State(fullBody, q=s1_.q(), limbsIncontact=s1_.getLimbsInContact()) moving_limb = s0.contactsVariations(s1) smid, success = StateHelper.removeContact(s0, moving_limb[0]) if not success: print("Error in creation of intermediate state") return False, False phases = [] c_array = [] dc_array = [] ddc_array = [] t_array = [] states = [s0, smid, s1] states_copy = [ State(fullBody, q=s0_.q(), limbsIncontact=s0_.getLimbsInContact()), State(fullBody, q=smid.q(), limbsIncontact=smid.getLimbsInContact()), State(fullBody, q=s1_.q(), limbsIncontact=s1_.getLimbsInContact()) ] for k in range(3): phases += [cs.contact_phases[k]] state_traj = stdVecToMatrix(phases[k].state_trajectory).transpose() control_traj = stdVecToMatrix(phases[k].control_trajectory).transpose() c_array = state_traj[:, :3] dc_array = state_traj[:, 3:6] ddc_array = control_traj[:, :3] t_array = stdVecToMatrix(phases[k].time_trajectory).transpose() for i in range(len(c_array)): #if kin_valid and not states[k].projectToCOM(c_array[i].tolist()[0],500) : # kin_valid = False q = states_copy[k].q() q[-6:-3] = dc_array[i].tolist()[0] q[-3:] = ddc_array[i].tolist()[0] if stab_valid_discretized and not states_copy[ k].fullBody.isConfigBalanced( q, states_copy[k].getLimbsInContact(), CoM=c_array[i].tolist()[0]): stab_valid_discretized = False for i in range(int(len(c_array) / 100)): q = states_copy[k].q() q[-6:-3] = dc_array[i * 100].tolist()[0] q[-3:] = ddc_array[i * 100].tolist()[0] if stab_valid and not states_copy[k].fullBody.isConfigBalanced( q, states_copy[k].getLimbsInContact(), CoM=c_array[i * 100].tolist()[0]): stab_valid = False return kin_valid, stab_valid, stab_valid_discretized
def addNewContact(state, limbName, p, n, num_max_sample=0, lockOtherJoints=False): sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample, lockOtherJoints) if (sId != -1): return State(state.fullBody, sId=sId), True return state, False
def check_contact_plan(ps, r, pp, fullBody, idBegin, idEnd): fullBody.client.basic.robot.setExtraConfigSpaceBounds( [-0, 0, -0, 0, -0, 0, 0, 0, 0, 0, 0, 0]) fullBody.setStaticStability(False) validPlan = True for id_state in range(idBegin, idEnd - 1): print("#### check for transition between state " + str(id_state) + " and " + str(id_state + 1)) s0 = State(fullBody, sId=id_state) s1 = State(fullBody, sId=id_state + 1) # make a copy of each state because they are modified by the projection s0_ = State(fullBody, q=s0.q(), limbsIncontact=s0.getLimbsInContact()) s1_ = State(fullBody, q=s1.q(), limbsIncontact=s1.getLimbsInContact()) valid = check_one_transition(ps, fullBody, s0_, s1_, r, pp) validPlan = validPlan and valid global curves_initGuess global timings_initGuess return validPlan, curves_initGuess, timings_initGuess
def load_save(fname): f = open(fname, "r+") all_data = load(f) f.close() sc(0) global states states = [] #~ for i in range(0,len(all_data[0]),2): #~ print "q",all_data[0][i] #~ print "lic",all_data[0][i+1] #~ states+=[State(fullBody,q=all_data[0][i], limbsIncontact = all_data[0][i+1]) ] for _, s in enumerate(all_data[0]): states += [State(fullBody, q=s[0], limbsIncontact=s[1])] r(states[0].q()) sc(1) global states states = [] #~ for i in range(0,len(all_data[1]),2): #~ states+=[State(fullBody,q=all_data[1][i], limbsIncontact = all_data[1][i+1]) ] for _, s in enumerate(all_data[1]): states += [State(fullBody, q=s[0], limbsIncontact=s[1])] r(states[0].q())
def generateConfigFromPhase(fb, phase, projectCOM=False): fb.usePosturalTaskContactCreation(False) contacts = getActiveContactLimbs(phase, fb) #q = phase.reference_configurations[0].T.tolist()[0] # should be the correct config for the previous phase, if used only from high level helper methods q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic ! root = computeCenterOfSupportPolygonFromPhase(phase, fb).T.tolist()[0] q[0:2] = root[0:2] q[2] += root[2] - fb.DEFAULT_COM_HEIGHT quat = Quaternion( rootOrientationFromFeetPlacement(phase, None)[0].rotation) q[3:7] = [quat.x, quat.y, quat.z, quat.w] # create state in fullBody : state = State(fb, q=q, limbsIncontact=contacts) # check if q is consistent with the contact placement in the phase : fb.setCurrentConfig(q) for limbId in contacts: eeName = fb.dict_limb_joint[limbId] placement_fb = SE3FromConfig(fb.getJointPosition(eeName)) placement_phase = JointPlacementForEffector(phase, eeName, fb) if placement_fb != placement_phase: # add a threshold instead of 0 ? how ? # need to project the new contact : placement = getContactPlacement(phase, eeName, fb) p = placement.translation.T.tolist()[0] n = computeContactNormal(placement).T.tolist()[0] state, success = StateHelper.addNewContact(state, limbId, p, n, 1000) if not success: print "Cannot project the configuration to contact, for effector : ", eeName return state.q() if projectCOM: success = projectCoMInSupportPolygon(state) if not success: print "cannot project com to the middle of the support polygon." phase.reference_configurations[0] = np.matrix(state.q()).T return state.q()
def test_contacts_3d(self): with ServerManager('hpp-rbprm-server'): fullbody = Robot() fullbody.client.robot.setDimensionExtraConfigSpace(6) fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10]) fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10]) fullbody.loadAllLimbs("static", nbSamples=10000) q = fullbody.referenceConfig[::] + [0] * 6 fullbody.setCurrentConfig(q) com = fullbody.getCenterOfMass() contacts = [fullbody.rLegId, fullbody.lLegId, fullbody.rArmId, fullbody.lArmId] state = State(fullbody, q=q, limbsIncontact=contacts) self.assertTrue(state.isBalanced()) self.assertTrue(state.isValid()[0]) self.assertTrue(state.isLimbInContact(fullbody.rLegId)) self.assertTrue(state.isLimbInContact(fullbody.lLegId)) self.assertTrue(state.isLimbInContact(fullbody.rArmId)) self.assertTrue(state.isLimbInContact(fullbody.lArmId)) self.assertEqual(com, state.getCenterOfMass()) # remove rf contact : state1, success = StateHelper.removeContact(state, fullbody.rLegId) self.assertTrue(success) self.assertFalse(state1.isLimbInContact(fullbody.rLegId)) self.assertTrue(state1.isLimbInContact(fullbody.lLegId)) self.assertTrue(state.isLimbInContact(fullbody.rArmId)) self.assertTrue(state.isLimbInContact(fullbody.lArmId)) self.assertEqual(com, state1.getCenterOfMass()) # create a new contact : n = [0, 0, 1] p = [0.45, -0.2, 0.002] state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n) self.assertTrue(success) self.assertTrue(state2.isLimbInContact(fullbody.rLegId)) self.assertTrue(state2.isLimbInContact(fullbody.lLegId)) self.assertTrue(state.isLimbInContact(fullbody.rArmId)) self.assertTrue(state.isLimbInContact(fullbody.lArmId)) self.assertTrue(state2.isBalanced()) p_real, n_real = state2.getCenterOfContactForLimb(fullbody.rLegId) self.assertEqual(n, n_real)
def genStates(fullbody, limbs, num_samples=1000, coplanar=True): q_0 = fullBody.getCurrentConfig() #~ fullBody.getSampleConfig() qs = [] states = [] for _ in range(num_samples): if (coplanar): q = fullBody.generateGroundContact(limbs) else: q = _genbalance(fullBody, limbs) if len(q) > 1: qs.append(q) s = State(fullbody, q=q, limbsIncontact=limbs) states.append(s) return states
def createRandomState(fullBody, limbsInContact): extraDof = int(fullBody.client.robot.getDimensionExtraConfigSpace()) q0 = fullBody.referenceConfig[::] if extraDof > 0: q0 += [0] * extraDof qr = fullBody.shootRandomConfig() root = SE3.Identity() root.translation = np.matrix(qr[0:3]).T # sample random orientation along z : root = sampleRotationAlongZ(root) q0[0:7] = se3ToXYZQUATtuple(root) # apply random config to legs (FIXME : ID hardcoded for Talos) q0[7:19] = qr[7:19] fullBody.setCurrentConfig(q0) s0 = State(fullBody, q=q0, limbsIncontact=limbsInContact) return s0
def computeIntermediateState(sfrom, sto): sid = sfrom.cl.computeIntermediary(sfrom.sId, sto.sId) return State(sfrom.fullBody, sid, False)
def planToStates(fullBody, configs): states = [] for i, _ in enumerate(configs): states += [State(fullBody, i)] return states
""" tStart = time.time() configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 1, filterStates = False) tInterpolateConfigs = time.time() - tStart print "number of configs :", len(configsFull) """ q_init[0] += 0.05 createSphere('s',r) n = [0,0,1] p = [0,0.1,0] q_init[-6:-3] = [0.2,0,0] q_goal[-6:-3] = [0.1,0,0] sf = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId]) si = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId]) n = [0.0, -0.42261828000211843, 0.9063077785212101] p = [0.775, 0.22, -0.019] moveSphere('s',r,p) smid,success = StateHelper.addNewContact(si,lLegId,p,n,100) assert(success) smid2,success = StateHelper.addNewContact(sf,lLegId,p,n,100) assert(success) r(smid.q()) sf2 = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
0.0, 0.0 ] + [0] * 6 q_ref[0:2] = [-3., -1.7575] v(q_ref) # move root position q_ref[2] -= 0.015 fb.setReferenceConfig(q_ref) fb.setPostureWeights(fb.postureWeights_straff[::] + [0] * 6) # create hand contacts : pRH = SE3.Identity() pRH = rotatePlacement(pRH, 'y', -0.8115781021773631) n = computeContactNormal(pRH).T.tolist()[0] state = State(fb, q=q_ref, limbsIncontact=[fb.rLegId, fb.lLegId]) pLHand = [-2.75, -1.235, 1.02] pRHand = [-2.75, -2.28, 1.02] """ from tools.display_tools import * createSphere('s',v) moveSphere('s',v,pLHand) """ state, success = StateHelper.addNewContact(state, fb.rArmId, pRHand, n, lockOtherJoints=True) assert success, "Unable to create contact for right hand" state, success = StateHelper.addNewContact(state,
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 import tools.display_tools as disp disp.createSphere("c",v,color = v.color.green) v.addLandmark("c",0.1) q = fullBody.getCurrentConfig() v(q) s = State(fullBody, q = q, limbsIncontact = [fullBody.lLegId, fullBody.rLegId]) idfirst = 2 #coms[0] = array(s.getCenterOfMass()) coms[0][2] -= 0.02 def normal(phase): s = phase["S"][0] i = 0 n = np.zeros(3) while i < s.shape[1]-3 and norm(n) < 1e-4 : n = cross(s[:,i+1] - s[:,i+0], s[:,i+2] - s[:,i+0]) i+=1 if norm(n) < 1e-4: return Z_AXIS