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
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"
            )
    if cfg.EFF_T_PREDEF > 0:
        predef_curves = generatePredefBeziers(time_interval, placement_init,
                                              placement_end)
    else:
        predef_curves = generateSmoothBezierTraj(time_interval, placement_init,
                                                 placement_end).curves
    id_middle = int(math.floor(len(predef_curves.curves) / 2.))
    predef_middle = predef_curves.curves[id_middle]
    pos_init = predef_middle(0)
    pos_end = predef_middle(predef_middle.max())
    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_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(math.floor(t_begin /
                                   cfg.IK_dt))]  # after the predef takeoff
    id_end = int(math.ceil(t_end / cfg.IK_dt)) - 1
    if id_end >= q_t.shape[
            1]:  # FIXME : why does it happen ? usually it's == to the size when the bug occur
        id_end = q_t.shape[1] - 1
    q_end = q_t[:, id_end]
    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 VERBOSE:
        print "weights_var id = ", 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]
    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_ = predef_middle(0)
    pData.dc0_ = predef_middle.derivate(0, 1)
    pData.ddc0_ = predef_middle.derivate(0, 2)
    pData.j0_ = predef_middle.derivate(0, 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)
    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))
Ejemplo n.º 3
0
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