Exemplo n.º 1
0
 def compute_from_cs(self):
     """
     Split the complete ContactSequence stored in self.cs and send each subsequence in the pipe_cs
     """
     pid_centroidal = 0
     last_iter_centroidal = False
     logger.info("## Compute from cs,  size = %d", self.cs.size())
     last_phase = self.get_last_phase()
     if last_phase:
         tools.setFinalFromInitialValues(last_phase, self.cs.contactPhases[0])
     while pid_centroidal + 5 < self.cs.size():
         logger.debug("## Current pid = %d", pid_centroidal)
         if pid_centroidal + 7 >= self.cs.size():
             logger.debug("## Last centroidal iter")
             # last iter, take all the remaining phases
             num_phase = self.cs.size() - pid_centroidal
             last_iter_centroidal = True
         else:
             num_phase = 5
         logger.debug("## Num phase = %d", num_phase)
         # Extract the phases [pid_centroidal; pid_centroidal +num_phases] from cs_full
         cs_iter = ContactSequence(0)
         for i in range(pid_centroidal, pid_centroidal + num_phase):
             logger.debug("-- Add phase : %d", i)
             cs_iter.append(self.cs.contactPhases[i])
         self.pipe_cs_in.send([cs_iter, last_iter_centroidal])  # This call may be blocking if the pipe is full
         pid_centroidal += 2
     self.pipe_cs_in.close()
Exemplo n.º 2
0
 def test_step_in_place(self):
     cs = ContactSequence()
     cs.loadFromBinary(str(PATH / "step_in_place.cs"))
     self.assertEqual(cs.size(), 9)
     self.assertTrue(cs.haveConsistentContacts())
     self.assertFalse(cs.haveFriction())
     self.assertFalse(cs.haveContactModelDefined())
def create_stairs_cs():
    ENV_NAME = "multicontact/bauzil_stairs"

    fb, v = display_tools.initScene(Robot, ENV_NAME, False)
    cs = ContactSequence(0)

    # Create an initial contact phase :
    q_ref = fb.referenceConfig[::] + [0] * 6
    q_ref[0:2] = [0.07, 1.2]
    addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId])

    step_height = 0.1
    step_width = 0.3
    displacement = SE3.Identity()
    displacement.translation = array([step_width, 0, step_height])
    cs.moveEffectorOf(fb.rfoot, displacement)
    cs.moveEffectorOf(fb.lfoot, displacement)

    q_end = q_ref[::]
    q_end[0] += step_width
    q_end[2] += step_height
    fb.setCurrentConfig(q_end)
    com = fb.getCenterOfMass()
    setFinalState(cs, array(com), q=q_end)
    return cs
def generate_effector_trajectories_for_sequence(cfg, cs, generate_end_effector_traj, fullBody = None):
    """
    Generate an effector trajectory for each effectors which are going to be in contact in the next phase
    :param cfg: an instance of the configuration class
    :param cs: the contact sequence
    :param generate_end_effector_traj: a pointer to the method used to generate an end effector trajectory for one phase
    :param fullBody: an instance of rbprm FullBody
    :return: a new contact sequence, containing the same data as the one given as input
    plus the effector trajectories for each swing phases
    """
    cs_res = ContactSequence(cs)
    effectors = cs_res.getAllEffectorsInContact()
    previous_phase = None
    for pid in range(cs_res.size()-1): # -1 as last phase never have effector trajectories
        phase = cs_res.contactPhases[pid]
        next_phase = cs_res.contactPhases[pid+1]
        if pid > 0 :
            previous_phase = cs_res.contactPhases[pid-1]

        for eeName in effectors:
            if not phase.isEffectorInContact(eeName) and next_phase.isEffectorInContact(eeName):
                # eeName will be in compute in the next phase, a trajectory should be added in the current phase
                placement_end = next_phase.contactPatch(eeName).placement
                time_interval = [phase.timeInitial, phase.timeFinal]
                if previous_phase is not None and previous_phase.isEffectorInContact(eeName):
                    placement_init = previous_phase.contactPatch(eeName).placement
                else:
                    placement_init = effectorPlacementFromPhaseConfig(phase,eeName,fullBody)
                # build the trajectory :
                traj = generate_end_effector_traj(cfg, time_interval,placement_init,placement_end)
                phase.addEffectorTrajectory(eeName,traj)
    return cs_res
Exemplo n.º 5
0
def generate_contact_sequence_load(cfg):
    fb, v = display_tools.initScene(cfg.Robot, cfg.ENV_NAME)
    cs = ContactSequence(0)
    print("Import contact sequence binary file : ", cfg.CS_FILENAME)
    cs.loadFromBinary(cfg.CS_FILENAME)
    display_tools.displayWBconfig(v, cs.contactPhases[0].q_init)
    return cs, fb, v
def generate_effector_trajectories_for_sequence_load(cfg, cs, fullBody=None):
    cs_ref = ContactSequence()
    filename = cfg.REF_FILENAME
    print("Load contact sequence with end effector trajectories from : ",
          filename)
    cs_ref.loadFromBinary(filename)
    return cs_ref
Exemplo n.º 7
0
 def test_walk_20cm_quasistatic(self):
     cs = ContactSequence()
     cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs"))
     self.assertEqual(cs.size(), 23)
     self.assertFalse(cs.haveFriction())
     self.assertFalse(cs.haveContactModelDefined())
     self.assertTrue(cs.haveConsistentContacts())
