SafetyTaskspaceImpedanceController.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * 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     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
 
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 
#include <armarx/control/common/control_law/aron/SafetyTaskspaceImpedanceControllerConfig.aron.generated.h>
#include <armarx/control/common/control_law/common.h>
#include <armarx/control/common/ft/FTSensor.h>
#include <armarx/control/common/ft/aron/FTConfig.aron.generated.h>
 
#include <simox/control/environment/CollisionRobot.h>
#include <simox/control/geodesics/metric/inertia.h>
#include <simox/control/impl/simox/robot/Robot.h>
#include <simox/control/robot/NodeInterface.h>
 
namespace armarx::control::common::control_law
{
 
    class SafetyTaskspaceImpedanceController
    {
    public:
        using InertiaPtr = std::shared_ptr<simox::control::geodesics::metric::Inertia>;
        using DistanceResultsPtr =
            std::shared_ptr<std::vector<simox::control::environment::DistanceResult>>;
        using SimoxRobotPtr = std::shared_ptr<simox::control::simox::robot::Robot>;
        using FTConfig = common::ft::arondto::FTConfig;
        using Config = common::control_law::arondto::SafetyTaskspaceImpedanceControllerConfig;
        using ConfigDict =
            common::control_law::arondto::SafetyTaskspaceImpedanceControllerConfigDict;
 
        /// internal status of the controller, containing intermediate variables, mutable targets
        struct RtStatus : public RobotStatus
        {
            /// targets
            Eigen::VectorXf desiredJointTorques;
            Eigen::VectorXf nullspaceTorque;
            Eigen::VectorXf jointDampingTorque;
 
            /// force torque
            Eigen::Vector6f currentForceTorque;
 
            /// task space variables
            Eigen::Vector6f forceImpedance;
            Eigen::Vector6f kdForceImpedance;
 
            /// current status
            Eigen::VectorXf qpos;
            Eigen::VectorXf qvel;
            Eigen::VectorXf qvelFiltered;
 
            Eigen::Matrix4f currentPose;
            Eigen::Vector6f currentTwist;
            Eigen::Matrix4f desiredPose;
 
            /// intermediate impedance results
            Eigen::Matrix3f poseDiffMatImp;
            Eigen::Vector6f poseErrorImp;
            float trackingError;
 
            Eigen::Vector3f dirErrorImp;
            float totalForceImpedance;
            Eigen::Vector6f projForceImpedance;
            float projTotalForceImpedance;
 
            /// intermediate torque results
            Eigen::VectorXf impedanceJointTorque;
            Eigen::VectorXf kdImpedanceTorque;
            Eigen::VectorXf selfCollisionJointTorque;
            Eigen::VectorXf jointLimitJointTorque;
 
            Eigen::VectorXf selfCollisionTorqueFiltered;
            Eigen::VectorXf jointLimitTorqueFiltered;
 
            /// intermediate projected torques via null space matrices
            Eigen::VectorXf projImpedanceJointTorque; // TODO: rename it to proImpedanceJointTorque
            Eigen::VectorXf projSelfCollJointTorque;
            Eigen::VectorXf projJointLimJointTorque;
 
            float impTorqueRatio;
            float impForceRatio;
 
            /// intermediate null space matrices (self-collision and joint limit avoidance)
            Eigen::MatrixXf selfCollNullSpace;
            Eigen::MatrixXf jointLimNullSpace;
            float desiredNullSpace;
 
            Eigen::MatrixXf selfCollNullSpaceFiltered;
            Eigen::MatrixXf jointLimNullSpaceFiltered;
 
            /// self-collision avoidance initialization parameters
            Eigen::Vector4f selfCollisionNullSpaceWeights;
 
            /// self-collision avoidance intermediate results
            const simox::control::robot::NodeInterface* node;
            simox::control::Pose localTransformation;
            bool node1OnArm;
            bool node2OnArm;
            unsigned int node1Index;
            unsigned int node2Index;
 
            struct selfCollisionData
            {
                /// the distances and projected jacobians from the generateSelfCollisionTorque() method
                /// are needed for the null space calculation
                std::string nodeName; // for debugging
                std::string otherName; // for debugging
                Eigen::MatrixXf jacobian;
                float minDistance;
 
