CollisionAvoidanceVel.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 <simox/control/environment/DistanceResult.h>
#include <simox/control/impl/simox/robot/Robot.h>
#include <simox/control/robot/RobotInterface.h>
 
#include <RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h>
 
#include <armarx/control/common/control_law/aron/CollisionAvoidanceVelConfig.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>
 
namespace armarx::control::common::control_law
{
    namespace collision
    {
        void validateConfigData(const arondto::CollisionAvoidanceVelConfig& cfg);
    }
 
    /*
     * This class should be used for one kinematric chain / robot node set
     */
    class CollisionAvoidanceVelController
    {
    public:
        using SCRobot = simox::control::simox::robot::Robot;
        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::CollisionAvoidanceVelConfig;
        using CollisionRobotIndices =
            std::unordered_map<unsigned int, const simox::control::robot::NodeInterface*>;
 
        struct CollisionData
        {
            /// 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 repulsiveVel = -1.0f;  // === changed to repulsiveVel
            float repulsiveVel = -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;
 
            CollisionData(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;
                repulsiveVel = -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 invThresholdSq = 0.0F;
            float kRepulsive = 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;  // === will be replaced by repulsiveVel
            float repulsiveJointVel = 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;  // === desiredJointVel;
            Eigen::VectorXf desiredJointVel;
 
            /// Adaptive threshold for collision recovery
            double timeSinceStart = 0.0;
            bool inRecoveryMode = false;
 
            float trackingError;
 
            Eigen::Vector3f dirErrorImp;
            float totalForceImpedance;
            Eigen::Vector6f projForceImpedance;
            float projTotalForceImpedance;
 
            /// intermediate torque results
            //            Eigen::VectorXf impedanceJointTorque;  // === trajFollowJointVel;
            Eigen::VectorXf trajFollowJointVel;
            //            Eigen::VectorXf kdImpedanceTorque;  // === not needed
            //            Eigen::VectorXf selfCollisionJointTorque;  // === sel
            Eigen::VectorXf selfCollisionJointVel;
            //            Eigen::VectorXf objectCollisionJointTorque;
            Eigen::VectorXf objectCollisionJointVel;
            //            Eigen::VectorXf jointLimitJointTorque;
            Eigen::VectorXf jointLimitJointVel;
 
            //            Eigen::VectorXf selfCollisionTorqueFiltered;
            //            Eigen::VectorXf objectCollisionTorqueFiltered;
            //            Eigen::VectorXf jointLimitTorqueFiltered;
            Eigen::VectorXf selfCollisionVelFiltered;
            Eigen::VectorXf objectCollisionVelFiltered;
            Eigen::VectorXf jointLimitVelFiltered;
 
            /// intermediate projected torques via null space matrices
            /// ===== to remove the following
            //            Eigen::VectorXf projImpedanceJointTorque;
            //            Eigen::VectorXf projCollJointTorque;
            //            Eigen::VectorXf projJointLimJointTorque;
 
            /// === todo find out the usage below
            float impTorqueRatio;
            float impForceRatio;
 
            /// intermediate null space matrices ((self-)collision and joint limit avoidance)
            Eigen::MatrixXf selfCollNullSpace;
            Eigen::MatrixXf objectCollNullSpace;
            Eigen::MatrixXf jointLimNullSpace;
            Eigen::MatrixXf impedanceNullSpace; // === trajFollowNullSpace
            float desiredNullSpace;
 
            Eigen::MatrixXf selfCollNullSpaceFiltered;
            Eigen::MatrixXf objectCollNullSpaceFiltered;
            Eigen::MatrixXf jointLimNullSpaceFiltered;
 
            /// collision avoidance initialization parameters
            Eigen::Vector4f selfCollisionNullSpaceWeights;
            Eigen::Vector4f objectCollisionNullSpaceWeights;
 
            /// distance results
            std::vector<CollisionData> collDataVec;
            unsigned int
                objectCollDataIndex; // from this index, the object collision avoidance data starts
 
            /// self collision avoidance null space intermediate results
            float selfCollK1, selfCollK2, selfCollK3, selfCollK4;
            Eigen::VectorXf selfCollNormalizedJacT;
            Eigen::MatrixXf selfCollTempNullSpaceMatrix;
 
            /// object collision avoidance null space intermediate results
            float objectCollK1, objectCollK2, objectCollK3, objectCollK4;
            Eigen::VectorXf objectCollNormalizedJacT;
            Eigen::MatrixXf objectCollTempNullSpaceMatrix;
 
            /// 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 objectCollNullspaceTime;
            double jointLimitNullspaceTime;
 
            /// collision pair info
            unsigned int collisionPairsNum;
            unsigned int activeCollPairsNum;
 
            /// for generating torques according to priority
            Eigen::MatrixXf nullSpaceAcc;
            Eigen::VectorXf torqueAcc;
            Eigen::VectorXf finalTorque;
 
            void reset(const Config& c,
                       unsigned int nDoF,
                       unsigned int maxCollisionPairs,
                       VirtualRobot::RobotNodeSetPtr& rns);
            void rtPreActivate(const Config& c);
 
        private:
            void initJointLimtDataVec(const Config& c,
                                      unsigned int nDoF,
                                      VirtualRobot::RobotNodeSetPtr& rns);
        };
 