Exemplo n.º 8
0
def generateCentroidalTrajectory(cs,
                                 cs_initGuess=None,
                                 fullBody=None,
                                 viewer=None):
    cs_res = ContactSequence(0)
    cs_res.loadFromBinary(cfg.COM_FILENAME)
    print("Import contact sequence binary file : ", cfg.COM_FILENAME)
    return cs_res
Exemplo n.º 9
0
 def test_com_motion_above_feet_COM(self):
     cs = ContactSequence()
     cs.loadFromBinary(str(PATH / "com_motion_above_feet_COM.cs"))
     self.assertEqual(cs.size(), 1)
     self.assertTrue(cs.haveConsistentContacts())
     self.assertTrue(cs.haveTimings())
     self.assertTrue(cs.haveCentroidalValues())
     self.assertTrue(cs.haveCentroidalTrajectories())
     checkCS(self, cs)
def generate_wholebody_load(cfg, cs, fullBody=None, viewer=None):
    rp = RosPack()
    urdf = rp.get_path(cfg.Robot.packageName) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
    logger.info("load robot : %s", urdf)
    robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
    logger.info("robot loaded.")
    cs_wb = ContactSequence()
    logger.warning("Load wholebody contact sequence from  file : %s", cfg.WB_FILENAME)
    cs_wb.loadFromBinary(cfg.WB_FILENAME)
    return cs_wb, robot
Exemplo n.º 11
0
 def test_walk_20cm_quasistatic_COM(self):
     cs = ContactSequence()
     cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_COM.cs"))
     self.assertEqual(cs.size(), 23)
     self.assertTrue(cs.haveConsistentContacts())
     self.assertTrue(cs.haveTimings())
     self.assertTrue(cs.haveCentroidalValues())
     self.assertTrue(cs.haveCentroidalTrajectories())
     self.assertFalse(cs.haveFriction())
     self.assertFalse(cs.haveContactModelDefined())
     checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def generate_centroidal_load(cfg,
                             cs,
                             cs_initGuess=None,
                             fullBody=None,
                             viewer=None,
                             first_iter=True):
    cs_res = ContactSequence(0)
    cs_res.loadFromBinary(cfg.COM_FILENAME)
    logger.warning("Import contact sequence binary file : %s",
                   cfg.COM_FILENAME)
    return cs_res
Exemplo n.º 13
0
    def compute_wholebody(self, robot, cs_com, last_q=None, last_v=None, last_iter=False):
        """
        Compute the wholebody motion for the given ContactSequence
        :param robot: a TSID RobotWrapper instance
        :param cs_com: a ContactSequence with centroidal trajectories
        :param last_q: the last wholebody configuration (used as Initial configuration for this iteration)
        :param last_v:  the last joint velocities vector (used as initial joint velocities for this iteration)
        :param last_iter: if True, the complete ContactSequence is used, if False only the first 2 phases are used
        :return: a ContactSequence with wholebody data, the last wholebody configuration, the last joint velocity,
        the last phase, the TSID RobotWrapper used
        """
        if not self.EffectorInputs.checkAndFillRequirements(cs_com, self.cfg, self.fullBody):
            raise RuntimeError(
                "The current contact sequence cannot be given as input to the end effector method selected.")
        # Generate end effector trajectories for the contactSequence
        cs_ref_full = self.generate_effector_trajectories(self.cfg, cs_com, self.fullBody)
        # EffectorOutputs.assertRequirements(cs_ref_full)
        # Split contactSequence if it is not the last iteration
        if last_iter:
            cs_ref = cs_ref_full
            last_phase = cs_com.contactPhases[-1]
        else:
            cs_cut = ContactSequence()
            for i in range(2):
                cs_cut.append(cs_ref_full.contactPhases[i])
            cs_ref = cs_cut
            # last_phase should be a double support phase, it should contains all the contact data:
            last_phase = cs_com.contactPhases[2]
            tools.setFinalFromInitialValues(last_phase, last_phase)
        if last_q is not None:
            cs_ref.contactPhases[0].q_init = last_q
        if last_v is not None:
            t_init = cs_ref.contactPhases[0].timeInitial
            cs_ref.contactPhases[0].dq_t = polynomial(last_v.reshape(-1, 1), t_init, t_init)

        ### Generate the wholebody trajectory:
        update_root_traj_timings(cs_ref)
        if not self.WholebodyInputs.checkAndFillRequirements(cs_ref, self.cfg, self.fullBody):
            raise RuntimeError(
                "The current contact sequence cannot be given as input to the wholeBody method selected.")
        cs_wb, robot = self.generate_wholebody(self.cfg, cs_ref, robot=robot)
        logger.info("-- compute whole body END")
        # WholebodyOutputs.assertRequirements(cs_wb)
        # Retrieve the last phase, q, and v from this outputs:
        last_phase_wb = cs_wb.contactPhases[-1]
        last_q = last_phase_wb.q_t(last_phase_wb.timeFinal)
        last_v = last_phase_wb.dq_t(last_phase_wb.timeFinal)
        tools.deletePhaseCentroidalTrajectories(last_phase)  # Remove unnecessary data to reduce serialized size
        last_phase.q_final = last_q
        last_phase.dq_t = polynomial(last_v.reshape(-1, 1), last_phase.timeFinal, last_phase.timeFinal)
        #last_phase.c_final = last_phase_wb.c_final
        #last_phase.dc_final = last_phase_wb.dc_final
        #last_phase.L_final = last_phase_wb.L_final
        return cs_wb, last_q, last_v, last_phase, robot
