# copy extraconfig for start and init configurations q_init[configSize:configSize+3] = dir_init[::] q_init[configSize+3:configSize+6] = acc_init[::] q_goal[configSize:configSize+3] = dir_goal[::] q_goal[configSize+3:configSize+6] = acc_goal[::] # FIXME : test q_init[2] = q_init[2]+0.15 q_goal[2] = q_goal[2]+0.15 fullBody.setStaticStability(False) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig (q_init) r(q_init) q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold) r(q_init) # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig (q_goal) q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold) r(q_goal) # specifying the full body configurations as start and goal state of the problem r(q_init) fullBody.setStartState(q_init,[rLegId,lLegId]) fullBody.setEndState(q_goal,[rLegId,lLegId])
acc_init = tp.ps.configAtParam(pathId, 0.01)[10:13] dir_goal = tp.ps.configAtParam(pathId, tp.ps.pathLength(pathId))[7:10] acc_goal = tp.ps.configAtParam(pathId, tp.ps.pathLength(pathId))[10:13] configSize = fullBody.getConfigSize( ) - fullBody.client.basic.robot.getDimensionExtraConfigSpace() # copy extraconfig for start and init configurations q_init[configSize:configSize + 3] = dir_init[::] q_init[configSize + 3:configSize + 6] = acc_init[::] q_goal[configSize:configSize + 3] = dir_goal[::] q_goal[configSize + 3:configSize + 6] = acc_goal[::] fullBody.setStaticStability(False) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig(q_init) q_init = fullBody.generateContacts(q_init, dir_init, acc_init, 2) # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig(q_goal) q_goal = fullBody.generateContacts(q_goal, dir_goal, acc_goal, 2) # specifying the full body configurations as start and goal state of the problem fullBody.setStartState(q_init, [larmId, rLegId, rarmId, lLegId]) fullBody.setEndState(q_goal, [larmId, rLegId, rarmId, lLegId]) r(q_init) # computing the contact sequence configs = fullBody.interpolate(0.001, pathId=pathId, robustnessTreshold=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) fullBody.setCurrentConfig(q_goal) #~ r(q_goal) q_goal = fullBody.generateContacts(q_goal, [0, 0, 1]) #~ r(q_goal) fullBody.setStartState(q_init, [rLegId, lLegId]) #,rarmId,larmId]) fullBody.setEndState(q_goal, [rLegId, lLegId]) #,rarmId,larmId]) #~ #~ configs = fullBody.interpolate(0.1) configs = fullBody.interpolate(0.1) #~ configs = fullBody.interpolate(0.15) i = 0 fullBody.draw(configs[i], r) i = i + 1 i - 1 #~ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
-0.11841270055084166, -0.7903724619230399, 1.5860588209778248, -0.7956764813669736, 0.11846562142112675] r(q_init) q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize] fullBody.setCurrentConfig (q_init) #~ q_0 = fullBody.getCurrentConfig(); q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init) fullBody.setCurrentConfig (q_goal) #~ r(q_goal) q_goal = fullBody.generateContacts(q_goal, [0,0,1]) #~ r(q_goal) #~ gui = r.client.gui #~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId]) fullBody.setStartState(q_init,[rLegId,lLegId,larmId]) fullBody.setEndState(q_goal,[rLegId,lLegId]) #~
rLegId = 'RFoot' lLegId = 'LFoot' rarmId = 'RHand' larmId = 'LHand' nbSamples = 50000 x = 0.03 y = 0.08 fullBody.addLimb(rLegId, 'RThigh_rx', 'SpidermanRFootSphere', [0, 0, 0], [0, 0, 1], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF") fullBody.addLimb(lLegId, 'LThigh_rx', 'SpidermanLFootSphere', [0, 0, 0], [0, 0, 1], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF") fullBody.addLimb(rarmId, 'RHumerus_rx', 'SpidermanRHandSphere', [0, 0, 0], [0, -1, 0], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF") fullBody.addLimb(larmId, 'LHumerus_rx', 'SpidermanLHandSphere', [0, 0, 0], [0, 1, 0], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF") print("Limbs added to fullbody") confsize = len(tp.q11) ### TEST OF CONTACT CREATION FOR INTERMEDIATE CONFIG, NOT USED FOR INTERPOLATION q_tmp = q_contact_takeoff[::] q_tmp[0:confsize - tp.ecsSize] = tp.q_cube[0:confsize - tp.ecsSize] rr(q_tmp) fullBody.setCurrentConfig(q_tmp) q_tmp_test = fullBody.generateContacts(q_tmp, [0, -1, 0], True) rr(q_tmp_test) fullBody.isConfigValid(q_tmp_test) # result: # q_tmp_test = [-1.2, -2.8, 3.6, 0.707107, 0.0, 0.0, 0.707107, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.9324800803082636, -0.9184709614284768, 0.6886200849241174, -0.6066622060535428, 0.47649495495305294, 1.0976823065116303, -0.538404483799899, -1.0681738092891575, -1.1021076588270258, 1.1498838725595328, -0.6156809000975677, 0.5815958533218022, -1.4659758542959642, -0.3603605133380307, 0.36116581520970376, -1.048638878548546, 0.24059108997189355, 1.23953255675232, -0.7519812787252685, -0.1402404928640359, -1.0, 0.023118656707620415, -0.6298340889273957, -0.15800407650545129, 0.4963824557225752, -0.4989080182494368, 0.2774303858753873, -0.9974339561414656]
rarm = 'rh_haa_joint' rHand = 'rh_foot_joint' fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType) larmId = 'lfleg' larm = 'lf_haa_joint' lHand = 'lf_foot_joint' fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType) q_0 = fullBody.getCurrentConfig(); q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7] q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7] # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig (q_init) q_init = fullBody.generateContacts(q_init, [0,0,1]) #~ from pickle import load #~ f = open("config_"+str(tp.config_i), 'r') #~ q_init = load(f) #~ f.close() # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig (q_goal) q_goal = fullBody.generateContacts(q_goal, [0,0,1]) # specifying the full body configurations as start and goal state of the problem fullBody.setStartState(q_init,[]) #~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId]) fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
acc_init = tp.ps.configAtParam(0, 0.01)[tp.indexECS + 3:tp.indexECS + 6] dir_goal = tp.ps.configAtParam( 0, tp.ps.pathLength(0))[tp.indexECS:tp.indexECS + 3] acc_goal = tp.ps.configAtParam(0, tp.ps.pathLength(0))[tp.indexECS + 3:tp.indexECS + 6] fullBody.setStaticStability(False) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig(q_init) r(q_init) #~ q_init = fullBody.generateContacts(q_init,dir_init,acc_init) r(q_init) # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig(q_goal) q_goal = fullBody.generateContacts(q_goal, dir_goal, acc_goal) # copy extraconfig for start and init configurations q_init[configSize:configSize + 3] = dir_init[::] q_init[configSize + 3:configSize + 6] = acc_init[::] q_goal[configSize:configSize + 3] = dir_goal[::] q_goal[configSize + 3:configSize + 6] = acc_goal[::] # specifying the full body configurations as start and goal state of the problem r(q_init) #~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) fullBody.setStartState(q_init, [rLegId, lLegId]) fullBody.setEndState(q_goal, [rLegId, lLegId]) """
fullBody.setStaticStability(True) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig(q_init) r(q_init) # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig(q_goal) # copy extraconfig for start and init configurations q_init[configSize:configSize + 3] = dir_init[::] q_init[configSize + 3:configSize + 6] = acc_init[::] q_goal[configSize:configSize + 3] = dir_goal[::] q_goal[configSize + 3:configSize + 6] = acc_goal[::] # specifying the full body configurations as start and goal state of the problem q_init = fullBody.generateContacts(q_init, dir_init, acc_init, 1) q_goal = fullBody.generateContacts(q_goal, dir_goal, acc_goal, 1) r(q_init) fullBody.setStartState(q_init, [rLegId, lLegId]) fullBody.setEndState(q_goal, [rLegId, lLegId]) from hpp.gepetto import PathPlayer pp = PathPlayer(fullBody.client.basic, r) pp.dt = 0.001 configs = fullBody.interpolate(0.05, pathId=0, robustnessTreshold=0, filterStates=True)
lHand = 'lf_foot_joint' lArmOffset = [0., 0, -0.] lArmNormal = [0, 1, 0] lArmx = 0.02 lArmy = 0.02 fullBody.addLimb(larmId, larm, lHand, lArmOffset, lArmNormal, lArmx, lArmy, nbSamples, "manipulability", 0.1) q_0 = fullBody.getCurrentConfig() q_init = fullBody.getCurrentConfig() q_init[0:7] = tp.q_init[0:7] q_goal = fullBody.getCurrentConfig() q_goal[0:7] = tp.q_goal[0:7] fullBody.setCurrentConfig(q_init) q_init = fullBody.generateContacts(q_init, [0, 0, 1]) q_0 = fullBody.getCurrentConfig() fullBody.setCurrentConfig(q_goal) q_goal = fullBody.generateContacts(q_goal, [0, 0, 1]) fullBody.setStartState(q_init, []) fullBody.setEndState(q_goal, [rLegId, lLegId, rarmId, larmId]) r(q_init) configs = fullBody.interpolate(0.1) r.loadObstacleModel('hpp-rbprm-corba', "stair_bauzil", "contact") i = 0