rHand = 'rh_foot_joint'
rArmOffset = [0,0,0]
rArmNormal = [1,0,0]
rArmx = 0.02; rArmy = 0.02
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
lArmOffset = [0,0,0]
lArmNormal = [1,0,0]
lArmx = 0.02; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
#make sure this is 0
q_0 = fullBody.getCurrentConfig ()
def printEffPosition(limbId, nbSamples):
limit = nbSamples-1;
f1=open('./data/roms/hyq/'+limbId+'.erom', 'w+')
for i in range(0,limit):
q = fullBody.getSamplePosition(limbId,i)
f1.write(str(q[0]) + "," + str(q[1]) + "," + str(q[2]) + "\n")
f1.close()
printEffPosition(rarmId, nbSamples)
printEffPosition(rLegId, nbSamples)
printEffPosition(larmId, nbSamples)
printEffPosition(lLegId, nbSamples)
rLegOffset = [0,0,-0.105] rLegNormal = [0,0,1] rLegx = 0.09; rLegy = 0.05 fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "manipulability", 0.1) lLegId = 'hrp2_lleg_rom' lLeg = 'LLEG_JOINT0' lLegOffset = [0,0,-0.105] lLegNormal = [0,0,1] lLegx = 0.09; lLegy = 0.05 fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "manipulability", 0.1) q_0 = fullBody.getCurrentConfig(); #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,) q_init =[0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,0.0, 0.0, 0.0, 0.0,0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init) fullBody.setCurrentConfig (q_init) configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace() q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7] dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3] acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6] dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3] acc_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS+3:tp.indexECS+6]
fullBody.addLimbDatabase(str(db_dir + limbId + '.db'), limbId,
heuristicName, loadValues,
disableEffectorCollision)
rLegId = 'rfleg'
lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
addLimbDb(rLegId, "manipulability")
addLimbDb(lLegId, "manipulability")
addLimbDb(rarmId, "manipulability")
addLimbDb(larmId, "manipulability")
q_0 = fullBody.getCurrentConfig()
q_init = fullBody.getCurrentConfig()
q_init[0:7] = tp.ps.configAtParam(
0, 0.01)[0:7] # use this to get the correct orientation
q_goal = fullBody.getCurrentConfig()
q_goal[0:7] = tp.ps.configAtParam(pathId, tp.ps.pathLength(pathId))[0:7]
dir_init = tp.ps.configAtParam(pathId, 0.01)[7:10]
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[::]
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.045,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
#~
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
confsize = len(tp.q_init)
q_init = [0.36,
-0.9940043559354331,
0.9006595208471023,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
"manipulability", 0.1, cType)
#~
rarmId = 'rhleg'
rarm = 'RH_HAA'
rHand = 'RH_MOUNT_TO_FOOT'
fullBody.addLimb(rarmId, rarm, rHand, offset, normal, legx, legy, nbSamples,
"manipulability", 0.1, cType)
larmId = 'lfleg'
larm = 'LF_HAA'
lHand = 'LF_MOUNT_TO_FOOT'
#fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
fullBody.addLimb(larmId, larm, lHand, offset, normal, legx, legy, nbSamples,
"manipulability", 0.1, cType)
q_0 = fullBody.getCurrentConfig()
q_init = [
-2.5, 0.0, 0.38418442387922846, 0.0, 0.0, 0.0, 1.0, 0.28621482125006287,
0.389380188410535, -1.0701831582488357, 0.18103701803493258,
-0.3633371504537386, 1.1405892092366712, -0.32541312483462415,
0.47768600410524753, -1.0819534508855166, -0.29647189017581244,
-0.23659478921159968, 0.9527203353688805
]
fullBody.setCurrentConfig(q_init)
def runall(lid, dbName):
fullBody.runLimbSampleAnalysis(lid, "ReferenceConfiguration", False)
fullBody.runLimbSampleAnalysis(lid, "minimumSingularValue", False)
#~ fullBody.runLimbSampleAnalysis(lid, "selfCollisionProbability", False)
rArmOffset = [-0.040,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.040,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
#~
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
confsize = len(tp.q_init)
q_init = [
#~ 0.12, -0.45, 0.95, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.36, 0, 1.01, 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, -1, 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[0:confsize] = tp.q_init[0:confsize]
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
#~
rarmId = 'rhleg'
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])
lLegId = 'lhleg' lLeg = 'lh_haa_joint' lfoot = 'lh_foot_joint' fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType) rarmId = 'rhleg' rarm = 'rh_haa_joint' rHand = 'rh_foot_joint' fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType) larmId = 'lfleg' larm = 'lf_haa_joint' lHand = 'lf_foot_joint' fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 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]) # 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.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
0.0, # LLEG 25-30
0.0,
0.0,
-0.453785606,
0.872664626,
-0.41887902,
0.0, # RLEG 31-36
]
r(q_init)
#~ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLegId,rLegId, rarmId, larmId] ]
#~ limbs = [[lLegId,rLegId] ]
limbs = [[lLegId, rLegId, rarmId, larmId]]
#~ limbs = [[larmId, rarmId]]
q_init = fullBody.getCurrentConfig()
#~ gen(limbs[0], 1000)
for ls in limbs:
gen(ls, 1, False, unstable=True)
#~ gen(limbs[0], 1000, unstable=True)
i = 0
a = None
b = None
#~ a = all_states[0][0]['candidates'][0]
#~ b = a ['candidates_per_effector']
def init():
r(a['init_config'])
rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05)
larmId = '4Larm'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05)
#~
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
confsize = len(tp.q_init)
q_init = fullBody.getCurrentConfig(); q_init[0:confsize] = tp.q_init[0:confsize]
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)
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
fullBody.setCurrentConfig (q_init)
confsize = len(falseNeg[0])
falseFalseNegative = 0
trueFalseNegative = 0
nbnegative = 0
totalconfigs = 0
q_init = fullBody.getCurrentConfig();
for q in falseNeg:
q_init[0:confsize] = q[0:confsize]
totalconfigs = totalconfigs + 1
#~ print "avant " + str(fullBody.isConfigValid(q_init)) + str(q_init)
#~ q_init = fullBody.makeCollisionFree(q_init)
#~ print "apres " + str(fullBody.isConfigValid(q_init)) + str(q_init)
#~ raise ValueError ("tg")
#~ fullBody.canGenerateLimbContact(limb, q_init)
if (fullBody.canGenerateBalancedContact(q_init, [0,0,1])):
trueFalseNegative = trueFalseNegative + 1
else:
falseFalseNegative = falseFalseNegative +1
#~
urdfSuffix = ""
srdfSuffix = ""
#~ V0list = tp.V0list
#~ Vimplist = tp.Vimplist
base_joint_xyz_limits = tp.base_joint_xyz_limits
fullBody = FullBody ()
robot = fullBody.client.basic.robot
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", base_joint_xyz_limits)
ps = path_planner.ProblemSolver( fullBody )
r = path_planner.Viewer (ps, viewerClient=path_planner.r.client)
rr = r
pp = PathPlayer (fullBody.client.basic, rr); pp.speed = 0.6
q_0 = fullBody.getCurrentConfig(); rr(q_0)
rLegId = 'RFoot'
lLegId = 'LFoot'
rarmId = 'RHand'
larmId = 'LHand'
rfoot = 'SpidermanRFootSphere'
lfoot = 'SpidermanLFootSphere'
lHand = 'SpidermanLHandSphere'
rHand = 'SpidermanRHandSphere'
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", grasp = True)
fullBody.addLimb(larmId,'LHumerus_rx','SpidermanLHandSphere',[0,0,0],[0,1,0], x, y, nbSamples, "EFORT_Normal", 0.01,"_6_DOF", grasp = True)
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
"""
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
q_init =[0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,0.0, 0.0, 0.0, 0.0,0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init)
fullBody.setCurrentConfig (q_init)
configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.0001)[0:7] # use this to get the correct orientation
q_init[2] = 0.648702
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:7]
dir_init = tp.ps.configAtParam(0,0.01)[tp.indexECS:tp.indexECS+3]
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]
rarm = 'RARM_JOINT0' rHand = 'RARM_JOINT5' rArmOffset = [-0.05,-0.050,-0.050] rArmNormal = [1,0,0] rArmx = 0.024; rArmy = 0.024 fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "forward", 0.05) larmId = '4Larm' larm = 'LARM_JOINT0' lHand = 'LARM_JOINT5' lArmOffset = [-0.05,-0.050,-0.050] lArmNormal = [1,0,0] lArmx = 0.024; lArmy = 0.024 fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "forward", 0.05) q_0 = fullBody.getCurrentConfig(); confsize = len(tp.q_init) q_init = fullBody.getCurrentConfig(); q_init[0:confsize] = tp.q_init[0:confsize] 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)