def contactSequenceFromRBPRMConfigs(fb, beginId, endId):
    """
    Build a multicontact_api ContactSequence from rbprm states list
    :param fb: an instance of rbprm.FullBody
    :param beginId: the first state Id
    :param endId: the last state Id
    :return: a multicontact_api ContactSequence
    """
    logger.warning("generate contact sequence from planning : ")
    n_states = endId - beginId + 1
    # There could be either contact break, creation or repositionning between each adjacent states.
    # But there should be only contacts break or creation between each adjacent contactPhases
    cs = ContactSequence(0)
    prev_phase = None
    phaseId = 0

    # create initial ContactPhase
    cs.append(createPhaseFromRBPRMState(fb, beginId))
    for stateId in range(beginId + 1,
                         endId + 1):  # from the second state to the last one
        logger.info("current state id = %d", stateId)
        previous_phase = cs.contactPhases[-1]
        eeName, variationType = getContactVariationBetweenStates(
            fb, stateId - 1, stateId)

        if eeName is not None:
            if variationType == VariationType.REPOSITIONNED:
                # in case of repositionning, the centroidal motion will happend in the next intermediate phase,
                # and thus the previous_phase will not move :
                copyPhaseInitToFinal(previous_phase)
            else:
                # set the final values of the previous phase to be the current one :
                setPhaseFinalValues(fb, stateId, previous_phase)

            if variationType == VariationType.BROKEN:
                # remove the given contact :
                cs.breakContact(eeName)
            else:
                # get placement of the new contact from the planning :
                contact_placement = contactPlacementFromRBPRMState(
                    fb, stateId, eeName)
            if variationType == VariationType.CREATED:
                # create a new contact :
                cs.createContact(eeName, ContactPatch(contact_placement))
            elif variationType == VariationType.REPOSITIONNED:
                # move existing contact to new placement :
                cs.moveEffectorToPlacement(eeName, contact_placement)
                # set the initial values for the current phase from planning :
                setPhaseInitialValues(fb, stateId, cs.contactPhases[-1])
                # set the same values as the final ones for the intermediate state created :
                setPhaseFinalValues(fb, stateId, cs.contactPhases[-2])
        # else : no contact changes, ignore this state
    setPhaseFinalValues(fb, endId, cs.contactPhases[-1])
    return cs
Exemplo n.º 15
0
 def test_com_motion_above_feet_WB(self):
     cs = ContactSequence()
     cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs"))
     self.assertEqual(cs.size(), 1)
     self.assertTrue(cs.haveConsistentContacts())
     self.assertTrue(cs.haveTimings())
     self.assertTrue(cs.haveCentroidalValues())
     self.assertTrue(cs.haveCentroidalTrajectories())
     self.assertTrue(cs.haveJointsTrajectories())
     self.assertTrue(cs.haveJointsDerivativesTrajectories())
     self.assertTrue(cs.haveContactForcesTrajectories())
     self.assertTrue(cs.haveZMPtrajectories())
     checkCS(self, cs, wholeBody=True)
Exemplo n.º 16
0
def generateCentroidalTrajectory(cs,
                                 cs_initGuess=None,
                                 fullBody=None,
                                 viewer=None,
                                 first_iter=True):
    """
    Generate straight line trajectories from the center of the support polygon of one phase
    to the center in the next phase.
    Compute trajectories as Quintic splines, starting and ending with null acceleration and velocity at each phase change
    Add a null angular momentum.
    :param cs:
    :param cs_initGuess:
    :param fullBody:
    :param viewer:
    :return:
    """
    if cs_initGuess:
        print("WARNING : in centroidal.geometric, initial guess is ignored.")
    if not first_iter:
        print(
            "WARNING : in centroidal.geometric, it is useless to iterate several times."
        )

    if cs.haveCOMvalues():
        # do not overwrite com values in input sequence
        cs_result = ContactSequence(cs)
        computePhasesCOMValues(cs_result,
                               cfg.Robot.DEFAULT_COM_HEIGHT,
                               overwrite=True)
    else:
        # add them in output sequence if not present
        computePhasesCOMValues(cs, cfg.Robot.DEFAULT_COM_HEIGHT)
        cs_result = ContactSequence(cs)

    for pid, phase in enumerate(cs_result.contactPhases):
        genCOMTrajFromPhaseStates(phase)
        genAMTrajFromPhaseStates(phase)

    return cs_result
Exemplo n.º 17
0
 def test_step_in_place_REF(self):
     cs = ContactSequence()
     cs.loadFromBinary(str(PATH / "step_in_place_REF.cs"))
     self.assertEqual(cs.size(), 9)
     self.assertTrue(cs.haveConsistentContacts())
     self.assertTrue(cs.haveTimings())
     self.assertTrue(cs.haveCentroidalValues())
     self.assertTrue(cs.haveCentroidalTrajectories())
     self.assertTrue(cs.haveEffectorsTrajectories())
     self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
     self.assertTrue(cs.haveFriction())
     self.assertTrue(cs.haveContactModelDefined())
     checkCS(self, cs, root=True, effector=True, wholeBody=False)
