コード例 #1
0
def generateWholeBodyMotion(cs, fullBody=None, viewer=None):
    if not viewer:
        print "No viewer linked, cannot display end_effector trajectories."
    print "Start TSID ... "

    rp = RosPack()
    urdf = rp.get_path(
        cfg.Robot.packageName
    ) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
    if cfg.WB_VERBOSE:
        print "load robot : ", urdf
    #srdf = "package://" + package + '/srdf/' +  cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
    robot = tsid.RobotWrapper(urdf, pin.StdVec_StdString(),
                              pin.JointModelFreeFlyer(), False)
    if cfg.WB_VERBOSE:
        print "robot loaded in tsid."
    # FIXME : tsid robotWrapper don't have all the required methods, only pinocchio have them
    pinRobot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
                                              pin.JointModelFreeFlyer(), False)

    q = cs.contact_phases[0].reference_configurations[0][:robot.nq].copy()
    v = np.matrix(np.zeros(robot.nv)).transpose()
    t = 0.0  # time
    # init states list with initial state (assume joint velocity is null for t=0)
    invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
    invdyn.computeProblemData(t, q, v)
    data = invdyn.data()

    if cfg.EFF_CHECK_COLLISION:  # initialise object needed to check the motion
        from mlp.utils import check_path
        validator = check_path.PathChecker(fullBody, cs, len(q),
                                           cfg.WB_VERBOSE)

    if cfg.WB_VERBOSE:
        print "initialize tasks : "
    comTask = tsid.TaskComEquality("task-com", robot)
    comTask.setKp(cfg.kp_com * np.matrix(np.ones(3)).transpose())
    comTask.setKd(2.0 * np.sqrt(cfg.kp_com) *
                  np.matrix(np.ones(3)).transpose())
    invdyn.addMotionTask(comTask, cfg.w_com, cfg.level_com, 0.0)

    com_ref = robot.com(invdyn.data())
    trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)

    amTask = tsid.TaskAMEquality("task-am", robot)
    amTask.setKp(cfg.kp_am * np.matrix([1., 1., 0.]).T)
    amTask.setKd(2.0 * np.sqrt(cfg.kp_am * np.matrix([1., 1., 0.]).T))
    invdyn.addTask(amTask, cfg.w_am, cfg.level_am)
    trajAM = tsid.TrajectoryEuclidianConstant("traj_am",
                                              np.matrix(np.zeros(3)).T)

    postureTask = tsid.TaskJointPosture("task-joint-posture", robot)
    postureTask.setKp(cfg.kp_posture * cfg.gain_vector)
    postureTask.setKd(2.0 * np.sqrt(cfg.kp_posture * cfg.gain_vector))
    postureTask.mask(cfg.masks_posture)
    invdyn.addMotionTask(postureTask, cfg.w_posture, cfg.level_posture, 0.0)
    q_ref = cfg.IK_REFERENCE_CONFIG
    trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref[7:])

    orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot,
                                               'root_joint')
    mask = np.matrix(np.ones(6)).transpose()
    mask[0:3] = 0
    mask[5] = cfg.YAW_ROT_GAIN
    orientationRootTask.setKp(cfg.kp_rootOrientation * mask)
    orientationRootTask.setKd(2.0 * np.sqrt(cfg.kp_rootOrientation * mask))
    invdyn.addMotionTask(orientationRootTask, cfg.w_rootOrientation,
                         cfg.level_rootOrientation, 0.0)
    root_ref = robot.position(data, robot.model().getJointId('root_joint'))
    trajRoot = tsid.TrajectorySE3Constant("traj-root", root_ref)

    usedEffectors = []
    for eeName in cfg.Robot.dict_limb_joint.values():
        if isContactEverActive(cs, eeName):
            usedEffectors.append(eeName)
    # init effector task objects :
    dic_effectors_tasks = createEffectorTasksDic(cs, robot)
    effectorTraj = tsid.TrajectorySE3Constant(
        "traj-effector", SE3.Identity()
    )  # trajectory doesn't matter as it's only used to get the correct struct and size

    # init empty dic to store effectors trajectories :
    dic_effectors_trajs = {}
    for eeName in usedEffectors:
        dic_effectors_trajs.update({eeName: None})

    # add initial contacts :
    dic_contacts = {}
    for eeName in usedEffectors:
        if isContactActive(cs.contact_phases[0], eeName):
            contact = createContactForEffector(invdyn, robot,
                                               cs.contact_phases[0], eeName)
            dic_contacts.update({eeName: contact})

    if cfg.PLOT:  # init a dict storing all the reference trajectories used (for plotting)
        stored_effectors_ref = {}
        for eeName in dic_effectors_tasks:
            stored_effectors_ref.update({eeName: []})

    solver = tsid.SolverHQuadProg("qp solver")
    solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)

    # define nested function used in control loop
    def storeData(k_t, res, q, v, dv, invdyn, sol):
        # store current state
        res.q_t[:, k_t] = q
        res.dq_t[:, k_t] = v
        res.ddq_t[:, k_t] = dv
        res.tau_t[:, k_t] = invdyn.getActuatorForces(
            sol)  # actuator forces, with external forces (contact forces)
        #store contact info (force and status)
        if cfg.IK_store_contact_forces:
            for eeName, contact in dic_contacts.iteritems():
                if invdyn.checkContact(contact.name, sol):
                    res.contact_forces[eeName][:,
                                               k_t] = invdyn.getContactForce(
                                                   contact.name, sol)
                    res.contact_normal_force[
                        eeName][:, k_t] = contact.getNormalForce(
                            res.contact_forces[eeName][:, k_t])
                    res.contact_activity[eeName][:, k_t] = 1
        # store centroidal info (real one and reference) :
        if cfg.IK_store_centroidal:
            pcom, vcom, acom = pinRobot.com(q, v, dv)
            res.c_t[:, k_t] = pcom
            res.dc_t[:, k_t] = vcom
            res.ddc_t[:, k_t] = acom
            res.L_t[:, k_t] = pinRobot.centroidalMomentum(q, v).angular
            #res.dL_t[:,k_t] = pinRobot.centroidalMomentumVariation(q,v,dv) # FIXME : in robot wrapper, use * instead of .dot() for np matrices
            pin.dccrba(pinRobot.model, pinRobot.data, q, v)
            res.dL_t[:, k_t] = Force(
                pinRobot.data.Ag.dot(dv) + pinRobot.data.dAg.dot(v)).angular
            # same for reference data :
            res.c_reference[:, k_t] = com_desired
            res.dc_reference[:, k_t] = vcom_desired
            res.ddc_reference[:, k_t] = acom_desired
            res.L_reference[:, k_t] = L_desired
            res.dL_reference[:, k_t] = dL_desired
            if cfg.IK_store_zmp:
                tau = pin.rnea(
                    pinRobot.model, pinRobot.data, q, v, dv
                )  # tau without external forces, only used for the 6 first
                #res.tau_t[:6,k_t] = tau[:6]
                phi0 = pinRobot.data.oMi[1].act(Force(tau[:6]))
                res.wrench_t[:, k_t] = phi0.vector
                res.zmp_t[:, k_t] = shiftZMPtoFloorAltitude(
                    cs, res.t_t[k_t], phi0, cfg.EXPORT_OPENHRP)
                # same but from the 'reference' values :
                Mcom = SE3.Identity()
                Mcom.translation = com_desired
                Fcom = Force.Zero()
                Fcom.linear = cfg.MASS * (acom_desired - cfg.GRAVITY)
                Fcom.angular = dL_desired
                F0 = Mcom.act(Fcom)
                res.wrench_reference[:, k_t] = F0.vector
                res.zmp_reference[:, k_t] = shiftZMPtoFloorAltitude(
                    cs, res.t_t[k_t], F0, cfg.EXPORT_OPENHRP)
        if cfg.IK_store_effector:
            for eeName in usedEffectors:  # real position (not reference)
                res.effector_trajectories[eeName][:, k_t] = SE3toVec(
                    getCurrentEffectorPosition(robot, invdyn.data(), eeName))
                res.d_effector_trajectories[eeName][:, k_t] = MotiontoVec(
                    getCurrentEffectorVelocity(robot, invdyn.data(), eeName))
                res.dd_effector_trajectories[eeName][:, k_t] = MotiontoVec(
                    getCurrentEffectorAcceleration(robot, invdyn.data(),
                                                   eeName))

        return res

    def printIntermediate(v, dv, invdyn, sol):
        print "Time %.3f" % (t)
        for eeName, contact in dic_contacts.iteritems():
            if invdyn.checkContact(contact.name, sol):
                f = invdyn.getContactForce(contact.name, sol)
                print "\tnormal force %s: %.1f" % (contact.name.ljust(
                    20, '.'), contact.getNormalForce(f))

        print "\ttracking err %s: %.3f" % (comTask.name.ljust(
            20, '.'), norm(comTask.position_error, 2))
        for eeName, traj in dic_effectors_trajs.iteritems():
            if traj:
                task = dic_effectors_tasks[eeName]
                error = task.position_error
                if cfg.Robot.cType == "_3_DOF":
                    error = error[0:3]
                print "\ttracking err %s: %.3f" % (task.name.ljust(
                    20, '.'), norm(error, 2))
        print "\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv))

    def checkDiverge(res, v, dv):
        if norm(dv) > 1e6 or norm(v) > 1e6:
            print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
            print "/!\ ABORT : controler unstable at t = " + str(t) + "  /!\ "
            print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
            raise ValueError("ABORT : controler unstable at t = " + str(t))
        if math.isnan(norm(dv)) or math.isnan(norm(v)):
            print "!!!!!!    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
            print "/!\ ABORT : nan   at t = " + str(t) + "  /!\ "
            print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
            raise ValueError("ABORT : controler unstable at t = " + str(t))

    def stopHere():
        if cfg.WB_ABORT_WHEN_INVALID:
            return res.resize(phase_interval[0]), pinRobot
        elif cfg.WB_RETURN_INVALID:
            return res.resize(k_t), pinRobot

    # time check
    dt = cfg.IK_dt
    if cfg.WB_VERBOSE:
        print "dt : ", dt
    res = Result(robot.nq, robot.nv, cfg.IK_dt, eeNames=usedEffectors, cs=cs)
    N = res.N
    last_display = 0
    if cfg.WB_VERBOSE:
        print "tsid initialized, start control loop"
        #raw_input("Enter to start the motion (motion displayed as it's computed, may be slower than real-time)")
    time_start = time.time()

    # For each phases, create the necessary task and references trajectories :
    for pid in range(cs.size()):
        if cfg.WB_VERBOSE:
            print "## for phase : ", pid
            print "t = ", t
        phase = cs.contact_phases[pid]
        if pid < cs.size() - 1:
            phase_next = cs.contact_phases[pid + 1]
        else:
            phase_next = None
        if pid > 0:
            phase_prev = cs.contact_phases[pid - 1]
        else:
            phase_prev = None
        t_phase_begin = res.phases_intervals[pid][0] * dt
        t_phase_end = res.phases_intervals[pid][-1] * dt
        time_interval = [t_phase_begin, t_phase_end]
        if cfg.WB_VERBOSE:
            print "time_interval ", time_interval
        # generate com ref traj from phase :
        com_init = np.matrix(np.zeros((9, 1)))
        com_init[0:3, 0] = robot.com(invdyn.data())
        com_traj = computeCOMRefFromPhase(phase, time_interval)

        am_traj = computeAMRefFromPhase(phase, time_interval)
        # add root's orientation ref from reference config :
        if cfg.USE_PLANNING_ROOT_ORIENTATION:
            if phase_next:
                root_traj = trajectories.TrajectorySE3LinearInterp(
                    SE3FromConfig(phase.reference_configurations[0]),
                    SE3FromConfig(phase_next.reference_configurations[0]),
                    time_interval)
            else:
                root_traj = trajectories.TrajectorySE3LinearInterp(
                    SE3FromConfig(phase.reference_configurations[0]),
                    SE3FromConfig(phase.reference_configurations[0]),
                    time_interval)
        else:
            # orientation such that the torso orientation is the mean between both feet yaw rotations:
            placement_init, placement_end = rootOrientationFromFeetPlacement(
                phase, phase_next)
            root_traj = trajectories.TrajectorySE3LinearInterp(
                placement_init, placement_end, time_interval)
        # add newly created contacts :
        for eeName in usedEffectors:
            if phase_prev and not isContactActive(
                    phase_prev, eeName) and isContactActive(phase, eeName):
                invdyn.removeTask(dic_effectors_tasks[eeName].name,
                                  0.0)  # remove pin task for this contact
                dic_effectors_trajs.update(
                    {eeName:
                     None})  # delete reference trajectory for this task
                if cfg.WB_VERBOSE:
                    print "remove se3 task : " + dic_effectors_tasks[
                        eeName].name
                contact = createContactForEffector(invdyn, robot, phase,
                                                   eeName)
                dic_contacts.update({eeName: contact})

        # add se3 tasks for end effector not in contact that will be in contact next phase:
        for eeName, task in dic_effectors_tasks.iteritems():
            if phase_next and not isContactActive(
                    phase, eeName) and isContactActive(phase_next, eeName):
                if cfg.WB_VERBOSE:
                    print "add se3 task for " + eeName
                invdyn.addMotionTask(task, cfg.w_eff, cfg.level_eff, 0.0)
                #create reference trajectory for this task :
                placement_init = getCurrentEffectorPosition(
                    robot, invdyn.data(), eeName
                )  #FIXME : adjust orientation in case of 3D contact ...
                if cfg.Robot.cType == "_3_DOF":
                    placement_init.rotation = JointPlacementForEffector(
                        phase, eeName).rotation
                placement_end = JointPlacementForEffector(phase_next, eeName)
                ref_traj = generateEndEffectorTraj(time_interval,
                                                   placement_init,
                                                   placement_end, 0)
                if cfg.WB_VERBOSE:
                    print "t interval : ", time_interval
                    print "placement Init : ", placement_init
                    print "placement End  : ", placement_end
                    # display all the effector trajectories for this phase
                if viewer and cfg.DISPLAY_ALL_FEET_TRAJ:
                    display_tools.displaySE3Traj(
                        ref_traj, viewer.client.gui, viewer.sceneName,
                        eeName + "_traj_" + str(pid),
                        cfg.Robot.dict_limb_color_traj[eeName], time_interval,
                        cfg.Robot.dict_offset[eeName])
                    viewer.client.gui.setVisibility(
                        eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP')
                dic_effectors_trajs.update({eeName: ref_traj})

        # start removing the contact that will be broken in the next phase :
        # (This tell the solver that it should start minimzing the contact force on this contact, and ideally get to 0 at the given time)
        for eeName, contact in dic_contacts.iteritems():
            if phase_next and isContactActive(
                    phase, eeName) and not isContactActive(phase_next, eeName):
                transition_time = t_phase_end - t - dt / 2.
                if cfg.WB_VERBOSE:
                    print "\nTime %.3f Start breaking contact %s. transition time : %.3f\n" % (
                        t, contact.name, transition_time)
                invdyn.removeRigidContact(contact.name, transition_time)

        if cfg.WB_STOP_AT_EACH_PHASE:
            raw_input('start simulation')
        # save values at the beginning of the current phase
        q_begin = q.copy()
        v_begin = v.copy()
        phaseValid = False
        swingPhase = False  # will be true if an effector move during this phase
        iter_for_phase = -1
        # iterate until a valid motion for this phase is found (ie. collision free and which respect joint-limits)
        while not phaseValid:
            if iter_for_phase >= 0:
                # reset values to their value at the beginning of the current phase
                q = q_begin.copy()
                v = v_begin.copy()
            iter_for_phase += 1
            if cfg.WB_VERBOSE:
                print "Start simulation for phase " + str(
                    pid) + ", try number :  " + str(iter_for_phase)
            # loop to generate states (q,v,a) for the current contact phase :
            if pid == cs.size() - 1:  # last state
                phase_interval = res.phases_intervals[pid]
            else:
                phase_interval = res.phases_intervals[pid][:-1]
            for k_t in phase_interval:
                t = res.t_t[k_t]
                # set traj reference for current time :
                # com
                sampleCom = trajCom.computeNext()
                com_desired = com_traj(t)[0]
                vcom_desired = com_traj(t)[1]
                acom_desired = com_traj(t)[2]
                sampleCom.pos(com_desired)
                sampleCom.vel(vcom_desired)
                sampleCom.acc(acom_desired)
                comTask.setReference(sampleCom)

                # am
                sampleAM = trajAM.computeNext()
                L_desired = am_traj(t)[0]
                dL_desired = am_traj(t)[1]
                sampleAM.pos(L_desired)
                sampleAM.vel(dL_desired)
                amTask.setReference(sampleAM)

                # posture
                samplePosture = trajPosture.computeNext()
                #print "postural task ref : ",samplePosture.pos()
                postureTask.setReference(samplePosture)

                # root orientation :
                sampleRoot = trajRoot.computeNext()
                sampleRoot.pos(SE3toVec(root_traj(t)[0]))
                sampleRoot.vel(MotiontoVec(root_traj(t)[1]))

                orientationRootTask.setReference(sampleRoot)
                quat_waist = Quaternion(root_traj(t)[0].rotation)
                res.waist_orientation_reference[:, k_t] = np.matrix(
                    [quat_waist.x, quat_waist.y, quat_waist.z, quat_waist.w]).T
                if cfg.WB_VERBOSE == 2:
                    print "### references given : ###"
                    print "com  pos : ", sampleCom.pos()
                    print "com  vel : ", sampleCom.vel()
                    print "com  acc : ", sampleCom.acc()
                    print "AM   pos : ", sampleAM.pos()
                    print "AM   vel : ", sampleAM.vel()
                    print "root pos : ", sampleRoot.pos()
                    print "root vel : ", sampleRoot.vel()

                # end effector (if they exists)
                for eeName, traj in dic_effectors_trajs.iteritems():
                    if traj:
                        swingPhase = True  # there is an effector motion in this phase
                        sampleEff = effectorTraj.computeNext()
                        traj_t = traj(t)
                        sampleEff.pos(SE3toVec(traj_t[0]))
                        sampleEff.vel(MotiontoVec(traj_t[1]))
                        dic_effectors_tasks[eeName].setReference(sampleEff)
                        if cfg.WB_VERBOSE == 2:
                            print "effector " + str(eeName) + " pos : " + str(
                                sampleEff.pos())
                            print "effector " + str(eeName) + " vel : " + str(
                                sampleEff.vel())
                        if cfg.IK_store_effector:
                            res.effector_references[eeName][:, k_t] = SE3toVec(
                                traj_t[0])
                            res.d_effector_references[
                                eeName][:, k_t] = MotiontoVec(traj_t[1])
                            res.dd_effector_references[
                                eeName][:, k_t] = MotiontoVec(traj_t[2])
                    elif cfg.IK_store_effector:
                        if k_t == 0:
                            res.effector_references[eeName][:, k_t] = SE3toVec(
                                getCurrentEffectorPosition(
                                    robot, invdyn.data(), eeName))
                        else:
                            res.effector_references[
                                eeName][:, k_t] = res.effector_references[
                                    eeName][:, k_t - 1]
                        res.d_effector_references[eeName][:, k_t] = np.matrix(
                            np.zeros(6)).T
                        res.dd_effector_references[eeName][:, k_t] = np.matrix(
                            np.zeros(6)).T

                # solve HQP for the current time
                HQPData = invdyn.computeProblemData(t, q, v)
                if cfg.WB_VERBOSE and t < phase.time_trajectory[0] + dt:
                    print "final data for phase ", pid
                    HQPData.print_all()

                sol = solver.solve(HQPData)
                dv = invdyn.getAccelerations(sol)
                res = storeData(k_t, res, q, v, dv, invdyn, sol)
                # update state
                v_mean = v + 0.5 * dt * dv
                v += dt * dv
                q = pin.integrate(robot.model(), q, dt * v_mean)

                if cfg.WB_VERBOSE == 2:
                    print "v = ", v
                    print "dv = ", dv

                if cfg.WB_VERBOSE and int(t / dt) % cfg.IK_PRINT_N == 0:
                    printIntermediate(v, dv, invdyn, sol)
                try:
                    checkDiverge(res, v, dv)
                except ValueError:
                    return stopHere()

            # end while t \in phase_t (loop for the current contact phase)
            if swingPhase and cfg.EFF_CHECK_COLLISION:
                phaseValid, t_invalid = validator.check_motion(
                    res.q_t[:, phase_interval[0]:k_t])
                #if iter_for_phase < 3 :# FIXME : debug only, force limb-rrt
                #    phaseValid = False
                if not phaseValid:
                    print "Phase " + str(pid) + " not valid at t = " + str(
                        t_invalid)
                    if t_invalid <= cfg.EFF_T_PREDEF or t_invalid >= (
                        (t_phase_end - t_phase_begin) - cfg.EFF_T_PREDEF):
                        print "Motion is invalid during predefined phases, cannot change this."
                        return stopHere()
                    if effectorCanRetry():
                        print "Try new end effector trajectory."
                        try:
                            for eeName, oldTraj in dic_effectors_trajs.iteritems(
                            ):
                                if oldTraj:  # update the traj in the map
                                    placement_init = JointPlacementForEffector(
                                        phase_prev, eeName)
                                    placement_end = JointPlacementForEffector(
                                        phase_next, eeName)
                                    ref_traj = generateEndEffectorTraj(
                                        time_interval, placement_init,
                                        placement_end, iter_for_phase + 1,
                                        res.q_t[:, phase_interval[0]:k_t],
                                        phase_prev, phase, phase_next,
                                        fullBody, eeName, viewer)
                                    dic_effectors_trajs.update(
                                        {eeName: ref_traj})
                                    if viewer and cfg.DISPLAY_ALL_FEET_TRAJ:
                                        display_tools.displaySE3Traj(
                                            ref_traj, viewer.client.gui,
                                            viewer.sceneName,
                                            eeName + "_traj_" + str(pid), cfg.
                                            Robot.dict_limb_color_traj[eeName],
                                            time_interval,
                                            cfg.Robot.dict_offset[eeName])
                                        viewer.client.gui.setVisibility(
                                            eeName + "_traj_" + str(pid),
                                            'ALWAYS_ON_TOP')
                        except ValueError, e:
                            print "ERROR in generateEndEffectorTraj :"
                            print e.message
                            return stopHere()
                    else:
                        print "End effector method choosen do not allow retries, abort here."
                        return stopHere()
            else:  # no effector motions, phase always valid (or bypass the check)
                phaseValid = True
                if cfg.WB_VERBOSE:
                    print "Phase " + str(pid) + " valid."
            if phaseValid:
                # display all the effector trajectories for this phase
                if viewer and cfg.DISPLAY_FEET_TRAJ and not cfg.DISPLAY_ALL_FEET_TRAJ:
                    for eeName, ref_traj in dic_effectors_trajs.iteritems():
                        if ref_traj:
                            display_tools.displaySE3Traj(
                                ref_traj, viewer.client.gui, viewer.sceneName,
                                eeName + "_traj_" + str(pid),
                                cfg.Robot.dict_limb_color_traj[eeName],
                                time_interval, cfg.Robot.dict_offset[eeName])
                            viewer.client.gui.setVisibility(
                                eeName + "_traj_" + str(pid), 'ALWAYS_ON_TOP')
                            if cfg.PLOT:  # add current ref_traj to the list for plotting
                                stored_effectors_ref[eeName] += [ref_traj]
コード例 #2
0
ファイル: demo_romeo.py プロジェクト: wxmerkt/tsid
rightFootTask = tsid.TaskSE3Equality("task-right-foot", robot, rf_frame_name)
rightFootTask.setKp(kp_RF * np.matrix(np.ones(6)).transpose())
rightFootTask.setKd(2.0 * np.sqrt(kp_com) * np.matrix(np.ones(6)).transpose())
invdyn.addMotionTask(rightFootTask, w_RF, 1, 0.0)

H_rf_ref.translation += np.matrix([0., 0., DELTA_FOOT_Z]).T
rightFootTraj = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)

