pId = tp.ps.numberPaths() -1
fullBody = Robot ()
# Set the bounds for the root
fullBody.setJointBounds ("root_joint", [-20,20 ,-20, 20, 0, 2.8])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds(tp.extraDofBounds)
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
ps = tp.ProblemSolver( fullBody )
#load the viewer
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
# load a reference configuration
q_ref = fullBody.referenceConfig_elbowsUp[::] +[0]*6
fullBody.setCurrentConfig (q_ref)
fullBody.setReferenceConfig(q_ref)
fullBody.setPostureWeights(fullBody.postureWeights[::] + [0]*6)
#fullBody.usePosturalTaskContactCreation(True)
print("Generate limb DB ...")
tStart = time.time()
# generate databases :
nbSamples = 10000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.3)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.3)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
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)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 10000
fullBody.addLimb(fullBody.rLegId, fullBody.rleg, fullBody.rfoot,
fullBody.rLegOffset, fullBody.rLegNormal, fullBody.rLegx,
fullBody.rLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
fullBody.addLimb(fullBody.lLegId, fullBody.lleg, fullBody.lfoot,
fullBody.lLegOffset, fullBody.rLegNormal, fullBody.lLegx,
fullBody.lLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")