def generatePredefBeziers(cfg, time_interval, placement_init, placement_end):
t_total = time_interval[1] - time_interval[0] - 2 * cfg.EFF_T_DELAY
logger.info("Generate Bezier Traj :")
logger.info("placement Init = %s", placement_init)
logger.info("placement End = %s", placement_end)
logger.info("time interval = %s", time_interval)
# generate two curves for the takeoff/landing :
t_takeoff_min = time_interval[0] + cfg.EFF_T_DELAY
t_takeoff_max = t_takeoff_min + cfg.EFF_T_PREDEF
t_landing_max = time_interval[1] - cfg.EFF_T_DELAY
t_landing_min = t_landing_max - cfg.EFF_T_PREDEF
bezier_takeoff = buildPredefinedInitTraj(cfg, placement_init, t_total,
t_takeoff_min, t_takeoff_max)
bezier_landing = buildPredefinedFinalTraj(cfg, placement_end, t_total,
t_landing_min, t_landing_max)
t_middle = (t_total - (2. * cfg.EFF_T_PREDEF))
assert t_middle >= 0.1 and "Duration of swing phase too short for effector motion. Change the values of predef motion for effector or the duration of the contact phase. "
bezier_middle = generatePredefMiddle(bezier_takeoff, bezier_landing,
t_takeoff_max, t_landing_min)
curves = piecewise_SE3()
# create polybezier with concatenation of the 3 (or 5) curves :
# create constant curve at the beginning and end for the delay :
if cfg.EFF_T_DELAY > 0:
bezier_init_zero = bezier(
bezier_takeoff(bezier_takeoff.min()).reshape([-1, 1]),
time_interval[0], t_takeoff_min)
# Create SE3 curves with translation and duration defined from the bezier and constant orientation:
curves.append(
SE3Curve(bezier_init_zero, placement_init.rotation,
placement_init.rotation))
curves.append(
SE3Curve(bezier_takeoff, placement_init.rotation,
placement_init.rotation))
curves.append(
SE3Curve(bezier_middle, placement_init.rotation,
placement_end.rotation))
curves.append(
SE3Curve(bezier_landing, placement_end.rotation,
placement_end.rotation))
if cfg.EFF_T_DELAY > 0:
bezier_end_zero = bezier(
bezier_landing(bezier_landing.max()).reshape([-1, 1]),
t_landing_max, time_interval[1])
# Create SE3 curves with translation and duration defined from the bezier and constant orientation:
curves.append(
SE3Curve(bezier_end_zero, placement_end.rotation,
placement_end.rotation))
return curves
def update_root_traj_timings(cs):
"""
Update all the root_t trajectories of all contact phases of the given CS.
The new trajectories are linear interpolation from the initial to the final placement,
with a time definition matching the time definition of the contact phase
:param cs: The contact sequence to update
"""
for cp in cs.contactPhases:
cp.root_t = SE3Curve(cp.root_t(cp.root_t.min()), cp.root_t(cp.root_t.max()), cp.timeInitial, cp.timeFinal)
def computeRootTrajFromConfigurations(cs):
"""
Add root_t trajectories to each contact phases in the sequences. This trajecories are linear interpolation in SE3
from the initial root positon in q_init to it's final position in q_final, with the same duration as the phases.
:param cs: the ContactSequence to modify
"""
assert cs.haveConfigurationsValues(), "computeRootTrajFromConfigurations require haveConfigurationsValues"
for phase in cs.contactPhases:
p_init = SE3FromConfig(phase.q_init)
p_final = SE3FromConfig(phase.q_final)
phase.root_t = SE3Curve(p_init, p_final, phase.timeInitial, phase.timeFinal)
def constantSE3curve(placement, t_min, t_max = None):
"""
Create a constant SE3_curve at the given placement for the given duration
:param placement: the placement
:param t_min: the initial time
:param t_max: final time, if not provided the curve will have a duration of 0
:return: the constant curve
"""
if t_max is None:
t_max = t_min
rot = SO3Linear(placement.rotation, placement.rotation, t_min, t_max)
trans = polynomial(placement.translation.reshape(-1,1), t_min, t_max)
return SE3Curve(trans, rot)
def generateSmoothBezierTrajWithoutPredef(cfg, time_interval, placement_init,
placement_end):
t_tot = time_interval[1] - time_interval[0]
wps = np.zeros([3, 9])
for i in range(4): # init position. init vel,acc and jerk == 0
wps[:, i] = placement_init.translation.copy()
# compute mid point (average and offset along z)
wps[:, 4] = (placement_init.translation + placement_end.translation) / 2.
wps[2, 4] += cfg.p_max
for i in range(5, 9): # final position. final vel,acc and jerk == 0
wps[:, i] = placement_end.translation.copy()
translation = bezier(wps, time_interval[0], time_interval[1])
pBezier = piecewise_SE3(
SE3Curve(translation, placement_init.rotation, placement_end.rotation))
return pBezier
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 computeRootTrajFromContacts(Robot, cs):
"""
Add root_t trajectories to each contact phases in the sequences. This trajecories are linear interpolation in SE3
computed from the previous/next contact positions:
The initial orientation is a mean between both feet contact position in the current (or previous) phase
the final orientation is considering the new contact position of the moving feet
:param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot)
:param cs: the ContactSequence to modifu
"""
#Quaternion(rootOrientationFromFeetPlacement(phase, None)[0].rotation)
for pid in range(cs.size()):
phase = cs.contactPhases[pid]
if pid > 0:
previous_phase = cs.contactPhases[pid-1]
else:
previous_phase = None
if pid < (cs.size()-1):
next_phase = cs.contactPhases[pid+1]
else:
next_phase = None
p_init, p_final = rootOrientationFromFeetPlacement(Robot, previous_phase, phase, next_phase)
phase.root_t = SE3Curve(p_init, p_final, phase.timeInitial, phase.timeFinal)
def generate_effector_trajectory_limb_rrt_optimized(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 q_t is None or phase_previous is None or phase is None or phase_next is None or not fullBody or not eeName:
raise ValueError(
"Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments"
)
if cfg.EFF_T_PREDEF > 0:
predef_curves = generatePredefBeziers(cfg, time_interval,
placement_init, placement_end)
else:
predef_curves = generateSmoothBezierTraj(cfg, time_interval,
placement_init, placement_end)
id_middle = int(math.floor(predef_curves.num_curves() / 2.))
predef_middle = predef_curves.curve_at_index(id_middle).translation_curve()
pos_init = predef_middle(predef_middle.min())
pos_end = predef_middle(predef_middle.max())
logger.warning("generateLimbRRTOptimizedTraj, try number %d", numTry)
logger.info("bezier takeoff end : %s", pos_init)
logger.info("bezier landing init : %s", pos_end)
t_begin = predef_middle.min()
t_end = predef_middle.max()
t_middle = t_end - t_begin
logger.info("t begin : %f", t_begin)
logger.info("t end : %f", t_end)
logger.info("q_t min : %f", q_t.min())
logger.info("q_t max : %f", q_t.max())
q_init = q_t(t_begin)
if t_end > q_t.max():
t_end = q_t.max()
q_end = q_t(t_end)
global current_limbRRT_id
# compute new limb-rrt path if needed:
if not current_limbRRT_id or (numTry in recompute_rrt_at_tries):
logger.warning("Compute new limb-rrt path ...")
current_limbRRT_id = generateLimbRRTPath(q_init, q_end, phase_previous,
phase, phase_next, fullBody)
if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT_PATH:
from hpp.gepetto import PathPlayer
pp = PathPlayer(viewer)
pp.displayPath(current_limbRRT_id,
jointName=fullBody.getLinkNames(eeName)[0])
# find weight and number of variable to use from the numTry :
for offset in reversed(recompute_rrt_at_tries):
if numTry >= offset:
id = numTry - offset
break
logger.info("weights_var id = %d", id)
if id >= len(weights_vars):
raise ValueError(
"Max number of try allow to find a collision-end effector trajectory reached."
)
weight = weights_vars[id][0]
varFlag = weights_vars[id][1]
numVars = weights_vars[id][2]
logger.warning("use weight %f with num free var = %d", weight, numVars)
# compute constraints for the end effector trajectories :
pData = bezier_com.ProblemData()
pData.c0_ = predef_middle(predef_middle.min())
pData.dc0_ = predef_middle.derivate(predef_middle.min(), 1)
pData.ddc0_ = predef_middle.derivate(predef_middle.min(), 2)
pData.j0_ = predef_middle.derivate(predef_middle.min(), 3)
pData.c1_ = predef_middle(predef_middle.max())
pData.dc1_ = predef_middle.derivate(predef_middle.max(), 1)
pData.ddc1_ = predef_middle.derivate(predef_middle.max(), 2)
pData.j1_ = predef_middle.derivate(predef_middle.max(), 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 \
| varFlag
Constraints = bezier_com.computeEndEffectorConstraints(pData, t_middle)
Cost_smooth = bezier_com.computeEndEffectorVelocityCost(pData, t_middle)
Cost_distance = computeDistanceCostMatrices(fullBody, current_limbRRT_id,
pData, t_middle, eeName)
# formulate QP matrices :
# _ prefix = previous notation (in bezier_com_traj)
# min x' H x + 2 g' x
# subject to A*x <= b
_A = Constraints.A
_b = Constraints.b
_H = ((1. - weight) * Cost_smooth.A + weight * Cost_distance.A)
_g = ((1. - weight) * Cost_smooth.b + weight * Cost_distance.b)
logger.debug("A = %s", _A)
logger.debug("b = %s", _b)
logger.debug("H = %s", _H)
logger.debug("h = %s", _g)
"""
_A = np.array(_A)
_b = np.array(_b)
_H = np.array(_H)
_g = np.array(_g)
"""
# quadprog notation :
#min (1/2)x' P x + q' x
#subject to G x <= h
#subject to C x = d
G = _A
h = _b.flatten() # remove the transpose when working with array
P = _H * 2.
q = (_g * 2.).flatten()
logger.debug("G = %s", G)
logger.debug("h = %s", h)
logger.debug("P = %s", P)
logger.debug("q = %s", q)
logger.debug("Shapes : ")
logger.debug("G : %s", G.shape)
logger.debug("h : %s", h.shape)
logger.debug("P : %s", P.shape)
logger.debug("q : %s", q.shape)
# solve the QP :
solved = False
try:
res = quadprog_solve_qp(P, q, G, h)
solved = True
except ValueError as e:
logger.error("Quadprog error : ", exc_info=e)
raise ValueError(
"Quadprog failed to solve QP for optimized limb-RRT end-effector trajectory, for try number "
+ str(numTry))
logger.info("Quadprog solved.")
# build a bezier curve from the result of quadprog :
vars = np.split(res, numVars)
wps = bezier_com.computeEndEffectorConstantWaypoints(
pData, t_middle) # one wp per column
logger.debug("Constant waypoints computed.")
id_firstVar = 4 # depend on the flag defined above, but for end effector we always use this ones ...
i = id_firstVar
for x in vars:
wps[:, i] = np.array(x)
logger.debug("waypoint number %d : %s", i, wps[:, i])
i += 1
logger.debug("Variables waypoints replaced by quadprog results.")
bezier_middle = bezier(wps, t_begin, t_end)
# create concatenation with takeoff/landing
pBezier = piecewise_SE3()
for ci in range(predef_curves.num_curves()):
if ci == id_middle:
pBezier.append(
SE3Curve(bezier_middle, placement_init.rotation,
placement_end.rotation))
else:
pBezier.append(predef_curves.curve_at_index(ci))
logger.info("time interval = [%f ; %f]", pBezier.min(), pBezier.max())
return pBezier