NJointTaskSpaceImpedanceController.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    TaskSpaceActiveImpedanceControl::ArmarXObjects::NJointTaskSpaceImpedanceController
 * @author     zhou ( you dot zhou at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
 
#include "../ControllerParts/CartesianImpedanceController.h"
 
namespace armarx
{
    /**
    * @defgroup Library-NJointTaskSpaceImpedanceController NJointTaskSpaceImpedanceController
    * @ingroup Library-RobotUnit-NJointControllers
    * A description of the library NJointTaskSpaceImpedanceController.
    *
    * @class NJointTaskSpaceImpedanceController
    * @ingroup Library-NJointTaskSpaceImpedanceController
    * @brief Brief description of class NJointTaskSpaceImpedanceController.
    *
    * Detailed description of class NJointTaskSpaceImpedanceController.
    */
    class NJointTaskSpaceImpedanceController :
        public NJointControllerWithTripleBuffer<CartesianImpedanceController::Config>,
        public NJointTaskSpaceImpedanceControlInterface
    {
    public:
        using ConfigPtrT = NJointTaskSpaceImpedanceControlConfigPtr;
 
        NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit,
                                           const NJointControllerConfigPtr& config,
                                           const VirtualRobot::RobotPtr&);
 
        std::string
        getClassName(const Ice::Current&) const override
        {
            return "TaskSpaceImpedanceController";
        }
 
        // NJointController interface
        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                   const IceUtil::Time& timeSinceLastIteration) override;
 
    protected:
        void onPublish(const SensorAndControl&,
                       const DebugDrawerInterfacePrx&,
                       const DebugObserverInterfacePrx&) override;
 
        void setPosition(const Eigen::Vector3f&, const Ice::Current&) override;
        void setOrientation(const Eigen::Quaternionf&, const Ice::Current&) override;
        void setPositionOrientation(const Eigen::Vector3f& pos,
                                    const Eigen::Quaternionf& ori,
                                    const Ice::Current&) override;
        void setPose(const Eigen::Matrix4f& mat, const Ice::Current&) override;
 
        void setImpedanceParameters(const std::string&,
                                    const Ice::FloatSeq&,
                                    const Ice::Current&) override;
        void setNullspaceConfig(const Eigen::VectorXf& joint,
                                const Eigen::VectorXf& knull,
                                const Eigen::VectorXf& dnull,
                                const Ice::Current&) override;
        void setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg,
                       const Ice::Current&) override;
 
    private:
        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
 
        std::vector<ControlTarget1DoFActuatorTorque*> targets;
 
        struct NJointTaskSpaceImpedanceControllerDebugInfo
        {
            StringFloatDictionary desired_torques;
            float desiredForce_x;
            float desiredForce_y;
            float desiredForce_z;
            float tcpDesiredTorque_x;
            float tcpDesiredTorque_y;
            float tcpDesiredTorque_z;
 
            float tcpDesiredAngularError_x;
            float tcpDesiredAngularError_y;
            float tcpDesiredAngularError_z;
 
            float currentTCPAngularVelocity_x;
            float currentTCPAngularVelocity_y;
            float currentTCPAngularVelocity_z;
 
            float currentTCPLinearVelocity_x;
            float currentTCPLinearVelocity_y;
            float currentTCPLinearVelocity_z;
 
            float quatError;
            float posiError;
        };
 
        TripleBuffer<NJointTaskSpaceImpedanceControllerDebugInfo> debugDataInfo;
 
        std::vector<std::string> jointNames;
        CartesianImpedanceController controller;
 
 
        // NJointController interface
    protected:
        void rtPreActivateController() override;
    };
} // namespace armarx