def generate_centroidal_quasistatic(cfg,
                                    cs,
                                    cs_initGuess=None,
                                    fullBody=None,
                                    viewer=None,
                                    first_iter=True):
    if cs_initGuess:
        logger.warning("Initial guess is ignored.")
    if not fullBody:
        raise ValueError("quasiStatic called without fullBody object.")
    if not first_iter:
        logger.warning("It is useless to iterate several times.")
    beginId, endId = createFullbodyStatesFromCS(cs, fullBody)
    logger.info("beginid = %d", beginId)
    logger.info("endId   = %d", endId)
    cs_result = ContactSequence(cs)

    # for each phase in the cs, create a corresponding FullBody State and call 2-PAC,
    # then fill the cs struct with a quintic spline connecting the two points found
    id_phase = 0
    for id_state in range(beginId, endId + 1):
        logger.debug("id_state = %d", id_state)
        logger.debug("id_phase = %d", id_phase)
        phase_fixed = cs_result.contactPhases[
            id_phase]  # phase where the CoM move and the contacts are fixed
        # set initial state to be the final one of the previous phase :
        if id_phase > 1:
            setInitialFromFinalValues(phase_swing, phase_fixed)
        # compute 'optimal' position of the COM to go before switching phase:
        if id_state == endId:
            c = phase_fixed.c_final
        else:
            c = getTargetCOMPosition(fullBody, id_state, cfg.COM_SHIFT_Z)
            # set 'c' the final position of current phase :
            phase_fixed.c_final = c
        phase_fixed.dc_final = np.zeros(3)
        phase_fixed.ddc_final = np.zeros(3)
        connectPhaseTrajToFinalState(phase_fixed)

        id_phase += 1
        if id_state < endId:
            phase_swing = cs_result.contactPhases[
                id_phase]  # phase where the CoM is fixed and an effector move
            # in swing phase, com do not move :
            setInitialFromFinalValues(phase_fixed, phase_swing)
            copyPhaseInitToFinal(phase_swing)
            connectPhaseTrajToFinalState(phase_swing)
            id_phase += 1

    return cs_result
Exemplo n.º 19
0
 def test_walk_20cm_quasistatic_WB(self):
     cs = ContactSequence()
     cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs"))
     self.assertEqual(cs.size(), 23)
     self.assertTrue(cs.haveConsistentContacts())
     self.assertTrue(cs.haveTimings())
     self.assertTrue(cs.haveCentroidalValues())
     self.assertTrue(cs.haveCentroidalTrajectories())
     self.assertTrue(cs.haveEffectorsTrajectories(1e-1))
     self.assertTrue(cs.haveJointsTrajectories())
     self.assertTrue(cs.haveJointsDerivativesTrajectories())
     self.assertTrue(cs.haveContactForcesTrajectories())
     self.assertTrue(cs.haveZMPtrajectories())
     self.assertTrue(cs.haveFriction())
     self.assertTrue(cs.haveContactModelDefined())
     checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
Exemplo n.º 20
0
    def store_mcapi_traj(self, traj_name):
        if not mcapi_import:
            print(
                'multicontact_api package import has failed, check your install'
            )
            return

        self.set_data_lst_as_arrays()

        # trajectory with only one ContactPhase (simpler to read/write)
        # when feet not in contact, the force is exactly zero, that's the only diff
        cs = ContactSequence()
        cp = ContactPhase()

        # assign trajectories :
        t_arr = self.data_log['t']

        cp.timeInitial = t_arr[0]
        cp.timeFinal = t_arr[-1]
        cp.duration = t_arr[-1] - t_arr[0]

        # col number of trajectories should be the time traj size hence the transpose
        cp.q_t = piecewise.FromPointsList(self.data_log['q'].T, t_arr)
        cp.dq_t = piecewise.FromPointsList(self.data_log['v'].T, t_arr)
        cp.ddq_t = piecewise.FromPointsList(self.data_log['dv'].T, t_arr)
        cp.tau_t = piecewise.FromPointsList(self.data_log['tau'].T, t_arr)
        cp.c_t = piecewise.FromPointsList(self.data_log['c'].T, t_arr)
        cp.dc_t = piecewise.FromPointsList(self.data_log['dc'].T, t_arr)
        cp.L_t = piecewise.FromPointsList(self.data_log['Lc'].T, t_arr)

        # contact force trajectories
        for i_foot, frame_name in enumerate(self.contact_names):
            cp.addContact(frame_name, ContactPatch(
                pin.SE3(), 0.5))  # dummy placement and friction coeff
            cp.addContactForceTrajectory(
                frame_name,
                piecewise.FromPointsList(self.data_log['f{}'.format(i_foot)].T,
                                         t_arr))

        cs.append(cp)  # only one contact phase

        savepath = os.path.join(self.directory, traj_name + '.cs')
        cs.saveAsBinary(savepath)
        print('Saved ' + savepath)
