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/VirtualRobot.h>
 
#include <RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.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/DistanceResult.h>
#include <simox/control/robot/RobotInterface.h>
 
namespace armarx::control::common::control_law
{
    namespace helper
    {
        /// helper functions
        Eigen::Vector4f calculateTransitionWeights(float z1, float z2);
    } // namespace helper
 
    namespace collision
    {
        void validateConfigData(const arondto::CollisionAvoidanceConfig& cfg);
    }
 
    /*
     * This class should be used for one kinematric chain / robot node set
     */
    class CollisionAvoidanceController
    {
    public:
        using SCRobot = ::simox::control::robot::RobotInterface;
        using DistanceResult = ::simox::control::environment::DistanceResult;
        using DistanceResults = std::vector<DistanceResult>;
        using DynamicsModel = simox::control::dynamics::RBDLModel;
        using FTConfig = common::ft::arondto::FTConfig;
        using Config = common::control_law::arondto::CollisionAvoidanceConfig;
        using CollisionRobotIndices = std::unordered_map<unsigned int, const simox::control::robot::NodeInterface*>;
 
        struct SelfCollisionData
        {
            /// the distances and projected jacobians from the generateSelfCollisionTorque() method
            /// are needed for the null space calculation
            unsigned int node1 = 0;
            unsigned int node2 = 0;
            Eigen::MatrixXd 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 point1 = Eigen::Vector3f::Zero();
            Eigen::Vector3f point2 = 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(unsigned int size) :
                jacobian(Eigen::MatrixXd::Zero(3, size)),
                projectedJacT(Eigen::VectorXf::Zero(size))
            {
            }
 
            inline void
            clearValues()
            {
                node1 = 0;
                node2 = 0;
                jacobian.setZero();
                projectedJacT.setZero();
                direction.setZero();
                point1.setZero();
                point2.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;
            }
        };
 
        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:
            /// global pose
            Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
            /// 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;
 
            /// distance results
            std::vector<SelfCollisionData> selfCollDataVec;
 
            /// 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::MatrixXd inertia;
            Eigen::MatrixXf inertiaInverse;
 
            /// time status
            double collisionPairTime;
            double preFilterTime;
            double collisionTorqueTime;
            double jointLimitTorqueTime;
            double selfCollNullspaceTime;
            double jointLimitNullspaceTime;
 
            /// collision pair info
            unsigned int collisionPairsNum;
            unsigned int activeCollPairsNum;
 
            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);
        };
 
        CollisionAvoidanceController(const simox::control::robot::NodeSetInterface* nodeSet);
 
        ~CollisionAvoidanceController();
 
        void run(const Config& c,
                 RtStatusForSafetyStrategy& robotStatus,
                 const DistanceResults& collisionPairs,
                 const CollisionRobotIndices& collisionRobotIndices,
                 DynamicsModel& dynamicsModel,
                 const Eigen::VectorXf& qpos,
                 const Eigen::VectorXf& qvelFiltered,
                 float torqueLimit);
 
        inline std::size_t
        numNodes() const
        {
            return nodeSet->getSize();
        }
 
        inline const simox::control::robot::NodeSetInterface&
        getNodeSet() const
        {
            return *nodeSet;
        }
 
    private:
        const simox::control::robot::NodeSetInterface* nodeSet;
        const Eigen::MatrixXf I;
 
        /// avoidance torque methods
        void calculateSelfCollisionTorque(const Config& c,
                                          RtStatusForSafetyStrategy& rtStatus,
                                          const DistanceResults& collisionPairs,
                                          const CollisionRobotIndices& collisionRobotIndices,
                                          const Eigen::VectorXf& qvelFiltered) const;
 
        void calculateJointLimitTorque(const Config& c,
                                       RtStatusForSafetyStrategy& rtStatus,
                                       const Eigen::VectorXf& qpos,
                                       const Eigen::VectorXf& qvelFiltered) const;
 
        /// caclulate null spaces
        void calculateSelfCollisionNullspace(const Config& c, RtStatusForSafetyStrategy& rtStatus) const;
        void calculateJointLimitNullspace(const Config& c,
                                          RtStatusForSafetyStrategy& rtStatus,
                                          const Eigen::VectorXf& qpos) const;
    };
} // namespace armarx::control::common::control_law