scene.h

#pragma once
 
#include "base/robot.h"
 
#include <Eigen/Dense>
#include <vector>
#include <cstdlib>
#include <cmath>
#include <memory>
 
namespace bmpf {
 
    class Scene {
    public:
 
        Scene() = default;
 
        explicit Scene(const std::vector<std::shared_ptr<bmpf::BaseRobot>> &robot);
 
        ~Scene() = default;
 
        unsigned long getActiveRobotCnt() const;
 
        unsigned long addObject(std::string path);
 
        unsigned long addObject(std::string path, std::vector<double> &transformVector);
 
        void deleteRobot(long robotNum);
 
        void saveToFile(const std::string &path);
 
        void loadFromFile(const std::string &path);
 
        std::vector<std::pair<long, long>> getJointIndexRanges() { return _jointIndexRanges; }
 
        unsigned long getJointCnt() const { return _jointCnt; }
 
        bool isSingleActiveRobot() const { return getActiveRobotCnt() == 1; };
 
        std::vector<std::shared_ptr<bmpf::JointParams>> getJointParamsList() { return _jointParams; }
 
        std::vector<double> getMaxSpeedModules() { return _maxSpeedModules; }
 
        std::vector<double> getMaxAccelerationModules() { return _maxAccelerationModules; }
 
        std::vector<std::shared_ptr<bmpf::Link>> getLinks() const { return _links; }
 
        std::vector<Eigen::Matrix4d> getTransformMatrices(const std::vector<double> &state);
 
        std::vector<double> getEndEffectorPositions(const std::vector<double> &state);
 
        std::vector<double> getEndEffectorDiffPositions(const std::vector<double>& state, int iVal);
 
        std::vector<double> getEndEffectorDiff2Positions(const std::vector<double>& state, int iVal, int jVal);
 
        std::vector<Eigen::Matrix4d> getEndEffectorTransformMatrices(const std::vector<double>& state);
 
        std::vector<Eigen::Matrix4d> getEndEffectorDiffTransformMatrices(const std::vector<double>& state, int iVal);
 
        std::vector<Eigen::Matrix4d>
        getEndEffectorDiff2TransformMatrices(const std::vector<double>& state, int iVal, int jVal);
 
        std::vector<double> getEndEffectorRGVectors(const std::vector<double> &state);
 
        std::vector<double> getEndEffectorDiffRGVector(const std::vector<double> &state, int iVal);
 
        std::vector<double> getEndEffectorDiff2RGVector(const std::vector<double> &state, int iVal, int jVal);
 
        std::vector<double> getAllLinkPositions(const std::vector<double> &state);
 
        std::vector<double> getAllLinkRGVectors(const std::vector<double> &state);
 
        bool isStateEnabled(const std::vector<double> &state);
 
        bool isStateEnabled(std::vector<double> state, unsigned long robotNum);
 
        std::vector<double> getRandomState();
 
        const std::vector<std::shared_ptr<bmpf::BaseRobot>> &getRobots() const { return _objects; }
 
        std::string getScenePath() const { return _path; }
 
        std::vector<std::shared_ptr<Scene>> getSingleRobotScenes() { return _singleRobotScenes; }
 
        std::vector<double> getSingleObjectState(const std::vector<double> &state, unsigned long objectNum);
 
        std::vector<std::vector<double>> getGroupedTranslation() const;
 
        std::vector<std::vector<double>> getGroupedRotation() const;
 
        std::vector<std::vector<double>> getGroupedScale() const;
 
        std::vector<std::vector<double>> getGroupedTransformVector() const;
 
        std::vector<std::vector<std::string>> getGroupedModelPaths() const;
 
        void setGroupedTranslation(std::vector<std::vector<double>> groupedTranslation);
 
        void setGroupedRotation(std::vector<std::vector<double>> groupedRotation);
 
        void setGroupedScale(std::vector<std::vector<double>> groupedScale);
 
        void setGroupedTransformVector(std::vector<std::vector<double>> transformVectors);
 
        static std::vector<double> concatenateStates(const std::vector<std::vector<double>> &states);
 
        template<typename ...ArgsT>
        std::vector<double> concatenateStates(ArgsT ... states) { return concatenateStates({states...}); }
 
        static std::vector<double> concatenateStates(std::initializer_list<std::vector<double>> states);
 
        template<typename ...ArgsT>
        std::vector<int> concatenateCoords(ArgsT ... states) { return concatenateCoords({states...}); }
 
        static std::vector<int> concatenateCoords(const std::vector<std::vector<int>> &coords);
 
        std::vector<bool> areObjectsJointed() { return _areObjectsJointed; }
 
        std::vector<int> getJointedObjectIndexes() { return _jointedObjectIndexes; }
 
        std::vector<int> getNotJointedObjectIndexes() { return _notJointedObjectIndexes; }
 
    protected:
        void _initObjects();
 
    private:
        std::vector<double> _maxSpeedModules;
        std::vector<double> _maxAccelerationModules;
        std::vector<std::pair<long, long>> _jointIndexRanges;
        unsigned int _jointCnt{};
        std::vector<std::shared_ptr<bmpf::JointParams>> _jointParams;
        std::vector<std::shared_ptr<bmpf::Link>> _links;
        std::vector<std::shared_ptr<bmpf::BaseRobot>> _objects;
        std::string _path;
        std::vector<std::shared_ptr<Scene>> _singleRobotScenes;
        std::vector<bool> _areObjectsJointed;
        std::vector<int> _notJointedObjectIndexes;
        std::vector<int> _jointedObjectIndexes;
    };
 
 
}