ControlTarget1DoFActuator.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    RobotAPI::ArmarXObjects::ActuatorVelocityTarget
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
 
#include "ControlTargetBase.h"
 
namespace armarx
{
#define make_ControlTarget1DoFActuator(type, invalid, name, varname, cmode)                        \
    class name : public ControlTargetBase                                                          \
    {                                                                                              \
    public:                                                                                        \
        type varname = ControllerConstants::ValueNotSetNaN;                                        \
        name() = default;                                                                          \
        name(const name&) = default;                                                               \
        name(name&&) = default;                                                                    \
        name(type val) : varname{val}                                                              \
        {                                                                                          \
        }                                                                                          \
        name& operator=(const name&) = default;                                                    \
        name& operator=(name&&) = default;                                                         \
        name&                                                                                      \
        operator=(type val)                                                                        \
        {                                                                                          \
            varname = val;                                                                         \
            return *this;                                                                          \
        }                                                                                          \
        virtual const std::string&                                                                 \
        getControlMode() const override                                                            \
        {                                                                                          \
            return cmode;                                                                          \
        }                                                                                          \
        virtual void                                                                               \
        reset() override                                                                           \
        {                                                                                          \
            varname = invalid;                                                                     \
        }                                                                                          \
        virtual bool                                                                               \
        isValid() const override                                                                   \
        {                                                                                          \
            return std::isfinite(varname);                                                         \
        }                                                                                          \
        DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION static ControlTargetInfo<name>      \
        GetClassMemberInfo()                                                                       \
        {                                                                                          \
            ControlTargetInfo<name> cti;                                                           \
            cti.addMemberVariable(&name::varname, #varname);                                       \
            return cti;                                                                            \
        }                                                                                          \
    }
 
    make_ControlTarget1DoFActuator(float,
                                   ControllerConstants::ValueNotSetNaN,
                                   ControlTarget1DoFActuatorPosition,
                                   position,
                                   ControlModes::Position1DoF);
    make_ControlTarget1DoFActuator(float,
                                   ControllerConstants::ValueNotSetNaN,
                                   ControlTarget1DoFActuatorVelocity,
                                   velocity,
                                   ControlModes::Velocity1DoF);
    make_ControlTarget1DoFActuator(float,
                                   ControllerConstants::ValueNotSetNaN,
                                   ControlTarget1DoFActuatorTorque,
                                   torque,
                                   ControlModes::Torque1DoF);
    make_ControlTarget1DoFActuator(float,
                                   ControllerConstants::ValueNotSetNaN,
                                   ControlTarget1DoFActuatorZeroTorque,
                                   torque,
                                   ControlModes::ZeroTorque1DoF);
    make_ControlTarget1DoFActuator(float,
                                   ControllerConstants::ValueNotSetNaN,
                                   ControlTarget1DoFActuatorCurrent,
                                   current,
                                   ControlModes::Current1DoF);
#undef make_ControlTarget1DoFActuator
 
    class ControlTarget1DoFActuatorTorqueVelocity : public ControlTarget1DoFActuatorVelocity
    {
    public:
        float maxTorque;
 
        const std::string&
        getControlMode() const override
        {
            return ControlModes::VelocityTorque;
        }
 
        void
        reset() override
        {
            ControlTarget1DoFActuatorVelocity::reset();
            maxTorque = -1.0f; // if < 0, default value for joint is to be used
        }
 
        bool
        isValid() const override
        {
            return ControlTarget1DoFActuatorVelocity::isValid() && std::isfinite(maxTorque);
        }
 
        static ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity>
        GetClassMemberInfo()
        {
            ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> cti;
            cti.addBaseClass<ControlTarget1DoFActuatorVelocity>();
            cti.addMemberVariable(&ControlTarget1DoFActuatorTorqueVelocity::maxTorque, "maxTorque");
            return cti;
        }
 
        DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
    };
 
    class ActiveImpedanceControlTarget : public ControlTargetBase
    {
    public:
        float position;
        float kp;
        float kd;
 
        const std::string&
        getControlMode() const override
        {
            return ControlModes::ActiveImpedance;
        }
 
        void
        reset() override
        {
            position = 0;
            kp = 0;
            kd = 0;
        }
 
        bool
        isValid() const override
        {
            return std::isfinite(position) && kp >= 0;
        }
 
