#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)
tGenerate = time.time() - tStart
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init = q_ref[::]
q_init[0:7] = tp.q_init[0:7]
q_init[2] += q_ref[2] - 0.98 # 0.98 is the q_ref[2] for the rom device
v(q_init)
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
0.0 , 0.006761, #Chest
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1,-0.005, #Left Arm
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005,#Right Arm
0., 0.,
0,0,0,0,0,0]; # head
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
tStart = time.time()
# generate databases :
nbSamples = 1
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "EFORT", 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, "EFORT", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.rArmOffset,fullBody.rArmNormal, fullBody.rArmx, fullBody.rArmx, nbSamples, "EFORT", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.rArmId, "ReferenceConfiguration", True)
tGenerate = time.time() - tStart
print "generate databases in : "+str(tGenerate)+" s"
v = vf.createViewer(displayCoM=True)
v(q_init)
v.addLandmark(v.sceneName,0.5)
v.addLandmark('talos/arm_right_7_link',0.1)
q_init[0:2] = [-0.5,0.8]
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")
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : " + str(tGenerate) + " s"
#define initial and final configurations :
configSize = fullBody.getConfigSize(
q_ref = fullBody.referenceConfig[::] + [0] * 6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig(q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 50000
fullBody.addLimb(fullBody.rLegId,
fullBody.rleg,
fullBody.rfoot,
fullBody.rLegOffset,
fullBody.rLegNormal,
fullBody.rLegx,
fullBody.rLegy,
nbSamples,
"fixedStep08",
0.01,
kinematicConstraintsPath=fullBody.rLegKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,
fullBody.lleg,
fullBody.lfoot,
fullBody.lLegOffset,
fullBody.rLegNormal,
fullBody.lLegx,
fullBody.lLegy,
nbSamples,
"fixedStep08",