ZeroTorqueOrVelocityWithFT.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/>.
 *
 * @package    armarx::skills::control
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <VirtualRobot/Robot.h>
//#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
 
#include <armarx/control/njoint_controller/joint_space/ControllerInterface.h>
 
namespace armarx::control::njoint_controller::joint_space
{
 
    struct NJointZeroTorqueOrVelocityWithFTControllerTarget
    {
        Ice::FloatSeq targetTorqueOrVelocity;
    };
 
    TYPEDEF_PTRS_HANDLE(NJointZeroTorqueOrVelocityWithFTController);
 
    class NJointZeroTorqueOrVelocityWithFTController :
        public NJointControllerWithTripleBuffer<NJointZeroTorqueOrVelocityWithFTControllerTarget>,
        public NJointZeroTorqueOrVelocityWithFTControllerInterface
    {
    public:
        struct RtStatus
        {
            /// targets
            Eigen::VectorXf desiredJointTorques;
            Eigen::VectorXf desiredJointVelocity;
            //            Eigen::VectorXf nullspaceTorque;
            //            Eigen::VectorXf nullspaceVelocity;
 
            /// force torque
            Eigen::Vector6f currentForceTorque;
 
            /// task space variables
            //            Eigen::Vector6f forceImpedance;
            Eigen::Vector6f cartesianVelTarget;
 
            /// current status
            Eigen::VectorXf qpos;
            //            Eigen::VectorXf qvel;
            //            Eigen::VectorXf qvelFiltered;
            //            Eigen::Matrix4f currentPose;
            //            Eigen::Vector6f currentTwist;
            //            Eigen::Matrix4f desiredPose;
 
            //            /// intermediate results
            //            Eigen::Matrix3f poseDiffMatImp;
            //            Eigen::Vector6f poseErrorImp;
 
            /// others
            Eigen::MatrixXf jacobi;
            //            Eigen::MatrixXf jtpinv;
            Eigen::MatrixXf jpinv;
            bool rtSafe;
 
            size_t nDoFTorque;
            size_t nDoFVelocity;
        };
 
        using ConfigPtrT = NJointZeroTorqueOrVelocityWithFTControllerConfigPtr;
        NJointZeroTorqueOrVelocityWithFTController(
            RobotUnitPtr prov,
            const NJointZeroTorqueOrVelocityWithFTControllerConfigPtr& config,
            const VirtualRobot::RobotPtr&);
 
        // NJointControllerInterface interface
        std::string getClassName(const Ice::Current&) const override;
        //        WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
        //        void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
 
        // NJointController interface
        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                   const IceUtil::Time& timeSinceLastIteration) override;
        void onPublish(const SensorAndControl&,
                       const DebugDrawerInterfacePrx&,
                       const DebugObserverInterfacePrx&) override;
        static WidgetDescription::WidgetPtr
        GenerateConfigDescription(const VirtualRobot::RobotPtr&,
                                  const std::map<std::string, ConstControlDevicePtr>&,
                                  const std::map<std::string, ConstSensorDevicePtr>&);
 
        static NJointZeroTorqueOrVelocityWithFTControllerConfigPtr
        GenerateConfigFromVariants(const StringVariantBaseMap& values);
 
    protected:
        void rtPreActivateController() override;
        void rtPostDeactivateController() override;
 
        std::vector<ControlTarget1DoFActuatorZeroTorque*> targetsTorque;
        std::vector<ControlTarget1DoFActuatorZeroVelocity*> targetsVel;
        float maxTorque = std::numeric_limits<float>::max();
        float maxVelocity = std::numeric_limits<float>::max();
        std::atomic_bool useZeroVelocityModeForWrist{true};
        std::vector<bool> torqueControlled;
 
        // NJointZeroTorqueOrVelocityWithFTControllerInterface interface
    public:
        void setControllerTarget(const Ice::FloatSeq&, const Ice::Current&) override;
 
    private:
        VirtualRobot::DifferentialIKPtr ik;
        const float lambda = 2.0f;
        Eigen::MatrixXf I;
    };
 
} // namespace armarx::control::njoint_controller::joint_space