CscKnowledgeItemMultiPodWalk.h

Go to the documentation of this file.

//
//  CscKnowledgeItemMultiPodWalk.h
//  ConscienceRobotManager
//
//  Created by Max Lhermitte on 16/11/2022.
//  Copyright © 2022 IliesZaoui. All rights reserved.
//
 
#ifndef CscKnowledgeItemMultiPodWalk_h
#define CscKnowledgeItemMultiPodWalk_h
 
#include "Knowledge/CscKnowledgeItemTarget.h"
#include "Knowledge/CscKnowledgeItemTargetMemberToState.h"
#include "Knowledge/CscKnowledgeItemTargetArmToPosition.h"
#include "ArtificialIntelligence/CscSupervisedLearningRegression.h"
 
enum CscSide {
    Left,
    Right
};
 
enum WalkStep {
    BendKnees = 0,
    StandLeftLeg,
    RaiseRightFoot,
    SetRightFoot,
    TransfertWeightRightLeg,
    ThrowBackLegForward,
    SetLeftFoot,
    TransfertWeightLeftLeg
};
 
namespace conscience_core::knowledge {
 
    class CSC_DLL_IMPORTEXPORT CscKnowledgeItemMultiPodWalk : public CscKnowledgeItemTarget {
    public:
        // constructor
        CscKnowledgeItemMultiPodWalk(int knoItemIdIn, string knoItemNameIn, int stepCountTarget, int nbLegs, const vector<const TargetMemberValues *> *inputsTargetValuesIn, const vector<const TargetMemberPonderation *> *inputsTargetPercentsIn, map<string, float> *outputsCanMoveValues, CscUnitType unitTypeIn, string targetOriginMember, bool symemtrySearch, int nbStateMax,  const map<int, float> &maxDeltaPivotByPivotId, bool optionCutLastestIfTheSameAndWorst, bool optionCutBeginingIfTheSame, bool optionDeltaPerStateMaxDegressive, float percentAvg, float percentMax, float percentExtrapolation, int stateDuration, int stateDelay, float solvedAt, float processedAt, CscArtificialIntelligenceTypes aiTypes, CscKnowledgeItemTargetMemberToState *memberToState);
        
        // methods to override
        void sequencePostTraitment(CscSequence *sequenceIn) override;
        int getSequenceCountAutorized(float initStateValue) override;
        float getExtrapolationResult(CscSequence *sequenceIn) override;
        bool isStatesSupportOf(CscState *state1, CscState *state2) override;
        bool isObjectiveSupportOf(CscKnowledgeItem *knowledgeItem) override;
        float evaluateState(CscState *stateIn, const CscWorldEntity *entity, CscState *initialState) override;
        float evaluateSequence(CscSequence *sequence, const CscWorldEntity *entity, CscState *initialStateInit) override;
        CscPerson *generatePerson(CscEnvironmentSimulator *environment, const CscWorldEntity *entity, CscState *initialState, int personId, int generationId, CscGeneration *currentGeneration, CscGeneration *previousGeneration) override;
        
        // destructor
        virtual ~CscKnowledgeItemMultiPodWalk();
        
