def check_kin_feasibility(s0Id, s1Id): res = fullBody.isReachableFromState(s0Id, s1Id, True) if not res[0]: print "Error : isReachable returned False !" raise Exception('Error : isReachable returned False !') r(configs[s0Id]) c0 = np.array(fullBody.getCenterOfMass()) c1 = np.array(res[1]) c2 = np.array(res[2]) r(configs[s1Id]) c3 = np.array(fullBody.getCenterOfMass()) s0 = State(fullBody, sId=s0Id) s1 = State(fullBody, sId=s1Id) s0_orig = State(s0.fullBody, q=s0.q(), limbsIncontact=s0.getLimbsInContact()) s1_orig = State(s1.fullBody, q=s1.q(), limbsIncontact=s1.getLimbsInContact()) moving_limb = s0.contactsVariations(s1) smid, successMid = StateHelper.removeContact(s0_orig, moving_limb[0]) if not successMid: return False successFeasible = True print "check first part" successFeasible = successFeasible and check_traj(s0_orig, c0, c1) print "check mid part" successFeasible = successFeasible and check_traj(smid, c1, c2) print "check last part" successFeasible = successFeasible and check_traj(s1_orig, c2, c3, True) return successFeasible
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 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 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.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 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 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 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 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 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 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 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
#~ d(0.07);e(0.01) i = 0 #~ d(0.09); e(0.01); configs = d(0.1) e() states = planToStates(fullBody, configs)[:-1] s = State(fullBody, q=states[-1].q(), limbsIncontact=states[-1].getLimbsInContact()) x = s.getCenterOfMass() x[0] -= 0.05 x[2] -= 0.1 fullBody.projectStateToCOM(s.sId, x, 10) r(s.q()) states += [s] #~ lc() #~ le = min(38, len(states)-10) lc() #~ path = [] #~ all_paths = [[],[]] #~ states = list(reversed(states)) #~ for i in range(0,4): #~ print i #~ onepath(i,0,nopt=1,effector=False,mu=1); #~ #~ #~ onepath2(states [2:-2],nopt=0,mu=1,effector=False)
pids = [] pids += [fullBody.isDynamicallyReachableFromState(si.sId,sE.sId)] pids += [fullBody.isDynamicallyReachableFromState(sE.sId,sfe.sId)] for pid in pids : if pid > 0: print "success" pp.displayPath(pid,color=r.color.blue) r.client.gui.setVisibility('path_'+str(pid)+'_root','ALWAYS_ON_TOP') else: print "fail." """ configs = [] configs += [si.q()] configs += [smid.q()] configs += [smid2.q()] configs += [sf2.q()] from planning.config import * from generate_contact_sequence import * beginState = si.sId endState = sf2.sId cs = generateContactSequence(fullBody,configs,beginState, endState,r)
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, fb.lArmId, pLHand, n, lockOtherJoints=True) assert success, "Unable to create contact for right hand" q_ref = state.q() fb.setReferenceConfig(q_ref) fb.setPostureWeights(fb.postureWeights_straff[::] + [0] * 6) v(q_ref) addPhaseFromConfig(fb, v, cs, q_ref, [fb.rLegId, fb.lLegId, fb.rArmId, fb.lArmId]) num_steps = 1 step_height = 0.22 step_width = 0.207 gait = [fb.rhand, fb.lhand, fb.rfoot, fb.lfoot] for i in range(num_steps): for eeName in gait:
0.008020752595769589, 0.0008315553354996808, 0, 0, 0, 0, 0, 0] s2 = State(fullBody,q=q2,limbsIncontact=[fullBody.lLegId,fullBody.rLegId,fullBody.rArmId]) v(s2.q()) com = fullBody.getCenterOfMass() comF = [-0.24796187, 0.74366786, 0.81803711] com=comF[::] com[0] = -0.2165 moveSphere("com",v,com) s2.projectToCOM(com) s3 = State(fullBody,q=s2.q(),limbsIncontact=[fullBody.lLegId,fullBody.rLegId,fullBody.rArmId]) s3.projectToCOM(com,10000) v(s3.q()) """ configs = [s0.q(), s1.q(), s2.q()] #configs = [s0.q(),s2.q()] v(configs[-1])
qs = configs fb = fullBody ttp = path_planner from bezier_traj import * init_bezier_traj(fb, r, pp, qs, limbsCOMConstraints) #~ AFTER loading obstacles configs = qs fullBody = fb tp = ttp from hpp.corbaserver.rbprm.rbprmstate import State from hpp.corbaserver.rbprm.state_alg import addNewContact, isContactReachable, closestTransform, removeContact, addNewContactIfReachable, projectToFeasibleCom s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId, rarmId]) q0 = s1.q()[:] #~ def test(p, n): #~ global s1 #~ t0 = time.clock() #~ a, success = addNewContact(s1,rLegId,p, n) #~ print "projecttion successfull ? ", success #~ t1 = time.clock() #~ val, com = isContactReachable(s1, rLegId,p, n, limbsCOMConstraints) #~ t2 = time.clock() #~ print 'is reachable ? ', val, com #~ if(val): #~ com[2]+=0.1 #~ if(success > 0): #~ print 'a > 0' #~ t3 = time.clock()
def test_contacts_6d(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] 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.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.assertFalse(state1.isBalanced()) self.assertEqual(com, state1.getCenterOfMass()) # create a new contact : n = [0, 0, 1] p = [0.15, -0.085, 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(state2.isBalanced()) p2, n2 = state2.getCenterOfContactForLimb(fullbody.rLegId) self.assertEqual(n, n2) for i in range(3): self.assertAlmostEqual(p[i], p2[i], 2) # try to replace a contact : p = [0.2, 0.085, 0.002] state3, success = StateHelper.addNewContact( state2, fullbody.lLegId, p, n) self.assertTrue(success) self.assertTrue(state3.isLimbInContact(fullbody.rLegId)) self.assertTrue(state3.isLimbInContact(fullbody.lLegId)) self.assertTrue(state3.isBalanced()) # try com projection: com_target = com[::] com_target[2] -= 0.08 success = state.projectToCOM(com_target) self.assertTrue(success) fullbody.setCurrentConfig(state.q()) com_state = fullbody.getCenterOfMass() self.assertEqual(com_state, state.getCenterOfMass()) for i in range(3): self.assertAlmostEqual(com_state[i], com_target[i], 4) # try lockOtherJoints: p = [0.1, -0.085, 0.002] state4, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, lockOtherJoints=True) self.assertTrue(success) self.assertTrue(state4.isLimbInContact(fullbody.rLegId)) self.assertTrue(state4.isLimbInContact(fullbody.lLegId)) self.assertTrue(state4.isBalanced()) for i in range(7, 13): self.assertAlmostEqual(state.q()[i], state4.q()[i]) for i in range(19, len(q)): self.assertAlmostEqual(state.q()[i], state4.q()[i]) # try with a rotation rot = [0.259, 0, 0, 0.966] state5, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, rotation=rot) self.assertTrue(success) self.assertTrue(state5.isLimbInContact(fullbody.rLegId)) self.assertTrue(state5.isLimbInContact(fullbody.lLegId)) self.assertTrue(state5.isBalanced()) fullbody.setCurrentConfig(state5.q()) rf_pose = fullbody.getJointPosition(fullbody.rfoot) for i in range(4): self.assertAlmostEqual(rf_pose[i + 3], rot[i], 3)
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 = State(fullBody, q=fullBody.referenceConfig, limbsIncontact=[fullBody.limbs_names[0]]) v(s.q()) #~ printRootPosition(rLegId, rfoot, nbSamples) #~ printRootPosition(lLegId, lfoot, nbSamples) #~ printRootPosition(rarmId, rHand, nbSamples) #~ printRootPosition(larmId, lHand, nbSamples) printFootPositionRelativeToOther(6000) print("successes ", success) print("fails ", fails) import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from plot_polytopes import plot_hull # ~ for effector, comData, pointsData in zip(effectors, compoints, points): # ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:], compoints[:1], points[:1]):
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.4497476605315172, #~ 0.586723585687863, #~ 0.7007986764501943, #~ 0.9992040068259673, #~ 0.009915872351221816, #~ -0.011224843288091112, #~ 0.0369733838268027, #~ -0.21440808741905482, #~ 0.5819742125714021, #~ -0.9540663648074178, #~ -0.444827228611522, #~ -0.2814234791010941, #~ 0.6601172395940408, #~ 0.05057907671648097, #~ 0.03165298930985974, #~ -0.8105513007687147, #~ 0.23456431240474232, #~ -0.6906832652645104, #~ 1.042653827624378] #~ q_init = [1.403034485826222, #~ -0.0680461187483179, #~ 0.698067114148141, #~ 0.9994021773454228, #~ 0.029333888870596826, #~ -0.01699029633207977, #~ 0.006792695452013932, #~ -0.2670125225627798, #~ 0.5760223974412482, #~ -0.6323579390738086, #~ -0.3824808801598628, #~ 0.12703791928011066, #~ 0.41203223392262833, #~ 0.3545295626633363, #~ 0.4930851685230413, #~ -0.7792461682735826, #~ 0.1068537396039267, #~ -0.7497836830029866, #~ 1.1890550989396227] #~ q_init = [1.7248956090657142, #~ 0.10332894313987395, #~ 0.7135677069780713, #~ 0.8053782375870717, #~ 0.009162336020200251, #~ 0.017984134378282467, #~ -0.5924175190948179, #~ -0.09343758894532768, #~ -0.13260962085227604, #~ -0.7334459269711523, #~ -0.11305498904738602, #~ -0.1883955962566395, #~ 0.8346448329401047, #~ 0.27875376841185046, #~ 0.654114442736956, #~ -0.495495198017057, #~ -0.22902533402342157, #~ -0.0733460650991134, #~ 0.6485831393133032] q_init = [ 2.0073705139562783, 0.10455451701731842, 0.6636982709906831, 0.8052290989552832, -0.010905503215389124, -0.028433849502961527, -0.5921812935222833, 0.32502766737262234, 0.15429143500131443, -0.8306825282286409, 0.3321441608555586, 0.00475050546222314, 0.9719582647982088, -0.14368646477113056, 0.8209795520480985, -0.349065850399, 0.32573656479055596, -0.7194630454730782, 0.349065850399 ] #~ 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, rarmId, larmId]) #~ q_goal = fullBody.generateContacts(s1.q(), [0,0,1]) #~ s1.setQ(q_goal) q0 = s1.q()[:] r(q0) return s1
qs = configs fb = fullBody ttp = path_planner from bezier_traj import * init_bezier_traj(fb, r, pp, qs, limbsCOMConstraints) #~ AFTER loading obstacles configs = qs fullBody = fb tp = ttp from hpp.corbaserver.rbprm.rbprmstate import State from hpp.corbaserver.rbprm.state_alg import addNewContact, isContactReachable, closestTransform, removeContact, addNewContactIfReachable, projectToFeasibleCom s1 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId]) q0 = s1.q()[:] #~ def test(p, n): #~ global s1 #~ t0 = time.clock() #~ a, success = addNewContact(s1,rLegId,p, n) #~ print "projecttion successfull ? ", success #~ t1 = time.clock() #~ val, com = isContactReachable(s1, rLegId,p, n, limbsCOMConstraints) #~ t2 = time.clock() #~ print 'is reachable ? ', val, com #~ if(val): #~ com[2]+=0.1 #~ if(success > 0): #~ print 'a > 0' #~ t3 = time.clock()