                // Debugging values
                float repulsiveForce;
                float localStiffness;
                Eigen::VectorXf projectedJacT;
                float distanceVelocity;
                Eigen::Vector3f direction;
                Eigen::Vector3f point;
                float damping;
                float projectedMass;
                float desiredNSColl;
 
 
                float
                    projectedImpedanceForce; // mapped back from joint space with projectedJac pinv
 
                selfCollisionData(int size) : jacobian(size, size), projectedJacT(size)
                {
                    nodeName = "";
                    otherName = "";
                    jacobian.setZero();
                    projectedJacT.setZero();
                    nodeName = "";
                    minDistance = -1.0f;
                    repulsiveForce = -1.0f;
                    localStiffness = -1.0f;
                    distanceVelocity = -1.0f;
                    direction.setZero();
                    point.setZero();
                    damping = -1.0f;
                    projectedMass = -1.0f;
                    desiredNSColl = -1.0f;
                    projectedImpedanceForce = -1.0f;
                }
 
                void
                clearValues()
                {
                    nodeName = "";
                    otherName = "";
                    jacobian.setZero();
                    projectedJacT.setZero();
                    direction.setZero();
                    point.setZero();
                    minDistance = -1.0f;
                    repulsiveForce = -1.0f;
                    localStiffness = -1.0f;
                    distanceVelocity = -1.0f;
                    damping = -1.0f;
                    projectedMass = -1.0f;
                    desiredNSColl = -1.0f;
                    projectedImpedanceForce = -1.0f;
                }
 
                void
                setDistValues(std::string nodeName, std::string otherName, float minDistance)
                {
                    this->nodeName = nodeName;
                    this->otherName = otherName;
                    this->minDistance = minDistance;
                }
 
                void
                setCalcValues(const Eigen::MatrixXf& jacobian,
                              float repulsiveForce,
                              float localStiffness,
                              const Eigen::VectorXf& projectedJacT,
                              float distanceVelocity,
                              const Eigen::Vector3f& direction,
                              const Eigen::Vector3f& point,
                              float damping,
                              float projectedMass,
                              float desiredNSColl)
                {
                    this->jacobian = jacobian;
                    this->repulsiveForce = repulsiveForce;
                    this->localStiffness = localStiffness;
                    this->projectedJacT = projectedJacT;
                    this->distanceVelocity = distanceVelocity;
                    this->direction = direction;
                    this->point = point;
                    this->damping = damping;
                    this->projectedMass = projectedMass;
                    this->desiredNSColl = desiredNSColl;
                }
            };
 
            std::vector<selfCollisionData> selfCollDataVec;
            simox::control::environment::DistanceResult activeDistPair;
            std::vector<selfCollisionData> evalData;
            int evalDataIndex;
 
            /// self-collision avoidance null space intermediate results
            float k1, k2, k3, k4;
            Eigen::VectorXf normalizedJacT;
            Eigen::MatrixXf tempNullSpaceMatrix;
 
            /// joint limit avoidance initialization parameters
            struct jointRangeBufferZoneData
            {
                /// for each joint, that has a limit, based on the config parameters:
                /// jointRangeBufferZone
                /// jointRangeBufferZone_z1
                /// jointRangeBufferZone_z2
                ///
                /// the specific values depending on the joint range of each joint have to
                /// be calculated (and stored, so the calculation is only done in the init)
                std::string jointName;
                bool isLimitless;
                float qLimLow;
                float qLimHigh;
                float thresholdRange;
                float qposThresholdLow; // unique for each joint in rep. torque function
                float qposThresholdHigh;
                /// null space parameters
                float qposZ1Low;
                float qposZ2Low;
                float qposZ1High;
                float qposZ2High;
                Eigen::Vector4f jointLimitNullSpaceWeightsLow;
                Eigen::Vector4f jointLimitNullSpaceWeightsHigh;
                float desiredNSjointLim;
                float repulsiveTorque;
                float totalDamping;
 