        static ControlTargetInfo<ActiveImpedanceControlTarget>
        GetClassMemberInfo()
        {
            ControlTargetInfo<ActiveImpedanceControlTarget> cti;
            cti.addMemberVariable(&ActiveImpedanceControlTarget::position, "position");
            cti.addMemberVariable(&ActiveImpedanceControlTarget::kp, "kp");
            cti.addMemberVariable(&ActiveImpedanceControlTarget::kd, "kd");
            return cti;
        }
 
        DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
    };
 
    class ControlTarget1DoFActuatorPWM : public ControlTargetBase
    {
    public:
        std::int16_t pwm;
 
    public:
        const std::string&
        getControlMode() const override
        {
            return ControlModes::PWM1DoF;
        }
 
        void
        reset() override
        {
            pwm = std::numeric_limits<std::int16_t>::max();
        }
 
        bool
        isValid() const override
        {
            return std::abs(pwm) < std::numeric_limits<std::int16_t>::max();
        }
 
        static ControlTargetInfo<ControlTarget1DoFActuatorPWM>
        GetClassMemberInfo()
        {
            ControlTargetInfo<ControlTarget1DoFActuatorPWM> cti;
            cti.addMemberVariable(&ControlTarget1DoFActuatorPWM::pwm, "pwm_motor");
            return cti;
        }
 
        DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
    };
 
    class ControlTarget1DoFActuatorPositionWithPWMLimit : public ControlTarget1DoFActuatorPosition
    {
    public:
        int16_t maxPWM;
 
    protected:
        std::int16_t pwmDefaultLimit = 256;
        std::int16_t pwmHardLimit = 256;
 
    public:
        const std::string&
        getControlMode() const override
        {
            return ControlModes::PositionWithPwmLimit1DoF;
        }
 
        void
        reset() override
        {
            maxPWM = pwmDefaultLimit;
            ControlTarget1DoFActuatorPosition::reset();
        }
 
        bool
        isValid() const override
        {
            return std::abs(maxPWM) <= pwmHardLimit && ControlTarget1DoFActuatorPosition::isValid();
        }
 
        static ControlTargetInfo<ControlTarget1DoFActuatorPositionWithPWMLimit>
        GetClassMemberInfo()
        {
            ControlTargetInfo<ControlTarget1DoFActuatorPositionWithPWMLimit> cti;
            cti.addBaseClass<ControlTarget1DoFActuatorPosition>();
            cti.addMemberVariable(&ControlTarget1DoFActuatorPositionWithPWMLimit::maxPWM, "maxPWM");
            cti.addMemberVariable(&ControlTarget1DoFActuatorPositionWithPWMLimit::pwmHardLimit,
                                  "pwmHardLimit");
            return cti;
        }
 
        void
        setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
        {
            pwmHardLimit = hard;
            pwmDefaultLimit = def;
        }
 
        DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
    };
 
    class ControlTarget1DoFActuatorVelocityWithPWMLimit : public ControlTarget1DoFActuatorVelocity
    {
    public:
        int16_t maxPWM;
 
    protected:
        std::int16_t pwmDefaultLimit = 256;
        std::int16_t pwmHardLimit = 256;
 
    public:
        const std::string&
        getControlMode() const override
        {
            return ControlModes::VelocityWithPwmLimit1DoF;
        }
 
        void
        reset() override
        {
            maxPWM = pwmDefaultLimit;
            ControlTarget1DoFActuatorVelocity::reset();
        }
 
        bool
        isValid() const override
        {
            return std::abs(maxPWM) <= pwmHardLimit && ControlTarget1DoFActuatorVelocity::isValid();
        }
 
        static ControlTargetInfo<ControlTarget1DoFActuatorVelocityWithPWMLimit>
        GetClassMemberInfo()
        {
            ControlTargetInfo<ControlTarget1DoFActuatorVelocityWithPWMLimit> cti;
            cti.addBaseClass<ControlTarget1DoFActuatorVelocity>();
            cti.addMemberVariable(&ControlTarget1DoFActuatorVelocityWithPWMLimit::maxPWM, "maxPWM");
            cti.addMemberVariable(&ControlTarget1DoFActuatorVelocityWithPWMLimit::pwmHardLimit,
                                  "pwmHardLimit");
            return cti;
        }
 
        void
        setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
        {
            pwmHardLimit = hard;
            pwmDefaultLimit = def;
        }
 
        DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
    };
} // namespace armarx