com_ref = robot.com(data)
com_ref[1] += DELTA_COM_Y
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)

q_ref = q[7:]
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)

solver = tsid.SolverHQuadProg("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)

for i in range(0, N_SIMULATION):
    time_start = time.time()
    
    if i == REMOVE_CONTACT_N:
        print "\nTime %.3f Start breaking contact %s\n"%(t, contactRF.name)
        invdyn.removeRigidContact(contactRF.name, CONTACT_TRANSITION_TIME)

    sampleCom = trajCom.computeNext()
    comTask.setReference(sampleCom)
    samplePosture = trajPosture.computeNext()
    postureTask.setReference(samplePosture)
    sampleRightFoot = rightFootTraj.computeNext()
    rightFootTask.setReference(sampleRightFoot)
コード例 #3
0
def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None, robot=None, queue_qt = None):
    """
    Generate the whole body motion corresponding to the given contactSequence
    :param cs: Contact sequence containing the references,
     it will only be modified if the end effector trajectories are not valid.
     New references will be generated and added to the cs
    :param fullBody: Required to compute collision free end effector trajectories
    :param viewer: If provided, and the settings are enabled, display the end effector trajectories and the last step computed
    :param robot: a tsid.RobotWrapper instance. If None, a new one is created from the urdf files defined in cfg
    :param queue_qt: If not None, the joint trajectories are send to this multiprocessing.Queue during computation
        The queue take a tuple: [q_t (a Curve object), ContactPhase (may be None), Bool (True mean that this is the
         last trajectory of the motion)]
    :return: a new ContactSequence object, containing the wholebody trajectories,
    and the other trajectories computed from the wholebody motion request with cfg.IK_STORE_* and a robotWrapper instance
    """

    # define nested functions used in control loop #

    def appendJointsValues(first_iter_for_phase = False):
        """
        Append the current q value to the current phase.q_t trajectory
        :param first_iter_for_phase: if True, set the current phase.q_init value
        and initialize a new Curve for phase.q_t
        """
        if first_iter_for_phase:
            phase.q_init = q
            phase.q_t = piecewise(polynomial(q.reshape(-1,1), t, t))
            #phase.root_t = piecewise_SE3(constantSE3curve(SE3FromConfig(q) ,t))
        else:
            phase.q_t.append(q, t)
            #phase.root_t.append(SE3FromConfig(q), t)
        if queue_qt:
            queue_qt.put([phase.q_t.curve_at_index(phase.q_t.num_curves()-1),
                          phase.dq_t.curve_at_index(phase.dq_t.num_curves()-1),
                          None,
                          False])

    def appendJointsDerivatives(first_iter_for_phase=False):
        """
        Append the current v and dv value to the current phase.dq_t and phase.ddq_t trajectory
        :param first_iter_for_phase: if True, initialize a new Curve for phase.dq_t and phase.ddq_t
        """
        if first_iter_for_phase:
            phase.dq_t = piecewise(polynomial(v.reshape(-1, 1), t, t))
            phase.ddq_t = piecewise(polynomial(dv.reshape(-1, 1), t, t))
        else:
            phase.dq_t.append(v, t)
            phase.ddq_t.append(dv, t)

    def appendTorques(first_iter_for_phase = False):
        """
        Append the current tau value to the current phase.tau_t trajectory
        :param first_iter_for_phase: if True, initialize a new Curve for phase.tau_t
        """
        tau = invdyn.getActuatorForces(sol)
        if first_iter_for_phase:
            phase.tau_t = piecewise(polynomial(tau.reshape(-1,1), t, t))
        else:
            phase.tau_t.append(tau, t)

    def appendCentroidal(first_iter_for_phase = False):
        """
        Compute the values of the CoM position, velocity, acceleration, the anuglar momentum and it's derivative
        from the wholebody data and append them to the current phase trajectories
        :param first_iter_for_phase: if True, set the initial values for the current phase
        and initialize the centroidal trajectories
        """
        pcom, vcom, acom = pinRobot.com(q, v, dv)
        L = pinRobot.centroidalMomentum(q, v).angular
        dL = pin.computeCentroidalMomentumTimeVariation(pinRobot.model, pinRobot.data, q, v, dv).angular
        if first_iter_for_phase:
            phase.c_init = pcom
            phase.dc_init = vcom
            phase.ddc_init = acom
            phase.L_init = L
            phase.dL_init = dL
            phase.c_t = piecewise(polynomial(pcom.reshape(-1,1), t , t))
            phase.dc_t = piecewise(polynomial(vcom.reshape(-1,1), t, t))
            phase.ddc_t = piecewise(polynomial(acom.reshape(-1,1), t, t))
            phase.L_t = piecewise(polynomial(L.reshape(-1,1), t, t))
            phase.dL_t = piecewise(polynomial(dL.reshape(-1,1), t, t))
        else:
            phase.c_t.append(pcom, t)
            phase.dc_t.append(vcom, t)
            phase.ddc_t.append(acom, t)
            phase.L_t.append(L, t)
            phase.dL_t.append(dL, t)

    def appendZMP(first_iter_for_phase = False):
        """
        Compute the zmp from the current wholebody data and append it to the current phase
        :param first_iter_for_phase: if True, initialize a new Curve for phase.zmp_t
        """
        tau = pin.rnea(pinRobot.model, pinRobot.data, q, v, dv)
        # tau without external forces, only used for the 6 first
        # res.tau_t[:6,k_t] = tau[:6]
        phi0 = pinRobot.data.oMi[1].act(Force(tau[:6]))
        wrench = phi0.vector
        zmp = shiftZMPtoFloorAltitude(cs, t, phi0, cfg.Robot)
        if first_iter_for_phase:
            phase.zmp_t = piecewise(polynomial(zmp.reshape(-1,1), t, t))
            phase.wrench_t = piecewise(polynomial(wrench.reshape(-1,1), t, t))
        else:
            phase.zmp_t.append(zmp, t)
            phase.wrench_t.append(wrench, t)

    def appendEffectorsTraj(first_iter_for_phase = False):
        """
        Append the current position of the effectors not in contact to the current phase trajectories
        :param first_iter_for_phase: if True, initialize a new Curve for the phase effector trajectories
        """
        if first_iter_for_phase and phase_prev:
            for eeName in phase_prev.effectorsWithTrajectory():
                if t > phase_prev.effectorTrajectory(eeName).max():
                    placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
                    phase_prev.effectorTrajectory(eeName).append(placement, t)

        for eeName in phase.effectorsWithTrajectory():
            placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
            if first_iter_for_phase:
                phase.addEffectorTrajectory(eeName, piecewise_SE3(constantSE3curve(placement, t)))
            else:
                phase.effectorTrajectory(eeName).append(placement, t)


    def appendContactForcesTrajs(first_iter_for_phase = False):
        """
        Append the current contact force value to the current phase, for all the effectors in contact
        :param first_iter_for_phase: if True, initialize a new Curve for the phase contact trajectories
        """
        for eeName in phase.effectorsInContact():
            contact = dic_contacts[eeName]
            if invdyn.checkContact(contact.name, sol):
                contact_forces = invdyn.getContactForce(contact.name, sol)
                contact_normal_force = np.array(contact.getNormalForce(contact_forces))
            else:
                logger.warning("invdyn check contact returned false while the reference contact is active !")
                contact_normal_force = np.zeros(1)
                if cfg.Robot.cType == "_3_DOF":
                    contact_forces = np.zeros(3)
                else:
                    contact_forces = np.zeros(12)
            if first_iter_for_phase:
                phase.addContactForceTrajectory(eeName, piecewise(polynomial(contact_forces.reshape(-1,1), t, t)))
                phase.addContactNormalForceTrajectory(eeName, piecewise(polynomial(contact_normal_force.reshape(1,1), t, t)))
            else:
                phase.contactForce(eeName).append(contact_forces, t)
                phase.contactNormalForce(eeName).append(contact_normal_force.reshape(1), t)


    def storeData(first_iter_for_phase = False):
        """
        Append all the required data (selected in the configuration file) to the current ContactPhase
        :param first_iter_for_phase: if True, set the initial values for the current phase
        and correctly initiliaze empty trajectories for this phase
        """
        if cfg.IK_store_joints_derivatives:
            appendJointsDerivatives(first_iter_for_phase)
        appendJointsValues(first_iter_for_phase)
        if cfg.IK_store_joints_torque:
            appendTorques(first_iter_for_phase)
        if cfg.IK_store_centroidal:
            appendCentroidal(first_iter_for_phase)
        if cfg.IK_store_zmp:
            appendZMP(first_iter_for_phase)
        if cfg.IK_store_effector:
            appendEffectorsTraj(first_iter_for_phase)
        if cfg.IK_store_contact_forces:
            appendContactForcesTrajs(first_iter_for_phase)


    def printIntermediate():
        """
        Print the current state: active contacts, tracking errors, computed joint acceleration and velocity
        :return:
        """
        if logger.isEnabledFor(logging.INFO):
            print("Time %.3f" % (t))
            for eeName, contact in dic_contacts.items():
                if invdyn.checkContact(contact.name, sol):
                    f = invdyn.getContactForce(contact.name, sol)
                    print("\tnormal force %s: %.1f" % (contact.name.ljust(20, '.'), contact.getNormalForce(f)))

            print("\ttracking err %s: %.3f" % (comTask.name.ljust(20, '.'), norm(comTask.position_error, 2)))
            for eeName in phase.effectorsWithTrajectory():
                task = dic_effectors_tasks[eeName]
                error = task.position_error
                if cfg.Robot.cType == "_3_DOF":
                    error = error[0:3]
                print("\ttracking err %s: %.3f" % (task.name.ljust(20, '.'), norm(error, 2)))
            print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv)))

    def checkDiverge():
        """
        Check if either the joint velocity or acceleration is over a treshold or is NaN, and raise an error
        """
        if norm(dv) > 1e6 or norm(v) > 1e6:
            logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
            logger.error("/!\ ABORT : controler unstable at t = %f  /!\ ", t)
            logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
            raise ValueError("ABORT : controler unstable at t = " + str(t))
        if math.isnan(norm(dv)) or math.isnan(norm(v)):
            logger.error("!!!!!!    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
            logger.error("/!\ ABORT : nan   at t = %f   /!\ ", t)
            logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
            raise ValueError("ABORT : controler unstable at t = " + str(t))

    def stopHere():
        """
        Set the current data as final values for the current phase, resize the contact sequence if needed and return
        :return: [The current ContactSequence, the RobotWrapper]
        """
        setPreviousFinalValues(phase_prev, phase, cfg)
        if cfg.WB_ABORT_WHEN_INVALID:
            # cut the sequence up to the last phase
            cs.resize(pid)
            return cs, robot
        elif cfg.WB_RETURN_INVALID:
            # cut the sequence up to the current phase
            cs.resize(pid+1)
            return cs, robot

    ### End of nested functions definitions ###
    if not viewer:
        logger.warning("No viewer linked, cannot display end_effector trajectories.")
    logger.warning("Start TSID ... ")


    # copy the given contact sequence to keep it as reference :
    cs = ContactSequence(cs_ref)
    # delete all the 'reference' trajectories from result (to leave room for the real trajectories stored)
    deleteAllTrajectories(cs)

    # Create a robot wrapper
    if robot is None or cfg.IK_store_centroidal or cfg.IK_store_zmp:
        rp = RosPack()
        package_name = cfg.Robot.packageName.split("/")[0]
        package_path = rp.get_path(package_name)
        urdf = package_path + cfg.Robot.packageName.lstrip(
            package_name) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
        logger.info("load robot : %s", urdf)
    if robot is None:
        logger.info("load robot : %s", urdf)
        robot = tsid.RobotWrapper(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
    else:
        logger.info("Use given robot in tsid.")
    logger.info("robot loaded in tsid.")
    if cfg.IK_store_centroidal or cfg.IK_store_zmp:
        logger.info("load pinocchio robot ...")
        # FIXME : tsid robotWrapper don't have all the required methods, only pinocchio have them
        pinRobot = pin.RobotWrapper.BuildFromURDF(urdf, package_path, pin.JointModelFreeFlyer())
        logger.info("pinocchio robot loaded.")

    # get the selected end effector trajectory generation method
    effectorMethod, effectorCanRetry = cfg.get_effector_method()

    # get the selected simulator method
    Simulator = cfg.get_simulator_class()
    simulator = Simulator(cfg.IK_dt, robot.model())

    ### Define initial state of the robot ###
    phase0 = cs.contactPhases[0]
    q = phase0.q_init[:robot.nq].copy()
    if not q.any():
        raise RuntimeError("The contact sequence doesn't contain an initial whole body configuration")
    t = phase0.timeInitial
    if cs_ref.contactPhases[0].dq_t and cs_ref.contactPhases[0].dq_t.min() <= t <= cs_ref.contactPhases[0].dq_t.max():
        v = cs_ref.contactPhases[0].dq_t(t)
    else:
        v = np.zeros(robot.nv)
    logger.debug("V_init used in tsid : %s", v)

    invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
    invdyn.computeProblemData(t, q, v)
    simulator.init(q, v)

    # add initial contacts :
    dic_contacts = {}
    for eeName in cs.contactPhases[0].effectorsInContact():
        # replace the initial contact patch placements if needed to match exactly the current position in the problem:
        updateContactPlacement(cs, 0, eeName,
                               getCurrentEffectorPosition(robot, invdyn.data(), eeName),
                               cfg.Robot.cType == "_6_DOF")
        # create the contacts :
        contact = createContactForEffector(cfg, invdyn, robot, eeName, phase0.contactPatch(eeName), False)
        dic_contacts.update({eeName: contact})

    if cfg.EFF_CHECK_COLLISION:  # initialise object needed to check the motion
        from mlp.utils import check_path
        validator = check_path.PathChecker(fullBody, cfg.CHECK_DT, logger.isEnabledFor(logging.INFO))

    ### Initialize all task used  ###
    logger.info("initialize tasks : ")
    if cfg.w_com > 0. :
        comTask = tsid.TaskComEquality("task-com", robot)
        comTask.setKp(cfg.kp_com * np.ones(3))
        comTask.setKd(2.0 * np.sqrt(cfg.kp_com) * np.ones(3))
        invdyn.addMotionTask(comTask, cfg.w_com, cfg.level_com, 0.0)
    else:
        comTask = None

    if cfg.w_am > 0.:
        amTask = tsid.TaskAMEquality("task-am", robot)
        amTask.setKp(cfg.kp_am * np.array([1., 1., 0.]))
        amTask.setKd(2.0 * np.sqrt(cfg.kp_am * np.array([1., 1., 0.])))
        invdyn.addMotionTask(amTask, cfg.w_am, cfg.level_am, 0.)
    else:
        amTask = None

    if cfg.w_posture > 0.:
        postureTask = tsid.TaskJointPosture("task-joint-posture", robot)
        postureTask.setKp(cfg.kp_posture * cfg.gain_vector)
        postureTask.setKd(2.0 * np.sqrt(cfg.kp_posture * cfg.gain_vector))
        postureTask.setMask(cfg.masks_posture)
        invdyn.addMotionTask(postureTask, cfg.w_posture, cfg.level_posture, 0.0)
        q_ref = cfg.IK_REFERENCE_CONFIG
        samplePosture = tsid.TrajectorySample(q_ref.shape[0] - 7)
        samplePosture.pos(q_ref[7:]) # -7 because we remove the freeflyer part
    else :
        postureTask = None

    if cfg.w_rootOrientation > 0. :
        orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot, 'root_joint')
        mask = np.ones(6)
        mask[0:3] = 0
        mask[5] = cfg.YAW_ROT_GAIN
        orientationRootTask.setMask(mask)
        orientationRootTask.setKp(cfg.kp_rootOrientation * mask)
        orientationRootTask.setKd(2.0 * np.sqrt(cfg.kp_rootOrientation * mask))
        invdyn.addMotionTask(orientationRootTask, cfg.w_rootOrientation, cfg.level_rootOrientation, 0.0)
    else:
        orientationRootTask = None

    # init effector task objects :
    usedEffectors = cs.getAllEffectorsInContact()
    dic_effectors_tasks = createEffectorTasksDic(cfg, usedEffectors, robot)

    # Add bounds tasks if required:
    if cfg.w_torque_bounds > 0.:
        tau_max = cfg.scaling_torque_bounds*robot.model().effortLimit[-robot.na:]
        tau_min = -tau_max
        actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
        actuationBoundsTask.setBounds(tau_min, tau_max)
        invdyn.addActuationTask(actuationBoundsTask, cfg.w_torque_bounds, 0, 0.0)
    if cfg.w_joint_bounds > 0.:
        jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, cfg.IK_dt)
        v_max = cfg.scaling_vel_bounds * robot.model().velocityLimit[-robot.na:]
        v_min = -v_max
        print("v_max : ", v_max)
        jointBoundsTask.setVelocityBounds(v_min, v_max)
        invdyn.addMotionTask(jointBoundsTask, cfg.w_joint_bounds, 0, 0.0)

    solver = tsid.SolverHQuadProg("qp solver")
    solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)

    # time check
    dt = cfg.IK_dt
    logger.info("dt : %f", dt)
    logger.info("tsid initialized, start control loop")
    #raw_input("Enter to start the motion (motion displayed as it's computed, may be slower than real-time)")
    time_start = time.time()

    # For each phases, create the necessary task and references trajectories :
    for pid in range(cs.size()):
        logger.info("## for phase : %d", pid)
        logger.info("t = %f", t)
        # phase_ref contains the reference trajectories and should not be modified exept for new effector trajectories
        # when the first ones was not collision free
        phase_ref = cs_ref.contactPhases[pid]
        # phase de not contains trajectories (exept for the end effectors) and should be modified with the new values computed
        phase = cs.contactPhases[pid]
        if pid > 0:
            phase_prev = cs.contactPhases[pid - 1]
        else:
            phase_prev = None
        if pid < cs.size() - 1:
            phase_next = cs.contactPhases[pid + 1]
        else:
            phase_next = None

        time_interval = [phase_ref.timeInitial, phase_ref.timeFinal]

        logger.info("time_interval %s", time_interval)

        # take CoM and AM trajectory from the phase, with their derivatives
        com_traj = [phase_ref.c_t, phase_ref.dc_t, phase_ref.ddc_t]
        am_traj = [phase_ref.L_t, phase_ref.L_t, phase_ref.dL_t]

        # add root's orientation ref from reference config :
        root_traj = phase_ref.root_t


        # add se3 tasks for end effector when required
        for eeName in phase.effectorsWithTrajectory():
                logger.info("add se3 task for %s", eeName)
                task = dic_effectors_tasks[eeName]
                invdyn.addMotionTask(task, cfg.w_eff, cfg.level_eff, 0.)
                adjustEndEffectorTrajectoryIfNeeded(cfg, phase_ref, robot, invdyn.data(), eeName, effectorMethod)
                logger.info("t interval : %s", time_interval)

        # add newly created contacts :
        new_contacts_names = [] # will store the names of the contact tasks created at this phase
        for eeName in usedEffectors:
            if phase_prev and phase_ref.isEffectorInContact(eeName) and not phase_prev.isEffectorInContact(eeName):
                invdyn.removeTask(dic_effectors_tasks[eeName].name, 0.0)  # remove pin task for this contact
                logger.info("remove se3 effector task : %s", dic_effectors_tasks[eeName].name)
                if logger.isEnabledFor(logging.DEBUG):
                    current_placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName)
                    logger.debug("Current   effector placement : %s", current_placement)
                    logger.debug("Reference effector placement : %s", cs.contactPhases[pid].contactPatch(eeName).placement)
                updateContactPlacement(cs, pid, eeName,
                                       getCurrentEffectorPosition(robot, invdyn.data(), eeName),
                                       cfg.Robot.cType == "_6_DOF")
                contact = createContactForEffector(cfg, invdyn, robot, eeName, phase.contactPatch(eeName))
                new_contacts_names += [contact.name]
                dic_contacts.update({eeName: contact})
                logger.info("Create contact for : %s", eeName)

        # start removing the contact that will be broken in the next phase :
        # (This tell the solver that it should start minimizing the contact force on this contact, and ideally get to 0 at the given time)
        for eeName, contact in dic_contacts.items():
            if phase_next and phase.isEffectorInContact(eeName) and not phase_next.isEffectorInContact(eeName):
                transition_time = phase.duration + dt/2.
                logger.info("\nTime %.3f Start breaking contact %s. transition time : %.3f\n",
                            t, contact.name, transition_time)
                exist = invdyn.removeRigidContact(contact.name, transition_time)
                assert exist, "Try to remove a non existing contact !"

        # Remove all effectors not in contact at this phase,
        # This is required as the block above may not remove the contact exactly at the desired time
        # FIXME: why is it required ? Numerical approximation in the transition_time ?
        for eeName, contact in dic_contacts.items():
            if not phase.isEffectorInContact(eeName):
                exist = invdyn.removeRigidContact(contact.name, 0.)
                if exist:
                    logger.warning("Contact "+eeName+" was not remove after the given transition time.")

        if cfg.WB_STOP_AT_EACH_PHASE:
            input('start simulation')

        # save values at the beginning of the current phase
        q_begin = q.copy()
        v_begin = v.copy()
        phase.q_init = q_begin
        if phase_prev:
            phase_prev.q_final = q_begin
        phaseValid = False
        iter_for_phase = -1
        # iterate until a valid motion for this phase is found (ie. collision free and which respect joint-limits)
        while not phaseValid:
            deletePhaseWBtrajectories(phase) # clean previous invalid trajectories
            t = phase.timeInitial
            k_t = 0
            if iter_for_phase >= 0:
                # reset values to their value at the beginning of the current phase
                q = q_begin.copy()
                v = v_begin.copy()
                simulator.q = q
                simulator.v = v
            iter_for_phase += 1
            logger.info("Start computation for phase %d , t = %f, try number :  %d", pid, t, iter_for_phase)
            # loop to generate states (q,v,a) for the current contact phase :
            while t < phase.timeFinal - (dt / 2.):

                # set traj reference for current time :
                # com
                if comTask:
                    sampleCom = curvesToTSID(com_traj,t)
                    comTask.setReference(sampleCom)

                # am
                if amTask:
                    if cfg.IK_trackAM:
                        sampleAM =  curvesToTSID(am_traj,t)
                    else:
                        sampleAM = tsid.TrajectorySample(3)
                    amTask.setReference(sampleAM)


                # posture
                #print "postural task ref : ",samplePosture.pos()
                if postureTask:
                    postureTask.setReference(samplePosture)

                # root orientation :
                if orientationRootTask:
                    sampleRoot = curveSE3toTSID(root_traj,t)
                    orientationRootTask.setReference(sampleRoot)

                # update weight of regularization tasks for the new contacts:
                if len(new_contacts_names) > 0 :
                    # linearly decrease the weight of the tasks for the newly created contacts
                    u_w_force = (t - phase.timeInitial) / (phase.duration * cfg.w_forceRef_time_ratio)
                    if u_w_force <= 1.:
                        current_w_force = cfg.w_forceRef_init * (1. - u_w_force) + cfg.w_forceRef_end * u_w_force
                        for contact_name in new_contacts_names:
                            success = invdyn.updateRigidContactWeights(contact_name, current_w_force)
                            assert success, "Unable to change the weight of the force regularization task for contact "+contact_name

                logger.debug("### references given : ###")
                logger.debug("com  pos : %s", sampleCom.pos())
                logger.debug("com  vel : %s", sampleCom.vel())
                logger.debug("com  acc : %s", sampleCom.acc())
                if amTask:
                    logger.debug("AM   pos : %s", sampleAM.pos())
                    logger.debug("AM   vel : %s", sampleAM.vel())
                logger.debug("root pos : %s", sampleRoot.pos())
                logger.debug("root vel : %s", sampleRoot.vel())

                # end effector (if they exists)
                for eeName, traj in phase_ref.effectorTrajectories().items():
                    sampleEff = curveSE3toTSID(traj,t,True)
                    dic_effectors_tasks[eeName].setReference(sampleEff)
                    logger.debug("effector %s, pos = %s", eeName, sampleEff.pos())
                    logger.debug("effector %s, vel = %s", eeName, sampleEff.vel())

                logger.debug("previous q = %s", q)
                logger.debug("previous v = %s", v)
                # solve HQP for the current time
                HQPData = invdyn.computeProblemData(t, q, v)
                if t < phase.timeInitial + dt:
                    logger.info("final data for phase %d", pid)
                    if logger.isEnabledFor(logging.INFO):
                        HQPData.print_all()
                sol = solver.solve(HQPData)
                dv = invdyn.getAccelerations(sol)

                storeData(k_t == 0)
                q, v = simulator.simulate(dv)
                t += dt
                k_t += 1
                if t >= phase.timeFinal - (dt / 2.):
                    t = phase.timeFinal # avoid numerical imprecisions

                logger.debug("v = %s", v)
                logger.debug("dv = %s", dv)

                if int(t / dt) % cfg.IK_PRINT_N == 0:
                    printIntermediate()
                try:
                    checkDiverge()
                except ValueError:
                    return stopHere()


            # end while t \in phase_t (loop for the current contact phase)
            if len(phase.effectorsWithTrajectory()) > 0 and cfg.EFF_CHECK_COLLISION:
                phaseValid, t_invalid = validator.check_motion(phase.q_t)
                #if iter_for_phase < 3 :# debug only, force limb-rrt
                #    phaseValid = False
                #    t_invalid = (phase.timeInitial + phase.timeFinal) / 2.
                if not phaseValid:
                    if iter_for_phase == 0:
                        # save the first q_t trajectory computed, for limb-rrt
                        first_q_t = phase.q_t
                    logger.warning("Phase %d not valid at t = %f", pid, t_invalid)
                    logger.info("First invalid q : %s", phase.q_t(t_invalid))
                    if t_invalid <= (phase.timeInitial + cfg.EFF_T_PREDEF) \
                            or t_invalid >= (phase.timeFinal - cfg.EFF_T_PREDEF):
                        logger.error("Motion is invalid during predefined phases, cannot change this.")
                        return stopHere()
                    if effectorCanRetry():
                        logger.warning("Try new end effector trajectory.")
                        try:
                            for eeName, ref_traj in phase_ref.effectorTrajectories().items():
                                placement_init = ref_traj.evaluateAsSE3(phase.timeInitial)
                                placement_end = ref_traj.evaluateAsSE3(phase.timeFinal)
                                traj = effectorMethod(cfg, time_interval, placement_init, placement_end,
                                                                   iter_for_phase + 1, first_q_t, phase_prev,
                                                                   phase_ref, phase_next, fullBody, eeName, viewer)
                                # save the new trajectory in the phase with the references
                                phase_ref.addEffectorTrajectory(eeName,traj)
                                if cfg.DISPLAY_ALL_FEET_TRAJ and logger.isEnabledFor(logging.INFO):
                                    color = fullBody.dict_limb_color_traj[eeName]
                                    color[-1] = 0.6 # set small transparency
                                    display_tools.displaySE3Traj(phase_ref.effectorTrajectory(eeName),
                                                       viewer.client.gui,
                                                       viewer.sceneName,
                                                       eeName + "_traj_" + str(pid) + "_" + str(iter_for_phase),
                                                       color,
                                                       [phase.timeInitial, phase.timeFinal],
                                                       fullBody.dict_offset[eeName])
                        except ValueError as e:
                            logging.error("ERROR in generateEndEffectorTraj :", exc_info=e)
                            return stopHere()
                    else:
                        logging.error("End effector method choosen do not allow retries, abort here.")
                        return stopHere()
            else:  # no effector motions, phase always valid (or bypass the check)
                phaseValid = True
                logging.info("Phase %d valid.", pid)
            if phaseValid:
                setPreviousFinalValues(phase_prev, phase, cfg)
                # display the progress by moving the robot at the last configuration computed
                if viewer and cfg.IK_SHOW_PROGRESS:
                    display_tools.displayWBconfig(viewer,q)
        #end while not phaseValid
    # end for all phases
    # store the data of the last point
    phase_prev = phase
    phase = ContactPhase(phase_prev)
    storeData(True)
    setPreviousFinalValues(phase_prev, phase, cfg)
    time_end = time.time() - time_start
    logger.warning("Whole body motion generated in : %f s.", time_end)
    logger.info("\nFinal COM Position  %s", robot.com(invdyn.data()))
    logger.info("Desired COM Position %s", cs.contactPhases[-1].c_final)
    if queue_qt:
        queue_qt.put([phase.q_t.curve_at_index(phase.q_t.num_curves() - 1), None, True])
    return cs, robot