Exemplo n.º 21
0
 def compute_stopping_cs(self, move_to_support_polygon=True):
     """
     Compute a Contact Sequence with centroidal trajectories to bring the current last_phase
     to a stop without contact changes
     :param move_to_support_polygon: if True, add a trajectory to put the CoM above the center of the support polygon
     :return:
     """
     phase_stop = ContactPhase(self.get_last_phase())
     tools.setInitialFromFinalValues(phase_stop, phase_stop)
     phase_stop.timeInitial = phase_stop.timeFinal
     phase_stop.duration = DURATION_0_STEP  # FIXME !!
     # try 0-step:
     success, phase = zeroStepCapturability(phase_stop, self.cfg)
     if success:
         cs_ref = ContactSequence(0)
         cs_ref.append(phase)
         # TEST : add another phase to go back in the center of the support polygon
         if move_to_support_polygon:
             phase_projected = ContactPhase()
             phase_projected.timeInitial = phase.timeFinal
             phase_projected.duration = DURATION_0_STEP
             tools.copyContactPlacement(phase, phase_projected)
             tools.setInitialFromFinalValues(phase, phase_projected)
             phase_projected.c_final = tools.computeCenterOfSupportPolygonFromPhase(
                 phase_stop, self.fullBody.DEFAULT_COM_HEIGHT)
             #FIXME 'default height'
             tools.connectPhaseTrajToFinalState(phase_projected)
             cs_ref.append(phase_projected)
     else:
         # TODO try 1 step :
         raise RuntimeError("One step capturability not implemented yet !")
     tools.computeRootTrajFromContacts(self.fullBody, cs_ref)
     self.last_phase = cs_ref.contactPhases[-1].copy()
     # define the final root position, translation from the CoM position and rotation from the feet rotation
     q_final = np.zeros(7)
     q_final[:3] = self.last_phase.c_final[::]
     placement_rot_root, _ = tools.rootOrientationFromFeetPlacement(self.cfg.Robot, None, self.last_phase, None)
     quat_root = Quaternion(placement_rot_root.rotation)
     q_final[3:7] = [quat_root.x, quat_root.y, quat_root.z, quat_root.w]
     self.last_phase.q_final = q_final
     self.last_phase_flag.value = False
     self.last_phase_pickled = Array(c_ubyte, MAX_PICKLE_SIZE)  # reset currently stored whole body last phase
     return cs_ref
def copyEffectorTrajectories(cs_eff, cs):
    """
    Copy all the effector trajectories contained in cs_eff in a copy of cs
    :param cs_eff: the contact sequence containing the effector trajectories
    :param cs: the contact sequence to copy
    :return: A new ContactSequence, which is a copy of cs with the addition of the trajectories from cs_eff
    Return None if the phases do not have the same duration in cs_eff and cs
    """
    if cs_eff.size() != cs.size():
        return None
    cs_res = ContactSequence(cs)
    for pid, phase_eff in enumerate(cs_eff.contactPhases):
        if len(phase_eff.effectorsWithTrajectory()) > 0:
            phase = cs_res.contactPhases[pid]
            if phase.timeInitial != phase_eff.timeInitial or phase.timeFinal != phase_eff.timeFinal:
                print("Unable to copy effector trajectories, the phase duration have changed")
                return None
            for eeName, traj in phase_eff.effectorTrajectories().items():
                phase.addEffectorTrajectory(eeName, traj)
    return cs_res
Exemplo n.º 23
0
 def compute_centroidal(self, cs, previous_phase, last_iter=False):
     """
     Solve the centroidal problem for the given ContactSequence
     :param cs: the ContactSequence used
     :param previous_phase: If provided, copy the final data of this phase as initial data for
      the given ContactSequence
     :param last_iter: If True, return a ContactSequence corresponding to the complete contactSequence given as input
     If False, the result is splitted and only the first 3 phases are returned
     :return: The ContactSequence with centroidal trajectories, and the last phase
     """
     # update the initial state with the data from the previous intermediate state:
     if previous_phase:
         tools.setInitialFromFinalValues(previous_phase, cs.contactPhases[0])
         self.cfg.COM_SHIFT_Z = 0.
         self.cfg.TIME_SHIFT_COM = 0.
     #else:
     #    self.cfg.COM_SHIFT_Z = self.previous_com_shift_z
     #    self.cfg.TIME_SHIFT_COM = self.previous_time_shift_com
     if last_iter:
         # Set settings specific to the last iteration that need to connect exactly to the final goal position
         self.cfg.DURATION_CONNECT_GOAL = self.previous_connect_goal
         self.cfg.TIMEOPT_CONFIG_FILE = self.TIMEOPT_CONFIG_FILE
     else:
         # Set settings for the middle of the sequence: do not need to connect exactly to the goal
         self.cfg.DURATION_CONNECT_GOAL = 0.
         self.cfg.TIMEOPT_CONFIG_FILE = self.TIMEOPT_CONFIG_FILE.rstrip(".yaml") + "_lowgoal.yaml"
     if not self.CentroidalInputs.checkAndFillRequirements(cs, self.cfg, None):
         raise RuntimeError(
             "The current contact sequence cannot be given as input to the centroidal method selected.")
     cs_full = self.generate_centroidal(self.cfg, cs, None, None)
     # CentroidalOutputs.assertRequirements(cs_full)
     if last_iter:
         return cs_full, None
     else:
         cs_cut = ContactSequence(0)
         for i in range(3):
             cs_cut.append(cs_full.contactPhases[i])
         return cs_cut, cs_cut.contactPhases[1]
Exemplo n.º 24
0
    def generateEffectorTrajectoriesForSequence(cs, fullBody=None):
        """
        Generate an effector trajectory for each effectors which are going to be in contact in the next phase
        :param cs:
        :return:
        """
        cs_res = ContactSequence(cs)
        effectors = cs_res.getAllEffectorsInContact()
        previous_phase = None
        for pid in range(
                cs_res.size() -
                1):  # -1 as last phase never have effector trajectories
            phase = cs_res.contactPhases[pid]
            next_phase = cs_res.contactPhases[pid + 1]
            if pid > 0:
                previous_phase = cs_res.contactPhases[pid - 1]

            for eeName in effectors:
                if not phase.isEffectorInContact(
                        eeName) and next_phase.isEffectorInContact(eeName):
                    # eeName will be in compute in the next phase, a trajectory should be added in the current phase
                    placement_end = next_phase.contactPatch(eeName).placement
                    time_interval = [phase.timeInitial, phase.timeFinal]
                    if previous_phase is not None and previous_phase.isEffectorInContact(
                            eeName):
                        placement_init = previous_phase.contactPatch(
                            eeName).placement
                    else:
                        placement_init = effectorPlacementFromPhaseConfig(
                            phase, eeName, fullBody)
                    # build the trajectory :
                    traj = generateEndEffectorTraj(time_interval,
                                                   placement_init,
                                                   placement_end)
                    phase.addEffectorTrajectory(eeName, traj)

        return cs_res