        // methode create
        static CscKnowledgeItemMultiPodWalk *create(int targetId, string targetName,
                                                    int stepCountTarget, int nbLegs,
                                                           vector<string> targetMemberNames,
                                                           vector<string> relativeToMemberNames,
                                                           vector<float> posX, vector<float> posY, vector<float> posZ,
                                                           vector<float> posPondX, vector<float> posPondY, vector<float> posPondZ,
                                                           vector<float> pitch, vector<float> yaw, vector<float> roll,
                                                           vector<float> pitchPond, vector<float> yawPond, vector<float> rollPond,
                                                           vector<float> accX, vector<float> accY, vector<float> accZ,
                                                           vector<float> accPondX, vector<float> accPondY, vector<float> accPondZ,
                                                           vector<string> outputsCanMoveNames, vector<float> outputsCanMovePonderations,
                                                           CscUnitType unitType, string targetOriginMember,
                                                           bool symemtrySearch,
                                                           int nbStateMax, const map<int, float> &maxDeltaPivotByPivotId,
                                                           bool optionCutLastestIfTheSameAndWorst,
                                                           bool optionCutBeginingIfTheSame,
                                                           bool optionDeltaPerStateMaxDegressive,
                                                           float percentAvg,
                                                           float percentMax,
                                                           float percentExtrapolation,
                                                           int stateDuration,
                                                           int stateDelay,
                                                           float solvedAt,
                                                           float processedAt,
                                                           bool outputTypes[],
                                                           CscArtificialIntelligenceTypes aiTypes,
                                                           map<int, CscPoint3d*> centerOfMassOfAllMemberByMemberId) {
 
            vector<const TargetMemberValues *> *targetMembersValues = new vector<const TargetMemberValues *>();
            vector<const TargetMemberPonderation *> *inputsTargetPercents = new vector<const TargetMemberPonderation *>();
 
            // State objective
            for (int i = 0; i < targetMemberNames.size(); i++) {
                TargetMemberValues *targetMemberVal = new TargetMemberValues();
                CscMemberInputStatus *inputsFinalCore = new CscMemberInputStatus(targetMemberNames.at(i));
                map<CscInputType, float> inputsTargetValuesCore;
                inputsTargetValuesCore[POS_X] = posX.at(i);
                inputsTargetValuesCore[POS_Y] = posY.at(i);
                inputsTargetValuesCore[POS_Z] = posZ.at(i);
                inputsTargetValuesCore[PITCH] = pitch.at(i);
                inputsTargetValuesCore[YAW] = yaw.at(i);
                inputsTargetValuesCore[ROLL] = roll.at(i);
                inputsTargetValuesCore[ACC_X] = accX.at(i);
                inputsTargetValuesCore[ACC_Y] = accY.at(i);
                inputsTargetValuesCore[ACC_Z] = accZ.at(i);
                inputsFinalCore->values = inputsTargetValuesCore;
                targetMemberVal->memberValues = inputsFinalCore;
                targetMemberVal->relativeToMemberName = relativeToMemberNames.at(i);
                targetMembersValues->push_back(targetMemberVal);
            }
 
            // State Percents
            for (int i = 0; i < targetMemberNames.size(); i++) {
                TargetMemberPonderation *targetMemberPonderation = new TargetMemberPonderation();
                CscMemberInputPonderation *percentsFInalCore = new CscMemberInputPonderation(targetMemberNames.at(i));
                map<CscInputType, float> inputsTargetPercentsCore;
                inputsTargetPercentsCore[POS_X] = posPondX.at(i);
                inputsTargetPercentsCore[POS_Y] = posPondY.at(i);
                inputsTargetPercentsCore[POS_Z] = posPondZ.at(i);
                inputsTargetPercentsCore[PITCH] = pitchPond.at(i);
                inputsTargetPercentsCore[YAW] = yawPond.at(i);
                inputsTargetPercentsCore[ROLL] = rollPond.at(i);
                inputsTargetPercentsCore[ACC_X] = accPondX.at(i);
                inputsTargetPercentsCore[ACC_Y] = accPondY.at(i);
                inputsTargetPercentsCore[ACC_Z] = accPondZ.at(i);
                percentsFInalCore->values = inputsTargetPercentsCore;
                targetMemberPonderation->memberValues = percentsFInalCore;
                targetMemberPonderation->relativeToMemberName = relativeToMemberNames.at(i);
                inputsTargetPercents->push_back(targetMemberPonderation);
            }
 
            // OutputsCanMove
            map<string, float> *outputsCanMoveValues = new map<string, float>;
            for (int i = 0; i < outputsCanMoveNames.size(); i++) {
                if (outputsCanMovePonderations.at(i) > 0.0f) {
                    string name = outputsCanMoveNames.at(i);
                    float ponderation = outputsCanMovePonderations.at(i);
                    outputsCanMoveValues->insert({name, ponderation});
                }
            }
            
            CscKnowledgeItemTargetMemberToState *memberToState = CscKnowledgeItemTargetMemberToState::create(targetId, targetName, targetMemberNames, relativeToMemberNames, posX, posY, posZ, posPondX, posPondY, posPondZ, pitch, yaw, roll, pitchPond, yawPond, rollPond, accX, accY, accZ, accPondX, accPondY, accPondZ, outputsCanMoveNames, outputsCanMovePonderations, unitType, targetOriginMember, symemtrySearch, nbStateMax, maxDeltaPivotByPivotId, optionCutLastestIfTheSameAndWorst, optionCutBeginingIfTheSame, optionDeltaPerStateMaxDegressive, percentAvg, percentMax, percentExtrapolation, stateDuration, stateDelay, solvedAt, processedAt, outputTypes, aiTypes, centerOfMassOfAllMemberByMemberId);
            
            return new CscKnowledgeItemMultiPodWalk(targetId, targetName, stepCountTarget, nbLegs, targetMembersValues, inputsTargetPercents, outputsCanMoveValues, unitType, targetOriginMember, symemtrySearch, nbStateMax, maxDeltaPivotByPivotId, optionCutLastestIfTheSameAndWorst, optionCutBeginingIfTheSame, optionDeltaPerStateMaxDegressive, percentAvg, percentMax, percentExtrapolation, stateDuration, stateDelay, solvedAt, processedAt, aiTypes, memberToState);
        }
        
