# 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