from pinocchio import SE3
from numpy import array

from talos_rbprm.talos import Robot  # change robot here
multicontact_api.switchToNumpyArray()

ENV_NAME = "multicontact/ground"

# Build the robot object and the viewer
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
gui = v.client.gui
sceneName = v.sceneName
# display the origin and x,y,z axis in the viewer :
v.addLandmark(sceneName, 1)
#initialize an empty contact sequence
cs = ContactSequence(0)

# start from the reference configuration of the robot :
q_ref = fb.referenceConfig[::] + [0] * 6
#q_ref[0:2] = [ , ] # change the x,y position of the base as needed
# q_ref[2] +=  # the z position of the base already correspond to a floor at z=0, change it accordingly to the environment
# q_ref[3:7] =   # quaternion (x,y,z) of the base

# display the configuration
v(q_ref)
com = array(fb.getCenterOfMass())

# create a first contact phase corresponding to this configuration
limbsInContact = [fb.rLegId, fb.lLegId
                  ]  # define the limbs in contact for this first phase
addPhaseFromConfig(fb, cs, q_ref, limbsInContact)
Exemplo n.º 26
0
def CSfromMomentumopt(planner_setting,
                      cs,
                      init_state,
                      dyn_states,
                      t_init=0,
                      connect_goal=True):
    """
    Create a ContactSequence and fill it with the results from momentumopt
    :param planner_setting:
    :param cs: a multicontact_api ContactSequence from which the contacts are copied
    :param init_state: initial state used by momentumopt
    :param dyn_states: the results of momentumopt
    :return: a multicontact_api ContactSequence with centroidal trajectories
    """
    logger.info("Start to convert result to mc-api ...")
    cs_com = ContactSequence(cs)
    MASS = planner_setting.get(mopt.PlannerDoubleParam_RobotMass)
    p_id = 0  # phase id in cs
    # dyn_states[0] is at t == dt , not t == 0 ! use init_state for t == 0
    p0 = cs_com.contactPhases[0]
    c_init = init_state.com
    p0.timeInitial = t_init

    # init arrays to store the discrete points. one value per columns
    c_t = c_init.reshape(3, 1)
    dc_t = p0.dc_init.reshape(3, 1)
    ddc_t = p0.ddc_init.reshape(3, 1)
    L_t = p0.L_init.reshape(3, 1)
    dL_t = p0.dL_init.reshape(3, 1)
    times = array(t_init)
    current_t = t_init
    # loop for all dynamicsStates
    for k, ds in enumerate(dyn_states):
        #extract states values from ds :
        if k == 0:
            ddc = (ds.lmom / MASS) / ds.dt  # acceleration
            dL = ds.amom / ds.dt  # angular momentum variation
        else:
            ddc = ((ds.lmom / MASS) - (dyn_states[k - 1].lmom / MASS)) / ds.dt
            dL = (ds.amom - dyn_states[k - 1].amom) / ds.dt
        c = ds.com  # position
        dc = ds.lmom / MASS  # velocity
        L = ds.amom  # angular momentum
        # stack the values in the arrays:
        c_t = append(c_t, c.reshape(3, 1), axis=1)
        dc_t = append(dc_t, dc.reshape(3, 1), axis=1)
        ddc_t = append(ddc_t, ddc.reshape(3, 1), axis=1)
        L_t = append(L_t, L.reshape(3, 1), axis=1)
        dL_t = append(dL_t, dL.reshape(3, 1), axis=1)
        current_t += ds.dt
        times = append(times, current_t)

        if k > 0 and isNewPhase(dyn_states[k - 1],
                                ds) and p_id < cs_com.size() - 1:
            #last k of current phase, first k of next one (same trajectories and time)
            # set the trajectories for the current phase from the arrays :
            phase = cs_com.contactPhases[p_id]
            setCOMtrajectoryFromPoints(phase,
                                       c_t,
                                       dc_t,
                                       ddc_t,
                                       times,
                                       overwriteInit=(p_id > 0))
            setAMtrajectoryFromPoints(phase,
                                      L_t,
                                      dL_t,
                                      times,
                                      overwriteInit=(p_id > 0))
            # set final time :
            phase.timeFinal = times[-1]
            # Start new phase :
            p_id += 1
            phase = cs_com.contactPhases[p_id]
            # set initial time :
            phase.timeInitial = times[-1]
            # reset arrays of values to only the last point :
            c_t = c_t[:, -1].reshape(3, 1)
            dc_t = dc_t[:, -1].reshape(3, 1)
            ddc_t = ddc_t[:, -1].reshape(3, 1)
            L_t = L_t[:, -1].reshape(3, 1)
            dL_t = dL_t[:, -1].reshape(3, 1)
            times = times[-1]

    # set final phase :
    phase = cs_com.contactPhases[-1]
    setCOMtrajectoryFromPoints(phase,
                               c_t,
                               dc_t,
                               ddc_t,
                               times,
                               overwriteFinal=not connect_goal)
    setAMtrajectoryFromPoints(phase,
                              L_t,
                              dL_t,
                              times,
                              overwriteFinal=not connect_goal)
    # set final time :
    phase.timeFinal = times[-1]
    logger.info("Converting results to mc-api done.")
    return cs_com
