def generateSmoothBezierTraj(time_interval,placement_init,placement_end,numTry=None,q_t=None,phase_previous=None,phase=None,phase_next=None,fullBody=None,eeName=None,viewer=None):
if numTry > 0 :
raise ValueError("generateSmoothBezierTraj will always produce the same trajectory, cannot be called with numTry > 0 ")
predef_curves = generatePredefLandingTakeoff(time_interval,placement_init,placement_end)
bezier_takeoff = predef_curves.curves[predef_curves.idFirstNonZero()]
bezier_landing = predef_curves.curves[predef_curves.idLastNonZero()]
id_middle = int(math.floor(len(predef_curves.curves)/2.))
# update mid curve to minimize velocity along the curve:
# set problem data for mid curve :
pData = bezier_com.ProblemData()
pData.c0_ = bezier_takeoff(bezier_takeoff.max())
pData.dc0_ = bezier_takeoff.derivate(bezier_takeoff.max(),1)
pData.ddc0_ = bezier_takeoff.derivate(bezier_takeoff.max(),2)
pData.j0_ = bezier_takeoff.derivate(bezier_takeoff.max(),3)
pData.c1_ = bezier_landing(0)
pData.dc1_ = bezier_landing.derivate(0,1)
pData.ddc1_ = bezier_landing.derivate(0,2)
pData.j1_ = bezier_landing.derivate(0,3)
pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK
t_middle = predef_curves.curves[id_middle].max()
res = bezier_com.computeEndEffector(pData,t_middle)
bezier_middle = res.c_of_t
curves = predef_curves.curves[::]
curves[id_middle] = bezier_middle
pBezier = PolyBezier(curves)
ref_traj = trajectories.BezierTrajectory(pBezier,placement_init,placement_end,time_interval)
return ref_traj
def generateSmoothBezierTrajWithPredef(time_interval, placement_init,
placement_end):
predef_curves = generatePredefBeziers(time_interval, placement_init,
placement_end)
bezier_takeoff = predef_curves.curves[predef_curves.idFirstNonZero()]
bezier_landing = predef_curves.curves[predef_curves.idLastNonZero()]
id_middle = int(math.floor(len(predef_curves.curves) / 2.))
# update mid curve to minimize velocity along the curve:
# set problem data for mid curve :
pData = bezier_com.ProblemData()
pData.c0_ = bezier_takeoff(bezier_takeoff.max())
pData.dc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 1)
pData.ddc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 2)
pData.j0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 3)
pData.c1_ = bezier_landing(0)
pData.dc1_ = bezier_landing.derivate(0, 1)
pData.ddc1_ = bezier_landing.derivate(0, 2)
pData.j1_ = bezier_landing.derivate(0, 3)
pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK
t_middle = predef_curves.curves[id_middle].max()
res = bezier_com.computeEndEffector(pData, t_middle)
bezier_middle = res.c_of_t
curves = predef_curves.curves[::]
curves[id_middle] = bezier_middle
pBezier = PolyBezier(curves)
ref_traj = trajectories.BezierTrajectory(pBezier, placement_init,
placement_end, time_interval)
return ref_traj
def generateSmoothBezierTrajWithPredef(cfg, time_interval, placement_init,
placement_end):
predef_curves = generatePredefBeziers(cfg, time_interval, placement_init,
placement_end)
id_middle = int(math.floor(predef_curves.num_curves() / 2.))
bezier_takeoff = predef_curves.curve_at_index(id_middle -
1).translation_curve()
bezier_landing = predef_curves.curve_at_index(id_middle +
1).translation_curve()
# update mid curve to minimize velocity along the curve:
# set problem data for mid curve :
pData = bezier_com.ProblemData()
pData.c0_ = bezier_takeoff(bezier_takeoff.max())
pData.dc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 1)
pData.ddc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 2)
pData.j0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 3)
pData.c1_ = bezier_landing(bezier_landing.min())
pData.dc1_ = bezier_landing.derivate(bezier_landing.min(), 1)
pData.ddc1_ = bezier_landing.derivate(bezier_landing.min(), 2)
pData.j1_ = bezier_landing.derivate(bezier_landing.min(), 3)
pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS \
| bezier_com.ConstraintFlag.INIT_VEL \
| bezier_com.ConstraintFlag.INIT_ACC \
| bezier_com.ConstraintFlag.END_ACC \
| bezier_com.ConstraintFlag.END_VEL \
| bezier_com.ConstraintFlag.END_POS \
| bezier_com.ConstraintFlag.INIT_JERK \
| bezier_com.ConstraintFlag.END_JERK
t_min_middle = predef_curves.curve_at_index(id_middle).min()
t_max_middle = predef_curves.curve_at_index(id_middle).max()
t_middle = t_max_middle - t_min_middle
res = bezier_com.computeEndEffector(pData, t_middle)
wp_middle = res.c_of_t.waypoints()
bezier_middle = bezier(wp_middle, t_min_middle, t_max_middle)
# create a new piecewise-bezier, with the predef curves except for bezier_middle :
pBezier = piecewise_SE3()
for i in range(predef_curves.num_curves()):
if i == id_middle:
pBezier.append(
SE3Curve(bezier_middle, placement_init.rotation,
placement_end.rotation))
else:
pBezier.append(predef_curves.curve_at_index(i))
return pBezier
def generateConstrainedBezierTraj(cfg,
time_interval,
placement_init,
placement_end,
numTry,
q_t=None,
phase_previous=None,
phase=None,
phase_next=None,
fullBody=None,
eeName=None,
viewer=None):
if numTry == 0:
return generateSmoothBezierTraj(cfg, time_interval, placement_init,
placement_end)
else:
if not q_t or not phase_previous or not phase or not phase_next or not fullBody or not eeName:
raise ValueError(
"Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments"
)
predef_curves = generatePredefBeziers(cfg, time_interval, placement_init,
placement_end)
bezier_takeoff = predef_curves.curves[predef_curves.idFirstNonZero()]
bezier_landing = predef_curves.curves[predef_curves.idLastNonZero()]
id_middle = int(math.floor(len(predef_curves.curves) / 2.))
pos_init = bezier_takeoff(bezier_takeoff.max())
pos_end = bezier_landing(0)
if VERBOSE:
print("bezier takeoff end : ", pos_init)
print("bezier landing init : ", pos_end)
t_begin = predef_curves.times[id_middle]
t_middle = predef_curves.curves[id_middle].max()
t_end = t_begin + t_middle
if VERBOSE:
print("t begin : ", t_begin)
print("t end : ", t_end)
q_init = q_t[:, int(t_begin / cfg.IK_dt)] # after the predef takeoff
q_end = q_t[:, int(t_end / cfg.IK_dt)]
# compute limb-rrt path :
pathId = limb_rrt.generateLimbRRTPath(q_init, q_end, phase_previous, phase,
phase_next, fullBody)
if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT:
from hpp.gepetto import PathPlayer
pp = PathPlayer(viewer)
if DISPLAY_JOINT_LEVEL:
pp.displayPath(pathId, jointName=fullBody.getLinkNames(eeName)[0])
else:
#TODO
pp.displayPath(
pathId,
jointName=fullBody.getLinkNames(eeName)[0],
offset=cfg.Robot.dict_offset[eeName].translation.tolist())
# compute constraints for the end effector trajectories :
pData = bezier_com.ProblemData()
pData.c0_ = bezier_takeoff(bezier_takeoff.max())
pData.dc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 1)
pData.ddc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 2)
pData.j0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 3)
pData.c1_ = bezier_landing(0)
pData.dc1_ = bezier_landing.derivate(0, 1)
pData.ddc1_ = bezier_landing.derivate(0, 2)
pData.j1_ = bezier_landing.derivate(0, 3)
pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK
# now compute additional inequalities around the rrt path :
pDef = computeProblemConstraints(pData, fullBody, pathId, t_middle, eeName,
viewer)
solved = False
# loop and increase the number of variable control point until a solution is found
flagData = pData.constraints_.flag_
flags = [
bezier_com.ConstraintFlag.ONE_FREE_VAR, None,
bezier_com.ConstraintFlag.THREE_FREE_VAR, None,
bezier_com.ConstraintFlag.FIVE_FREE_VAR
]
numVars = 1
while not solved and numVars <= 5:
pData.constraints_.flag_ = flagData | flags[numVars - 1]
ineqEff = bezier_com.computeEndEffectorConstraints(pData, t_middle)
Hg = bezier_com.computeEndEffectorVelocityCost(pData, t_middle)
res = bezier_com.computeEndEffector(
pData, t_middle) #only used for comparison/debug ?
bezier_unconstrained = res.c_of_t
pDef.degree = bezier_unconstrained.degree
ineqConstraints = curves.generate_problem(pDef)
# _ prefix = previous notation (in bezier_com_traj)
# min x' H x + 2 g' x
# subject to A*x <= b
_A = np.vstack([ineqEff.A, ineqConstraints.A])
_b = np.vstack([ineqEff.b, ineqConstraints.b])
_H = Hg.A
_g = Hg.b
#_H = ineqConstraints.cost.A
#_g = ineqConstraints.cost.b
# quadprog notation :
#min (1/2)x' P x + q' x
#subject to G x <= h
#subject to C x = d
G = _A
h = _b.reshape((-1))
P = _H * 2.
q = (_g * 2.).flatten()
try:
if VERBOSE:
print("try to solve quadprog with " + str(numVars) +
" variable")
res = quadprog_solve_qp(P, q, G, h)
solved = True
except ValueError:
if VERBOSE:
print("Failed.")
numVars += 2
if solved:
if VERBOSE:
print("Succeed.")
else:
print(
"Constrained End effector Bezier method failed for all numbers of variables control points."
)
print("Return predef trajectory (may be in collision).")
return
# retrieve the result of quadprog and create a bezier curve :
vars = np.split(res, numVars)
wps = bezier_unconstrained.waypoints()
id_firstVar = 4
i = id_firstVar
for x in vars:
wps[:, i] = x
i += 1
bezier_middle = curves.bezier(wps, 0., t_middle)
# create concatenation with takeoff/landing
curves = predef_curves.curves[::]
curves[id_middle] = bezier_middle
pBezier = PolyBezier(curves)
if VERBOSE:
print("time interval = ", time_interval[1] - time_interval[0])
print("polybezier length = ", pBezier.max())
ref_traj = trajectories.BezierTrajectory(pBezier, placement_init,
placement_end, time_interval)
return ref_traj