#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
## Add arms (not used for contact) :
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
fullBody.addNonContactingLimb(rarmId, rarm, rHand, 10000)
fullBody.runLimbSampleAnalysis(rarmId, "ReferenceConfiguration", True)
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
fullBody.addNonContactingLimb(larmId, larm, lHand, 10000)
fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
fullBody.setReferenceConfig(q_ref) # must be called after adding the limbs
tGenerate = time.time() - tStart
print "generate databases in : " + str(tGenerate) + " s"
"""
fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
tLoad = time.time() - tStart
print "Load databases in : "+str(tLoad)+" s"
"""
q_0 = fullBody.getCurrentConfig()
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
eps = 0.0001
configSize = fullBody.getConfigSize(
) - fullBody.client.basic.robot.getDimensionExtraConfigSpace()
load_paths("19_06_p")
save_paths("19_06_p_save")
save("19_06_s_save")
def sac():
save("19_06_s")
save_paths("19_06_p")
init_bezier_traj(fullBody, r, pp, [], limbsCOMConstraints)
all_paths = [[], []]
from hpp.corbaserver.rbprm.state_alg import *
#~ d(0.07);e(0.01)
i = 0
d(0.1)
e(0.01)
states = planToStates(fullBody, configs)
#~ lc()
#~ fullBody.setReferenceConfig(configs[-1])
fullBody.setReferenceConfig(states[0].q())
onepath2(states[0:-1],
nopt=0,
mu=1,
effector=False,
init_acc=[0, 0, 0.],
init_vel=[0., 0., 0.])
plall()