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
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
t = ps.solve ()
print "Guide planning time : ",t
#v.solveAndDisplay("rm",10,0.01)
for i in range(20):
print "Optimize path, "+str(i+1)+"/20 ... "
ps.optimizePath(ps.numberPaths()-1)
pathId = ps.numberPaths()-1
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.01
pp.displayPath(pathId)
v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(pathId)
v.client.gui.writeNodeFile("path_"+str(pathId)+"_root","guide_path_maze_hard.obj")
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
def generateLimbRRTOptimizedTraj(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(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"
)
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.))
pos_init = bezier_takeoff(bezier_takeoff.max())
pos_end = bezier_landing(0)
if VERBOSE:
print "generateLimbRRTOptimizedTraj, try number " + str(numTry)
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)]
global current_limbRRT_id
# compute new limb-rrt path if needed:
if not current_limbRRT_id or (numTry in recompute_rrt_at_tries):
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],
offset=cfg.Robot.dict_offset[eeName].translation.T.tolist()[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
if id >= len(weights_vars) - 1:
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]
if VERBOSE:
print "use weight " + str(weight) + " with num free var = " + str(
numVars)
# 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 | 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)
if VERBOSE > 1:
print "A = ", _A
print "b = ", _b
print "H = ", _H
print "h = ", _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()
P = _H * 2.
q = (_g * 2.).flatten()
# solve the QP :
solved = False
try:
res = quadprog_solve_qp(P, q, G, h)
solved = True
except ValueError, e:
print "Quadprog error : "
print e.message
raise ValueError(
"Quadprog failed to solve QP for optimized limb-RRT end-effector trajectory, for try number "
+ str(numTry))
#ps.selectSteeringMethod("RBPRMKinodynamic")
#ps.selectDistance("Kinodynamic")
#ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
success = ps.client.problem.prepareSolveStepByStep()
if not success:
print "planning failed."
import sys
sys.exit(1)
ps.client.problem.finishSolveStepByStep()
try:
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.1
pp.displayPath(0) #pp.displayVelocityPath(0) #
#v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(0)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
r.solveAndDisplay("rm",1,0.02)
#t = ps.solve ()
#r.displayRoadmap("rm",0.02)
r.displayPathMap("rmPath",0,0.02)
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.displayPath(0,r.color.lightGreen)
pp(0)
pp.displayPath(1,blue)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.displayPath(1,black)
pp (1)
#r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmPath",r.sceneName)
r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
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)
q_init = q_t(t_begin)
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
v.client.gui.removeFromGroup("rmPath", v.sceneName)
# display roadmap for the tools, the full prototype is :
# displayRoadmap(name,sizeNode,sizeAxis,colorNode,colorEdge,JointName)
# The joint defined by "jointName" (see robot.getAllJointNames()) is used to compute the position of the node of the roadmap
# You can use the colors defined in viewer.color or give an array of 4 float which define a normalized RGBA color
v.displayRoadmap("rmR", 0.02, 1, v.color.blue, v.color.lightBlue,
'r_gripper_tool_joint')
v.displayRoadmap("rmL", 0.02, 1, v.color.green, v.color.lightGreen,
'l_gripper_tool_joint')
# hide roadmap in the scene
#v.client.gui.removeFromGroup("rmL",v.sceneName)
#v.client.gui.removeFromGroup("rmR",v.sceneName)
# alternative method : replace ps.solve() and v.displayRoadmap() with :
# v.solveAndDisplay("rmL1",2,0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint')
# v.displayRoadmap( "rmR1" ,0.02,1,v.color.blue ,v.color.lightBlue ,'r_gripper_tool_joint')
# solveAndDisplay show the construction of the roadmap, the first int set the refresh rate of the display (here, every 2 iteration of the algorithm)
################################################################
pp = PathPlayer(robot.client, v)
#display path
# The edge of the roadmap are always straight line, if you want to display the accurate trajectory of a body of the robot, you should use the following method :
# displayPath(pathId, color(default = green), jointName (default = root)
pp.displayPath(0, v.color.lightBlack, 'r_gripper_tool_joint')
pp(0)
#display path after optimisation
pp.displayPath(1, v.color.lightRed, 'r_gripper_tool_joint')
pp(1)
# You can use the colors defined in viewer.color or give an array of 4 float which define a normalized RGBA color
v.displayRoadmap("rmR",0.02,1,v.color.blue,v.color.lightBlue,'r_gripper_tool_joint')
v.displayRoadmap("rmL",0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint')
# hide roadmap in the scene
#v.client.gui.removeFromGroup("rmL",v.sceneName)
#v.client.gui.removeFromGroup("rmR",v.sceneName)
# alternative method : replace ps.solve() and v.displayRoadmap() with :
# v.solveAndDisplay("rmL1",2,0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint')
# v.displayRoadmap( "rmR1" ,0.02,1,v.color.blue ,v.color.lightBlue ,'r_gripper_tool_joint')
# solveAndDisplay show the construction of the roadmap, the first int set the refresh rate of the display (here, every 2 iteration of the algorithm)
################################################################
pp = PathPlayer (robot.client, v)
#display path
# The edge of the roadmap are always straight line, if you want to display the accurate trajectory of a body of the robot, you should use the following method :
# displayPath(pathId, color(default = green), jointName (default = root)
pp.displayPath(0 , v.color.lightBlack ,'r_gripper_tool_joint')
pp (0)
#display path after optimisation
pp.displayPath(1,v.color.lightRed,'r_gripper_tool_joint')
pp (1)
