rLeg = fullBody.rleg
rfoot = fullBody.rfoot
rLegOffset = fullBody.offset[:]
lLegOffset = fullBody.offset[:]
rArmOffset = fullBody.offset[:]
lArmOffset = fullBody.offset[:]
lLegId = fullBody.lLegId
lLeg = fullBody.lleg
lfoot = fullBody.lfoot
# make sure this is 0
q_0 = fullBody.getCurrentConfig()
zeroConf = [0, 0, 0, 0, 0, 0, 1.0]
q_0[0:7] = zeroConf
fullBody.setCurrentConfig(q_0)
effectors = [
fullBody.rfoot,
fullBody.lfoot,
fullBody.rhand,
fullBody.lhand,
]
limbIds = [fullBody.rLegId, fullBody.lLegId, fullBody.rArmId, fullBody.lArmId]
offsets = [
array(rLegOffset),
array(lLegOffset),
array(rArmOffset),
array(lArmOffset)
]
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
-tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,
tp.aMax, -0, 0
])
ps = tp.ProblemSolver(fullBody)
ps.setParameter("Kinodynamic/velocityBound", tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound", tp.aMax)
#load the viewer
v = tp.Viewer(ps, viewerClient=tp.v.client, displayCoM=True)
# load a reference configuration
q_ref = fullBody.referenceConfig[::] + [0, 0, 0, 0, 0, 0]
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig(q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6)
#fullBody.usePosturalTaskContactCreation(True)
print("Generate limb DB ...")
tStart = time.time()
fullBody.loadAllLimbs("fixedStep04", "ReferenceConfiguration")
tGenerate = time.time() - tStart
print("Done.")
print("Databases generated in : " + str(tGenerate) + " s")
fullBody.setReferenceConfig(q_ref)
#define initial and final configurations :
configSize = fullBody.getConfigSize(
) - fullBody.client.robot.getDimensionExtraConfigSpace()