ObjectCollisionAvoidanceVel.h

Go to the documentation of this file.

#pragma once
 
#include <armarx/control/common/control_law/aron/ObjectCollisionAvoidanceVelConfig.aron.generated.h>
 
#include "CollisionAvoidanceVel.h"
 
namespace armarx::control::common::control_law
 
{
 
    class ObjectCollisionAvoidanceVelController :
        public armarx::control::common::control_law::CollisionAvoidanceVelController
    {
 
    public:
        using Config = common::control_law::arondto::ObjectCollisionAvoidanceVelConfig;
 
        ObjectCollisionAvoidanceVelController(
            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,
                 const Eigen::VectorXf& qpos,
                 const Eigen::VectorXf& qvelFiltered,
                 float velocityLimit,
                 double deltaT);
 
        void calculateExternalCollisionVel(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