        class RecoveryState
        {
        public:
            float collDistanceThreshold = 0.0F;
            float collDistanceThresholdInit = 0.0F;
            void update(const Config& c, RtStatusForSafetyStrategy& rtStatus, double deltaT);
            void reset();
        };
 
        CollisionAvoidanceVelController(const simox::control::robot::NodeSetInterface* nodeSet);
 
        ~CollisionAvoidanceVelController();
 
        void run(const Config& c,
                 RtStatusForSafetyStrategy& robotStatus,
                 RecoveryState& rStateSelfColl,
                 const DistanceResults& collisionPairs,
                 const CollisionRobotIndices& collisionRobotIndices,
                 DynamicsModel& dynamicsModel,
                 const Eigen::VectorXf& qpos,
                 const Eigen::VectorXf& qvelFiltered,
                 float velocityLimit,
                 double deltaT);
 
        inline std::size_t
        numNodes() const
        {
            return nodeSet->getSize();
        }
 
        inline const simox::control::robot::NodeSetInterface&
        getNodeSet() const
        {
            return *nodeSet;
        }
 
    protected:
        const simox::control::robot::NodeSetInterface* nodeSet;
        const Eigen::MatrixXf I;
        RecoveryState selfCollRecoveryState;
 
        /// avoidance torque methods
        //        void calculateSelfCollisionTorque(const Config& c,
        void calculateSelfCollisionVel(const Config& c,
                                       RtStatusForSafetyStrategy& rtStatus,
                                       RecoveryState& rState,
                                       const DistanceResults& collisionPairs,
                                       const CollisionRobotIndices& collisionRobotIndices,
                                       const Eigen::VectorXf& qvelFiltered,
                                       double deltaT) const;
 
        //        void calculateJointLimitTorque(const Config& c,
        void calculateJointLimitVel(const Config& c,
                                    RtStatusForSafetyStrategy& rtStatus,
                                    const Eigen::VectorXf& qpos,
                                    const Eigen::VectorXf& qvelFiltered) const;
 
        //        void calculateCollisionTorque(CollisionData& collDataVec,
        void calculateCollisionJointVel(CollisionData& collDataVec,
                                        const Config& c,
                                        RtStatusForSafetyStrategy& rts,
                                        const CollisionRobotIndices& collisionRobotIndices,
                                        const Eigen::VectorXf& qvelFiltered,
                                        Eigen::VectorXf& jointTorque,
                                        float distanceThreshold) 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