CscJacobianInverseKinematic.h

Go to the documentation of this file.

/*
 * CscJacobianInverseKinematic.h
 *
 *  Created on: 31 mai 2014
 *      Author: AilvinCarbon
 */
 
#ifndef CscJacobianInverseKinematic_h
#define CscJacobianInverseKinematic_h
 
#include "../Simulator/CscEnvironmentSimulator.h"
#include "CoreObjects/Exchange/Common/CscWorldElementsFactory.h"
#include "Util/conscience_util.h"
 
#include <bullet/Bullet3Common/b3AlignedObjectArray.h>
#include <bullet/ThirdPartyLibs/BussIK/Jacobian.h>
#include <bullet/ThirdPartyLibs/BussIK/Node.h>
#include <bullet/ThirdPartyLibs/BussIK/Tree.h>
#include <bullet/ThirdPartyLibs/BussIK/VectorRn.h>
 
#include <math.h>
 
#define RADIAN(X) ((X) * RadiansToDegrees)
#define MAX_NUM_EFFECT 100
#define PROJECTION_DISTANCE 5.0f
#define MAX_TIME_FOR_RESET_TARGETS_IN_MS 1000
 
using namespace conscience_core::core_objects::exchange::common;
 
enum Method {
    IK_JACOB_TRANS = 0,
    IK_PURE_PSEUDO,
    IK_DLS,
    IK_SDLS,
    IK_DLS_SVD,
    NOTHING
};
 
class CSC_DLL_IMPORTEXPORT CscJacobianInverseKinematic {
    public:
    struct CSC_DLL_IMPORTEXPORT TargetIKParams {
    private:
        const int stationaryPivotId;
        const int memberToMoveId;
        CscPoint3d *targetToReach;
        optional<CscPoint3d *> orientationTarget;
    public:
        TargetIKParams(int stationaryPivotId, int memberToMoveId, CscPoint3d *targetToReach, optional<CscPoint3d *> orientationTarget = nullopt);
        const int getStationaryPivotId() const;
        const int getMemberToMoveId() const;
        CscPoint3d *getTargetToReach() const;
        CscPoint3d *getOrientationTarget() const;
        ~TargetIKParams();
        TargetIKParams *clone();
    };
 
    CscJacobianInverseKinematic(CscEnvironmentSimulator *env, float maxHorizontalDistance, float maxVerticalDistance, float maxDepthDistance, CscPoint3d *offsetCenter, CscState *beginState, const CscEntityModel *entityModel, vector<TargetIKParams *> targets, map<int, CscPoint3d *> &centerOfMassOfAllMemberByMemberId, bool debugWithBox = false, bool constraintOnDistance = false, vector<int> pivotIdToIgnore = vector<int>());
 
    void initIk();
 
    CscPoint3d *getEntityCenterOfMass();
 
    void changeTargets(vector<TargetIKParams *> newTargets, bool relativeValues = false);
    void compute();
    void computeRandomly();
    double getDistanceFromTarget();
    map<int, float> getResult() const;
    vector<vector<int>> getPivotIdPaths() const;
    vector<TargetIKParams *> getTargets() const;
    void updateThetasRandomly();
 
    ~CscJacobianInverseKinematic();
 
    const float targetPrecision = 1.5f;
 
    void changeTargetsWithCenterOfMass(const bool applyDimensionXYZ[3], vector<int> idTargetsToChange);
    
    Tree getIkTree();
 
private:
    b3AlignedObjectArray<Node *> ikNodes;
    Tree ikTree;
    Jacobian *ikJacobian = nullptr;
    int sleepCounter = 0;
    VectorR3 targetsPositions[MAX_NUM_EFFECT];
    const int sleepsPerStep = 100; // Make higher to take larger steps less frequently
    float offsetX = 1505;
    float offset = 1500;
    float offsetZ = 1500;
    float offsetY = 107;
    float maxHorizontalDistance;
    float maxVerticalDistance;
    float maxDepthDistance;
    CscPoint3d *offsetCenter = nullptr;
    CscWorldObject *targetBox = nullptr;
    CscState *beginState = nullptr;
    vector<vector<int>> pivotIdPaths; // contains the different paths of the members
    const CscEntityModel *entityModel = nullptr;
    Method choiceMethod = NOTHING;
    int countForMethod = 0;
    vector<TargetIKParams *> targets;
    vector<TargetIKParams *> lastTargetsValid;
    bool debugWithBox;
    bool constraintOnDistance;
    map<int, CscPoint3d *> initCenterOfMassOfAllMemberByMemberId;
 
    map<int, float> result;
    vector<int> nNodeCenterOfMass;
    map<int, int> memberIdFromNode;
    CscEnvironmentSimulator *env;
    CscPoint3d *entityCenterOfMass;
    vector<string> objectsId;
 
    int idObject = 3;
    unsigned long long timeForResetTargets;
    vector<int> pivotIdToIgnore;
    bool computeEnable = true;
 
    CscPoint3d *findCenterOfMass();
 
    void clearTargets();
 
    void updateThetas();
 
    Tree createTreePivots(bool centerOfMass = true);
 
    vector<int> createPivotIdPath(int stationaryPivotId, int memberToMoveId);
 
    void fillTreePivotId(vector<int> &treeToFill, int pivotId, int endId, const map<int, int> &memberAIdByPivotId, const map<int, int> &memberBIdByPivotId, const vector<const CscMemberJoint *> *pivots, int memberId);
 
    Method nextMethod(Method currentMethod);
 
    Tree addNodeCenterOfMass(int nbTarget, int indexPivotIdPaths, int ikNodesSizeWithoutCenterOfMass, int nbNodes, Tree &currentTree);
};
 
#endif /* CscJacobianInverseKinematic_H_ */