fullBody = Robot()
fullBody.setConstrainedJointsBounds()
fullBody.setJointBounds("LF_KFE", [-1.4, 0.0])
fullBody.setJointBounds("RF_KFE", [-1.4, 0.0])
fullBody.setJointBounds("LH_KFE", [0.0, 1.4])
fullBody.setJointBounds("RH_KFE", [0.0, 1.4])
fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, -20, 20])
dict_heuristic = {
fullBody.rLegId: "static",
fullBody.lLegId: "static",
fullBody.rArmId: "fixedStep04",
fullBody.lArmId: "fixedStep04",
}
fullBody.loadAllLimbs(dict_heuristic, "ReferenceConfiguration", nbSamples=12)
nbSamples = 1
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
v = vf.createViewer()
rootName = "root_joint"
zero = [0.0, 0.0, 0.0]
rLegId = fullBody.rLegId
rLeg = fullBody.rleg
rfoot = fullBody.rfoot
rLegOffset = fullBody.offset[:]
lLegOffset = fullBody.offset[:]
rArmOffset = fullBody.offset[:]
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()
q_init[0:7] = tp.ps.configAtParam(
pId, 0)[0:7] # use this to get the correct orientation
q_goal = q_init[::]
q_goal[0:7] = tp.ps.configAtParam(pId, tp.ps.pathLength(pId))[0:7]
dir_init = [0, 0, 0]
acc_init = [0, 0, 0]
heuristicL = "fixedStep06"
"""
fullBody.setCurrentConfig(q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
"""
nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, heuristicR, 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, heuristicL, 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
"""
fullBody.loadAllLimbs("fixedStep04")
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : " + str(tGenerate) + " s"
#define initial and final configurations :
configSize = fullBody.getConfigSize(
) - fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(
pId, 0)[0:7] # use this to get the correct orientation
q_goal = q_init[::]
q_goal[0:7] = tp.ps.configAtParam(pId, tp.ps.pathLength(pId))[0:7]
vel_init = tp.ps.configAtParam(pId, 0)[tp.indexECS:tp.indexECS + 3]
acc_init = tp.ps.configAtParam(pId, 0)[tp.indexECS + 3:tp.indexECS + 6]
# load a reference configuration
q_ref = fullBody.referenceConfig[::] + [0] * 6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6)
#fullBody.usePosturalTaskContactCreation(True)
fullBody.setCurrentConfig(q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
fullBody.loadAllLimbs("static",
"ReferenceConfiguration",
nbSamples=100000,
disableEffectorCollision=False)
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : " + str(tGenerate) + " s"
v.addLandmark('anymal/base_0', 0.2)
#define initial and final configurations :
configSize = fullBody.getConfigSize(
) - fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(
pId, 0)[0:7] # use this to get the correct orientation
q_goal = q_init[::]
q_goal[0:7] = tp.ps.configAtParam(pId, tp.ps.pathLength(pId))[0:7]
vel_init = tp.ps.configAtParam(pId, 0)[tp.indexECS:tp.indexECS + 3]
v = FakeViewer()
# load a reference configuration
q_ref = fullBody.referenceConfig[::] + [0] * 6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6)
#fullBody.usePosturalTaskContactCreation(True)
fullBody.setCurrentConfig(q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
fullBody.loadAllLimbs("static", "ReferenceConfiguration", nbSamples=100000)
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : " + str(tGenerate) + " s"
#define initial and final configurations :
configSize = fullBody.getConfigSize(
) - fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(
pId, 0)[0:7] # use this to get the correct orientation
q_goal = q_init[::]
q_goal[0:7] = tp.ps.configAtParam(pId, tp.ps.pathLength(pId))[0:7]
vel_init = tp.ps.configAtParam(pId, 0)[tp.indexECS:tp.indexECS + 3]
acc_init = tp.ps.configAtParam(pId, 0)[tp.indexECS + 3:tp.indexECS + 6]
"""
fullBody.setCurrentConfig(q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
"""
nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, heuristicR, 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, heuristicL, 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
"""
#~ fullBody.loadAllLimbs("fixedStep04","ReferenceConfiguration")
fullBody.loadAllLimbs("static", "ReferenceConfiguration")
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : " + str(tGenerate) + " s"
#define initial and final configurations :
configSize = fullBody.getConfigSize(
) - fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(
pId, 0)[0:7] # use this to get the correct orientation
q_goal = q_init[::]
q_goal[0:7] = tp.ps.configAtParam(pId, tp.ps.pathLength(pId))[0:7]
vel_init = tp.ps.configAtParam(pId, 0)[tp.indexECS:tp.indexECS + 3]
acc_init = tp.ps.configAtParam(pId, 0)[tp.indexECS + 3:tp.indexECS + 6]