NUM_SAMPLES = 18000
IT_DISPLAY_PROGRESS = NUM_SAMPLES / 10
MIN_DIST_BETWEEN_FEET_Y = 0.10
MIN_DIST_BETWEEN_FEET_X = 0.10
MAX_DIST_BETWEEN_FEET_X = 0.35
MAX_DIST_BETWEEN_FEET_Z = 0.35
MIN_HEIGHT_COM = 0.3
# margin used to constrain the com y position : if it's on the left of the left foot
# or on the right of the right foot
# for more than this margin, we reject this sample:
MARGIN_FEET_SIDE = 0.05
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)
print("Plan guide trajectory ...")
import anymal_plinth_path as tp
print("Done.")
import time
pId = tp.ps.numberPaths() - 1
fullBody = Robot()
# Set the bounds for the root, take slightly larger bounding box than during root planning
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.2
root_bounds[1] += 0.2
root_bounds[2] -= 0.2
root_bounds[3] += 0.2
root_bounds[-1] = 0.12
root_bounds[-2] = 0.3
fullBody.setJointBounds("root_joint", root_bounds)
fullBody.setConstrainedJointsBounds()
fullBody.setJointBounds("LF_KFE", [-1.4, 0.])
fullBody.setJointBounds("RF_KFE", [-1.4, 0.])
fullBody.setJointBounds("LH_KFE", [0., 1.4])
fullBody.setJointBounds("RH_KFE", [0., 1.4])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
-tp.vMax, tp.vMax, -tp.vMax, tp.vMax, -tp.vMax, tp.vMax, -tp.aMax, tp.aMax,
-tp.aMax, tp.aMax, -tp.aMaxZ, tp.aMaxZ
])
ps = tp.ProblemSolver(fullBody)
ps.setParameter("Kinodynamic/velocityBound", tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound", tp.aMax)
#load the viewer
print("Plan guide trajectory ...")
import anymal_slalom_debris_path as tp
print("Done.")
import time
pId = tp.ps.numberPaths() - 1
fullBody = Robot()
# Set the bounds for the root, take slightly larger bounding box than during root planning
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.5
root_bounds[1] += 0.5
root_bounds[2] -= 0.5
root_bounds[3] += 0.5
root_bounds[-1] = 0.8
root_bounds[-2] = 0.3
fullBody.setJointBounds("root_joint", root_bounds)
fullBody.setVeryConstrainedJointsBounds()
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
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]
print "Path planning OK."
f.write("Planning_success: True" + "\n")
f.close()
else:
print "Error during path planning"
f.write("Planning_success: False" + "\n")
f.close()
import sys
sys.exit(1)
fullBody = Robot()
root_bounds = tp.root_bounds
root_bounds[-1] = 0.6
root_bounds[-2] = 0.3
# Set the bounds for the root
fullBody.setJointBounds("root_joint", root_bounds)
## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
fullBody.setVeryConstrainedJointsBounds()
fullBody.setJointBounds('LF_HAA', [-0.2, 0.2])
fullBody.setJointBounds('RF_HAA', [-0.2, 0.2])
fullBody.setJointBounds('LH_HAA', [-0.2, 0.2])
fullBody.setJointBounds('RH_HAA', [-0.2, 0.2])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
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)
