ObjectCollisionAvoidance.h

Go to the documentation of this file.

#pragma once
 
#include <armarx/control/common/control_law/aron/ObjectCollisionAvoidanceConfig.aron.generated.h>
 
#include "CollisionAvoidance.h"
 
namespace armarx::control::common::control_law
 
{
 
    class ObjectCollisionAvoidanceController :
        public armarx::control::common::control_law::CollisionAvoidanceController
    {
 
    public:
        using Config = common::control_law::arondto::ObjectCollisionAvoidanceConfig;
 
        ObjectCollisionAvoidanceController(const simox::control::robot::NodeSetInterface* nodeSet);
 
        void run(const Config& c,
                 RtStatusForSafetyStrategy& robotStatus,
                 RecoveryState& rStateSelfColl,
                 RecoveryState& rStateObjColl,
                 const DistanceResults& collisionPairs,
                 const DistanceResults& externalCollisionPairs,
                 const CollisionRobotIndices& collisionRobotIndices,
                 DynamicsModel& dynamicsModel,
                 float torqueLimit,
                 float jointVelLimit,
                 TSCtrlRtStatus& rts);
 
        void calculateExternalCollisionTorque(const Config& c,
                                              RtStatusForSafetyStrategy& rts,
                                              RecoveryState& rState,
                                              const DistanceResults& externalCollisionPairs,
                                              const CollisionRobotIndices& collisionRobotIndices,
                                              const Eigen::VectorXf& qvelFiltered,
                                              double deltaT) const;
 
        void calculateObjectCollisionNullspace(const Config& c,
                                               RtStatusForSafetyStrategy& rtStatus) const;
 
    private:
        std::vector<std::tuple<unsigned int,
                               std::reference_wrapper<Eigen::VectorXf>,
                               std::reference_wrapper<Eigen::MatrixXf>>>
            controllerHierarchy; // priority, type, torque, nullspace
 
        void sortHierarchy();
    };
 
 
} // namespace armarx::control::common::control_law