                jointRangeBufferZoneData()
                {
                    jointName = "";
                    isLimitless = false;
                    qLimLow = 0.0;
                    qLimHigh = 0.0;
                    thresholdRange = 0.0;
                    qposThresholdLow = 0.0;
                    qposThresholdHigh = 0.0;
                    qposZ1Low = 0.0;
                    qposZ2Low = 0.0;
                    qposZ1High = 0.0;
                    qposZ2High = 0.0;
                    jointLimitNullSpaceWeightsLow.setZero();
                    jointLimitNullSpaceWeightsHigh.setZero();
                    desiredNSjointLim = -1.0;
                    repulsiveTorque = -1.0;
                    totalDamping = -1.0;
                }
            };
 
            std::vector<jointRangeBufferZoneData> jointLimitData;
 
            /// joint limit avoidance intermediate results
            float jointVel;
            float jointInertia;
 
            float localStiffnessJointLim;
            float dampingJointLim;
 
            float k1Lo, k2Lo, k3Lo, k4Lo;
            float k1Hi, k2Hi, k3Hi, k4Hi;
 
            /// others
            Eigen::MatrixXf jacobi;
            Eigen::MatrixXf jtpinv;
            Eigen::MatrixXf inertia;
            Eigen::MatrixXf inertiaInverse;
            bool rtSafe;
 
            /// time status
            double collisionPairTime;
            double preFilterTime;
            double collisionTorqueTime;
            double jointLimitTorqueTime;
            double selfCollNullspaceTime;
            double jointLimitNullspaceTime;
 
            /// collision pair info
            int collisionPairsNum;
            int activeCollPairsNum;
 
 
            /// other actuated joint debug info
            float torsoJointValuef;
            float neck1JointValuef;
            float neck2JointValuef;
            double torsoJointValued;
            double neck1JointValued;
            double neck2JointValued;
            Eigen::VectorXd setActuatedJointValues;
            Eigen::VectorXd getActuatedJointValues;
        };
 
        unsigned int numOfJoints; // for nonRT use case
    private:
        std::atomic_bool enablePreactivateInit{false};
 
        Eigen::MatrixXf I;
 
        VirtualRobot::DifferentialIKPtr ik;
 
        const float lambda = 2.0f;
 
        simox::control::robot::NodeSetInterface* simoxNodeSet;
 
 
        /// avoidance torque methods
        void calculateSelfCollisionTorque(
            Config& c,
            RtStatus& rtStatus,
            std::shared_ptr<std::vector<::simox::control::environment::DistanceResult>>
                collisionPairs,
            std::shared_ptr<std::vector<int>> pointsOnArm,
            int pointsOnArmIndex,
            std::vector<std::string> selfCollisionNodes);
        void calculateJointLimitTorque(Config& c, RtStatus& rtStatus);
 
        /// caclulate null spaces
        void calculateSelfCollisionNullspace(Config& c, RtStatus& rtStatus);
        void calculateJointLimitNullspace(Config& c, RtStatus& rtStatus);
 
        /// helper functions
        Eigen::Vector4f calculateTransitionWeights(float z1, float z2);
 
 
    public:
        VirtualRobot::RobotNodePtr tcp;
        VirtualRobot::RobotNodePtr rtTCP;
        common::ft::FTSensor ftsensor;
 
        // void updateFT(const FTConfig& c, RtStatus& rtStatus);
        void initialize(const VirtualRobot::RobotNodeSetPtr& rns,
                        const VirtualRobot::RobotNodeSetPtr& rtRns,
                        SimoxRobotPtr& simoxControlRobot);
        void
        preactivateInit(const VirtualRobot::RobotNodeSetPtr& rns, Config& c, RtStatus& rtStatus);
 
        void run(Config& c,
                 RtStatus& robotStatus,
                 DistanceResultsPtr& collisionPairs,
                 std::shared_ptr<std::vector<int>>& pointsOnArm,
                 int pointsOnArmIndex,
                 InertiaPtr& inertiaInstance,
                 std::vector<std::string>& selfCollisionNodes);
        void firstRun();
    };
} // namespace armarx::control::common::control_law