def load_fullbody(self):
from hpp.corbaserver.rbprm.anymal import Robot
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [
0
] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [
0
] * self.path_planner.extra_dof
# from plot_polytopes import *
# from pinocchio import Quaternion
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)
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import *
import time
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
import time
statusFilename = tp.statusFilename
pId = 0
f = open(statusFilename, "a")
if tp.ps.numberPaths() > 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()
# 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)
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import *
import time
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
])
#Robot.urdfSuffix += "_safeFeet"
statusFilename = tp.statusFilename
pId = tp.pid
f = open(statusFilename, "a")
if tp.ps.numberPaths() > 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()
# Set the bounds for the root
rootBounds = tp.rootBounds[::]
rootBounds[0] -= 0.2
rootBounds[1] += 0.2
rootBounds[2] -= 0.5
rootBounds[3] += 0.5
rootBounds[4] -= 0.2
rootBounds[5] += 0.2
fullBody.setJointBounds("root_joint", rootBounds)
fullBody.setVeryConstrainedJointsBounds()
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
#Robot.urdfSuffix += "_safeFeet"
statusFilename = tp.statusFilename
pId = 0
f = open(statusFilename, "a")
if tp.ps.numberPaths() > 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()
# Set the bounds for the root
rootBounds = tp.rootBounds[::]
rootBounds[-2] -= 0.2
rootBounds[0] -= 0.2
rootBounds[1] += 0.2
rootBounds[2] -= 0.2
rootBounds[3] += 0.2
fullBody.setJointBounds("root_joint", rootBounds)
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,