    protected:
        /*
         Addition of states that allow to check if the sequence can be repeated to walk without the robot falling
         */
        void addExtrapolationStates(CscSequence *sequenceIn, const CscWorldEntity *entity) override;
    private:
        // private attributes
        CscKnowledgeItemTargetMemberToState *memberToState;
        
        CscKnowledgeItemTargetArmToPosition *armToPosition;
        
        CscState *bendKneesState;
        
        // private methods
        
        /*
         Determines at which state to start applying the walking constraints according to the individual's creation strategy
         */
        int wichStateToBegin(CscPerson *personToBeCorrected) const;
        
        /*
         Find logic intermediate states between two states in modify it sequence given
         * if previousStateId = -1, previousStateId = initialState
         */
        void interpolateStates(int previousStateId, int nextStateId, int nbStatesToInterpolate, CscSequence *sequence, const CscEntityModel *robotModel);
        
        /*
         Find logic intermediate states between two states and return it in a vector
         */
        vector<CscState *> interpolateStates(CscState *previousState, CscState *nextState, int nbStatesToInterpolate);
        
        /*
         Calculate which state id corresponds to which step
         */
        int calculStateIdFromStageId(int stageId) const;
        
        /*
         Adds a bonus to the objectiveValue of the states, if they meet certain requirements
         */
        void evaluateSpecificsState(int idState, CscSequence *sequence);
        
        /*
         Evaluates if a foot is above the other
         */
        bool oneFootAboveOtherOne(CscState *state, CscSide leftOrRightAbove) const;
        /*
         Evaluates if a foot is in front of the other
         */
        bool oneFootInFront(CscState *state, CscSide leftOrRightAbove) const;
        /*
         Evaluates if a foot is most parallele to the ground the other
         */
        bool oneFootMostParalleleToGroundOtherOne(CscState *state, CscSide leftOrRight) const;
        /*
         Evaluates whether the body is closer to the front foot than the back foot
         */
        bool coreCloserToTheFrontFoot(CscState *state, CscSide sideFrontFoot) const;
        /*
         Evaluates if the foot does not go backwards
         */
        bool footDontGoBack(CscState *state, CscState *previousStageState, CscSide footLeftOrRight) const;
        
        /*
         Determine if all playSequence in physicsSimulator must be played in realTime.
         By default it return false but not with MultiPodWalk.
         */
        bool shouldAlwaysPlayInRealTime() const override;
        /*
         Randomly draw a strategy with a predefined probability for each strategy
         */
        CscStrategyType randomlyDrawStrategyType() const;
        
        /*
         Creates a complete sequence of targetIKParams for first generation
         */
 
        vector<vector<CscJacobianInverseKinematic::TargetIKParams*>> createFirstSequenceWalk(CscState *initialState, const CscWorldEntity *entity);
        
        /*
         Creates a InverseKinematic target from foot to move, initialState of sequence and deplacement required for each axes
         */
 
        CscJacobianInverseKinematic::TargetIKParams* createFootTarget(CscState* initialState, CscSide footSide, float xRelativeInit, float yRelativeInit, float zRelativeInit);
        
        map<int, CscPivotMovementType> createPivotMovementTypeByPivotId(const CscEntityModel *robotModel) override;
        
        map<int, CscPivotMovementType> getPivotMovementTypeByPivotId(const CscEntityModel *robotModel) override;
    };
 
}
#endif /* CscKnowledgeItemMultiPodWalk_h */