CollisionAvoidance.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/CollisionAvoidanceConfig.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
{
    namespace helper
    {
        /// helper functions
        Eigen::Vector4f calculateTransitionWeights(float z1, float z2);
    } // namespace helper
 
    namespace collision
    {
        void validateConfigData(arondto::CollisionAvoidanceConfig& cfg);
    }
 
    /*
     * This class should be used for one kinematric chain / robot node set
     */
    class CollisionAvoidanceController
    {
    public:
        using InertiaPtr = std::shared_ptr<simox::control::geodesics::metric::Inertia>;
        using DistanceResults = std::vector<::simox::control::environment::DistanceResult>;
        using DistanceResultsPtr = std::shared_ptr<DistanceResults>;
        using SimoxRobotPtr = std::shared_ptr<simox::control::simox::robot::Robot>;
        using FTConfig = common::ft::arondto::FTConfig;
        using Config = common::control_law::arondto::CollisionAvoidanceConfig;
 
        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 = -1.0f;
 
            // Debugging values
            float repulsiveForce = -1.0f;
            float localStiffness = -1.0f;
            Eigen::VectorXf projectedJacT;
            float distanceVelocity = -1.0f;
            Eigen::Vector3f direction = Eigen::Vector3f::Zero();
            Eigen::Vector3f point = Eigen::Vector3f::Zero();
            float damping = -1.0f;
            float projectedMass = -1.0f;
            float desiredNSColl = -1.0f;
 
            // mapped back from joint space with projectedJac pinv
            float projectedImpedanceForce = -1.0f;
 
            SelfCollisionData(int size) :
                jacobian(Eigen::MatrixXf::Zero(size, size)),
                projectedJacT(Eigen::VectorXf::Zero(size))
            {
            }
 
            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;
            }
        };
 
        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 = false;
            float qLimLow = 0.0f;
            float qLimHigh = 0.0f;
            float thresholdRange = 0.0f;
            float qposThresholdLow = 0.0f; // unique for each joint in rep. torque function
            float qposThresholdHigh = 0.0f;
            /// null space parameters
            float qposZ1Low = 0.0f;
            float qposZ2Low = 0.0f;
            float qposZ1High = 0.0f;
            float qposZ2High = 0.0f;
            Eigen::Vector4f jointLimitNullSpaceWeightsLow = Eigen::Vector4f::Zero();
            Eigen::Vector4f jointLimitNullSpaceWeightsHigh = Eigen::Vector4f::Zero();
            float desiredNSjointLim = 0.0f;
            float repulsiveTorque = 0.0f;
            float totalDamping = 0.0f;
        };
 
        /// internal status of the controller, containing intermediate variables, mutable targets
        class RtStatusForSafetyStrategy
        {
        public:
            /// targets
            Eigen::VectorXf desiredJointTorques;
 
            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;
            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;
 
            /// distance results
            simox::control::environment::DistanceResult activeDistPair;
            std::vector<SelfCollisionData> selfCollDataVec;
            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
 
            // TODO rename to jointLimitDataVec
            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 inertia;
            Eigen::MatrixXf inertiaInverse;
 
            /// 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;
 
            void reset(const Config& c,
                       const unsigned int nDoF,
                       const unsigned int maxCollisionPairs,
                       VirtualRobot::RobotNodeSetPtr& rns);
            void rtPreActivate(){};
 
        private:
            void initJointLimtDataVec(const Config& c,
                                      const unsigned int nDoF,
                                      VirtualRobot::RobotNodeSetPtr& rns);
        };
 
        unsigned int numOfJoints; // for nonRT use case
        VirtualRobot::RobotNodeSetPtr rtRNS;
 
    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,
                                          RtStatusForSafetyStrategy& rtStatus,
                                          DistanceResultsPtr collisionPairs,
                                          std::shared_ptr<std::vector<int>> pointsOnArm,
                                          int pointsOnArmIndex,
                                          std::vector<std::string> selfCollisionNodes,
                                          Eigen::VectorXf& qvelFiltered);
        void calculateJointLimitTorque(Config& c,
                                       RtStatusForSafetyStrategy& rtStatus,
                                       Eigen::VectorXf& qpos,
                                       Eigen::VectorXf& qvelFiltered);
 
        /// caclulate null spaces
        void calculateSelfCollisionNullspace(Config& c, RtStatusForSafetyStrategy& rtStatus);
        void calculateJointLimitNullspace(Config& c,
                                          RtStatusForSafetyStrategy& rtStatus,
                                          Eigen::VectorXf& qpos);
 
    public:
        CollisionAvoidanceController(const VirtualRobot::RobotNodeSetPtr& rtRns);
 
        ~CollisionAvoidanceController();
 
        void initialize(SimoxRobotPtr& simoxControlRobot);
        void preactivateInit(Config& c, RtStatusForSafetyStrategy& rtStatus);
 
        void run(Config& c,
                 RtStatusForSafetyStrategy& robotStatus,
                 DistanceResultsPtr& collisionPairs,
                 std::shared_ptr<std::vector<int>>& pointsOnArm,
                 int pointsOnArmIndex,
                 InertiaPtr& inertiaInstance,
                 std::vector<std::string>& selfCollisionNodes,
                 Eigen::VectorXf& qpos,
                 Eigen::VectorXf& qvelFiltered,
                 float torqueLimit);
    };
} // namespace armarx::control::common::control_law