Esempi in Python per Robot.loadAllLimbs

Esempio n. 1

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[:]

Esempio n. 2

File: anymal_slalom_debris.py Progetto: nim65s/hpp-rbprm-corba

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]

Esempio n. 3

    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]

Esempio n. 4

File: anymal_modular_palet_low.py Progetto: teguhSL/hpp-rbprm-corba

# 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]

Esempio n. 5

File: anymal_platform_random.py Progetto: teguhSL/hpp-rbprm-corba

    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]

Esempio n. 6

File: anymal_circle_one_step.py Progetto: nim65s/hpp-rbprm-corba

"""

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]