Exemplo n.º 27
0
def generate_centroidal_croc(cfg,
                             cs,
                             cs_initGuess=None,
                             fb=None,
                             viewer=None,
                             first_iter=True):
    if cs_initGuess:
        logger.warning("Initial guess is ignored.")
    if not fb:
        raise ValueError("CROC called without fullBody object.")
    if not first_iter:
        logger.warning("It is useless to iterate several times.")
    beginId, endId = createFullbodyStatesFromCS(cs, fb)
    logger.info("beginid = %d", beginId)
    logger.info("endId   = %d", endId)
    cs_result = ContactSequence(cs)
    resetCOMtrajectories(cs_result)  # erase all pre existing CoM trajectories

    if (2 * (endId - beginId) + 1) != cs.size():
        raise NotImplemented(
            "Current implementation of CROC require to have 2 contact phases per States in fullBody"
        )
    # for each phase in the cs, create a corresponding FullBody State and call CROC,
    # then fill the cs struct
    id_phase = 0
    current_t = cs.contactPhases[0].timeInitial
    for id_state in range(beginId, endId):
        logger.info("id_state = %d", id_state)
        logger.info("id_phase = %d", id_phase)
        # First, compute the bezier curves between the states :
        pid = fb.isDynamicallyReachableFromState(id_state,
                                                 id_state + 1,
                                                 True,
                                                 numPointsPerPhases=0)
        if len(pid) != 4:
            logger.error("# Cannot compute CROC between states %d, %d",
                         id_state, id_state + 1)
            return cs
        c1 = fb.getPathAsBezier(int(pid[1]))  # curve before contact break
        c2 = fb.getPathAsBezier(int(pid[2]))  # curve for swing phase
        c3 = fb.getPathAsBezier(int(pid[3]))  # curve after contact creation
        # NOTE : this bezier are defined in [0,tmax] ! We need to switch the time intervals

        # fill the phases with the curves from CROC :
        # c1 :
        phase = cs_result.contactPhases[id_phase]
        setCOMfromCurve(phase, c1)
        current_t = phase.timeFinal
        # c2
        id_phase += 1
        phase = cs_result.contactPhases[id_phase]
        phase.timeInitial = current_t
        setCOMfromCurve(phase, c2)
        current_t = phase.timeFinal
        # c3
        id_phase += 1
        phase = cs_result.contactPhases[id_phase]
        phase.timeInitial = current_t
        setCOMfromCurve(phase, c3)
        # pid should not increase here, as the next c1 trajectory will be added to the same phase as the current c3 one.

    return cs_result
