CartesianNaturalPositionControllerProxy.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
 * Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     armar-user (armar-user at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <memory>
 
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
#include "NaturalIK.h"
 
namespace armarx
{
    typedef std::shared_ptr<class CartesianNaturalPositionControllerProxy>
        CartesianNaturalPositionControllerProxyPtr;
 
    class CartesianNaturalPositionControllerProxy
    {
    public:
        struct DynamicKp
        {
            bool enabled = false;
            float maxKp = 1;
            float sigmaMM = 50;
            void calculate(float error, float& KpElb, float& KpJla);
        };
 
        struct KpBaseSettings
        {
            float baseKpTcpPos;
            float baseKpTcpOri;
            float baseKpElbPos;
            float baseKpJla;
            float baseKpNs;
            float maxPositionAcceleration;
            float maxOrientationAcceleration;
            float maxNullspaceAcceleration;
        };
 
        struct VelocityBaseSettings
        {
            float scaleTcpPosVel = 1;
            float scaleTcpOriVel = 1;
            float scaleElbPosVel = 1;
            float scaleJointVelocities = 1;
            float scaleNullspaceVelocities = 1;
 
            Ice::FloatSeq baseJointVelocity;
            float basePosVel;
            float baseOriVel;
        };
 
        struct WaypointConfig
        {
            float referencePosVel;
            float thresholdTcpPosReached;
            float rad2mmFactor = 100 / M_PI;
            //float thresholdTcpOriReached;
        };
 
        struct WaypointTargets
        {
            Eigen::Matrix4f tcpTarget;
            NaturalDiffIK::Mode setOri;
            std::optional<float> minElbowHeight = std::nullopt;
            std::vector<float> userNullspaceTargets;
        };
 
        //typedef std::shared_ptr<class Waypoint> WaypointPtr;
        class Waypoint
        {
        public:
            WaypointTargets targets;
            WaypointConfig config;
            //bool offsetFTonEnter = false;
            //bool resetFTonExit = false;
            //float forceTheshold = -1;
 
            bool reached(const VirtualRobot::RobotNodePtr& tcp);
            Waypoint& setConfig(const WaypointConfig& config);
            Waypoint& setNullspaceTargets(const std::vector<float>& userNullspaceTargets);
 
            //aron::AronObjectPtr toAron();
            //void fromAron(aron::AronObjectPtr aron);
        };
 
        class ScopedJointValueRestore
        {
        public:
            ScopedJointValueRestore(const VirtualRobot::RobotNodeSetPtr& rns);
            ~ScopedJointValueRestore();
 
        private:
            std::vector<float> jointValues;
            VirtualRobot::RobotNodeSetPtr rns;
        };
 
        CartesianNaturalPositionControllerProxy(
            const NaturalIK& _ik,
            const NaturalIK::ArmJoints& arm,
            const RobotUnitInterfacePrx& _robotUnit,
            const std::string& _controllerName,
            NJointCartesianNaturalPositionControllerConfigPtr _config);
        static NJointCartesianNaturalPositionControllerConfigPtr
        MakeConfig(const std::string& rns, const std::string& elbowNode);
 
        void init();
 
        bool setTarget(const Eigen::Matrix4f& tcpTarget,
                       NaturalDiffIK::Mode setOri,
                       std::optional<float> minElbowHeight = std::nullopt);
        void setNullspaceTarget(const std::vector<float>& nullspaceTargets);
 
        Eigen::Vector3f getCurrentTargetPosition();
        Eigen::Vector3f getCurrentElbowTargetPosition();
 
        float getTcpPositionError();
        float getTcpOrientationError();
        float getElbPositionError();
 
        bool isFinalWaypointReached();
 
        void useCurrentFTasOffset();
        void enableFTLimit(float force, float torque, bool useCurrentFTasOffset);
        void disableFTLimit();
        FTSensorValue getCurrentFTValue(bool substactOffset);
        FTSensorValue getAverageFTValue(bool substactOffset);
        void stopClear();
 
 
        void setRuntimeConfig(CartesianNaturalPositionControllerConfig runCfg);
        CartesianNaturalPositionControllerConfig getRuntimeConfig();
 
        void addWaypoint(const Waypoint& waypoint);
        Waypoint createWaypoint(const Eigen::Vector3f& tcpTarget,
                                const std::vector<float>& userNullspaceTargets,
                                std::optional<float> minElbowHeight = std::nullopt);
        Waypoint createWaypoint(const Eigen::Vector3f& tcpTarget,
                                std::optional<float> minElbowHeight = std::nullopt);
        Waypoint createWaypoint(const Eigen::Matrix4f& tcpTarget,
                                std::optional<float> minElbowHeight = std::nullopt);
        void clearWaypoints();
        void setDefaultWaypointConfig(const WaypointConfig& config);
 
 
        std::string getStatusText();
 
        //void setTargetVelocity(const Eigen::VectorXf& cv);
 
        //void setNullSpaceControl(bool enabled);
 
        static std::vector<float>
        CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr& rns, float maxPosVel);
 
        void setMaxVelocities(float referencePosVel);
        void updateBaseKpValues(const CartesianNaturalPositionControllerConfig& runCfg);
 
        bool update();
        Waypoint& currentWaypoint();
        bool isLastWaypoint();
 
        void cleanup();
 
 
        NJointCartesianNaturalPositionControllerInterfacePrx getInternalController();
 
        void setDynamicKp(DynamicKp dynamicKp);
        DynamicKp getDynamicKp();
        static std::vector<float> ScaleVec(const std::vector<float>& vec, float scale);
 
        void setActivateControllerOnInit(bool value);
 
    private:
        void updateDynamicKp();
        void updateNullspaceTargets();
 
        bool onWaypointChanged();
 
        NaturalIK _ik;
        RobotUnitInterfacePrx _robotUnit;
        std::string _side;
        std::string _controllerName;
        NJointCartesianNaturalPositionControllerConfigPtr _config;
        CartesianNaturalPositionControllerConfig _runCfg;
        bool _controllerCreated = false;
        NJointCartesianNaturalPositionControllerInterfacePrx _controller;
        DynamicKp _dynamicKp;
        Eigen::Matrix4f _tcpTarget;
        Eigen::Matrix4f _elbTarget;
        NaturalDiffIK::Mode _setOri;
        NaturalIK::SoechtingForwardPositions _fwd;
        VelocityBaseSettings _velocityBaseSettings;
        KpBaseSettings _kpBaseSettings;
        NaturalIK::ArmJoints _arm;
        std::vector<Waypoint> _waypoints;
        size_t _currentWaypointIndex = 0;
        NaturalIK::Parameters _natikParams;
        std::vector<float> _userNullspaceTargets;
        std::vector<float> _naturalNullspaceTargets;
        std::vector<VirtualRobot::RobotNodePtr> _rnsToElb;
        bool _setJointNullspaceFromNaturalIK = true;
 
        std::map<std::string, WaypointConfig> _defaultWaypointConfigs;
        bool _waypointChanged = false;
        FTSensorValue _ftOffset;
        bool _activateControllerOnInit = false;
    };
} // namespace armarx