Exemplo n.º 28
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
def build_cs_from_sl1m_mip(fb, q_ref, root_end, pb, allfeetpos, use_orientation, use_interpolated_orientation,
                       q_init = None, first_phase = None):
    """
    Build a multicontact_api.ContactSequence from the SL1M outputs when using the MIP formulation
    :param fb: an instance of rbprm.Fullbody
    :param q_ref: the reference wholebody configuration of the robot
    :param root_end: the final base position
    :param pb: the SL1M problem dictionary, containing all the contact surfaces and data
    :param allfeetpos: the list of all foot position for each phase, computed by SL1M
    :param use_orientation: if True, change the contact yaw rotation to match the orientation of the base in the guide
    :param use_interpolated_orientation: if True, the feet yaw orientation will 'anticipate' the base orientation
    of the next phase
    :param q_init: the initial wholebody configuration (either this or first_phase should be provided)
    :param first_phase: the first multicontact_api.ContactPhase object (either this or q_init should be provided)
    :return: the multicontact_api.ContactSequence, with all ContactPhase created at the correct placement
    """
    # init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
    # FIXME : allow to customize that first phase
    num_steps = len(pb["phaseData"]) - 1 # number of contact repositionning
    limbs_names = fb.limbs_names
    num_effectors = len(limbs_names)
    logger.info(" limbs names : %s", limbs_names)
    cs = ContactSequence(0)
    if first_phase:
        cs.append(first_phase)
    elif q_init:
        # if only the configuration is provided, assume all effectors are in contact
        addPhaseFromConfig(fb, cs, q_init, limbs_names)
    else:
        raise ValueError("build_cs_from_sl1m should have either q_init or first_phase argument defined")
    logger.info("Initial phase added, contacts : %s ", cs.contactPhases[0].effectorsInContact())
    # loop for all effector placements, and create the required contact phases
    previous_eff_placements = allfeetpos[0]
    if len(previous_eff_placements) != num_effectors:
        raise NotImplementedError("A phase in the output of SL1M do not have all the effectors in contact.")
    for pid, eff_placements in enumerate(allfeetpos[1:]):
        logger.info("Loop allfeetpos, id = %d", pid)
        if len(eff_placements) != num_effectors:
            raise NotImplementedError("A phase in the output of SL1M do not have all the effectors in contact.")
        switch = False # True if a repostionning have been detected
        for k, pos in enumerate(eff_placements):
            if norm(pos - previous_eff_placements[k]) > 1e-3:
                if switch:
                    raise NotImplementedError("Several contact changes between two adjacent phases in SL1M output")
                switch = True
                ee_name = fb.dict_limb_joint[limbs_names[k]]
                #pos[2] += EPS_Z # FIXME: apply epsz along the normal
                pos = fb.dict_offset[ee_name].actInv(pos)
                logger.info("Move effector %s ", ee_name)
                logger.info("To position %s ", pos)
                placement = SE3.Identity()
                placement.translation = pos
                # compute orientation of the contact from the surface normal:
                phase_data = pb["phaseData"][pid+1] # +1 because the for loop start at id = 1
                n = normal_from_ineq(phase_data["S"][phase_data["id_surface"]])
                placement.rotation = rotationFromNormal(n)
                logger.debug("new contact placement : %s", placement)
                # TODO add yaw rotation from guide here !
                cs.moveEffectorToPlacement(ee_name, placement)

        #if not switch:
        #   raise RuntimeError("No contact changes between two adjacent phases in SL1M output")
        # assign com position to the last two phases :
        # swinging phase, same init and final position
        """
        cs.contactPhases[-2].c_init = coms[pid * 2]
        cs.contactPhases[-2].c_final = coms[pid * 2 + 1]
        # phase with all the contacts:
        cs.contactPhases[-1].c_init = coms[pid * 2 + 1]
        if pid * 2 + 2 < len(coms):
            cs.contactPhases[-1].c_final = coms[pid * 2 + 2]
        else:
            cs.contactPhases[-1].c_final = cs.contactPhases[-1].c_init
        """
        previous_eff_placements = eff_placements

    # final phase :
    # fixme : assume root is in the middle of the last 2 feet pos ...
    """
    q_end = q_ref.tolist() + [0] * 6
    # p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
    # for i in range(3):
    #    q_end[i] += p_end[i]
    q_end[0:7] = root_end
    feet_height_end = allfeetpos[-1][0][2]
    logger.info("feet height final = %s", feet_height_end)
    q_end[2] = feet_height_end + q_ref[2]
    #q_end[2] += EPS_Z
    fb.setCurrentConfig(q_end)
    com = fb.getCenterOfMass()
    setFinalState(cs, com, q=q_end)
    """
    p_final = cs.contactPhases[-1]
    p_final.c_final = computeCenterOfSupportPolygonFromPhase(p_final, fb.DEFAULT_COM_HEIGHT)
    p_final.c_init = p_final.c_final
    return cs
def build_cs_from_sl1m_l1(fb, q_ref, root_end, pb, RF, allfeetpos, use_orientation, use_interpolated_orientation,
                       q_init = None, first_phase = None):
    """
    Build a multicontact_api.ContactSequence from the SL1M outputs, when not using the MIP formulation
    :param fb: an instance of rbprm.Fullbody
    :param q_ref: the reference wholebody configuration of the robot
    :param root_end: the final base position
    :param pb: the SL1M problem dictionary, containing all the contact surfaces and data
    :param RF: the Id of the right feet in the SL1M formulation
    :param allfeetpos: the list of all foot position for each phase, computed by SL1M
    :param use_orientation: if True, change the contact yaw rotation to match the orientation of the base in the guide
    :param use_interpolated_orientation: if True, the feet yaw orientation will 'anticipate' the base orientation
    of the next phase
    :param q_init: the initial wholebody configuration (either this or first_phase should be provided)
    :param first_phase: the first multicontact_api.ContactPhase object (either this or q_init should be provided)
    :return: the multicontact_api.ContactSequence, with all ContactPhase created at the correct placement
    """
    # init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
    # FIXME : allow to customize that first phase
    cs = ContactSequence(0)
    if q_init:
        addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId])
    elif first_phase:
        cs.append(first_phase)
    else:
        raise ValueError("build_cs_from_sl1m should have either q_init or first_phase argument defined")

    # loop over all phases of pb and add them to the cs :
    for pId in range(2, len(pb["phaseData"])):  # start at 2 because the first two ones are already done in the q_init
        prev_contactPhase = cs.contactPhases[-1]
        #n = normal(pb["phaseData"][pId])
        phase = pb["phaseData"][pId]
        moving = phase["moving"]
        movingID = fb.lfoot
        if moving == RF:
            movingID = fb.rfoot
        pos = allfeetpos[pId]  # array, desired position for the feet movingID
        pos[2] += EPS_Z  # FIXME it shouldn't be required !!
        # compute desired foot rotation :
        if use_orientation:
            rot = compute_orientation_for_feet_placement(fb, pb, pId, moving, RF, prev_contactPhase, use_interpolated_orientation)
        else:
            rot = Quaternion.Identity()
        placement = SE3()
        placement.translation = np.array(pos).T
        placement.rotation = rot.matrix()
        cs.moveEffectorToPlacement(movingID, placement)

    # final phase :
    # fixme : assume root is in the middle of the last 2 feet pos ...
    q_end = q_ref.tolist() + [0] * 6
    #p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
    #for i in range(3):
    #    q_end[i] += p_end[i]
    q_end[0:7] = root_end
    feet_height_end = allfeetpos[-1][2]
    logger.info("feet height final = %s", feet_height_end)
    q_end[2] = feet_height_end + q_ref[2]
    q_end[2] += EPS_Z
    fb.setCurrentConfig(q_end)
    com = fb.getCenterOfMass()
    setFinalState(cs, com, q=q_end)

    return cs