TrajectoryControllerSubUnit.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::TrajectoryControllerSubUnit
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <mutex>
 
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
 
#include "../NJointControllers/NJointTrajectoryController.h"
#include "../RobotUnit.h"
#include "KinematicSubUnit.h"
#include "RobotUnitSubUnit.h"
 
namespace armarx
{
    class TrajectoryControllerSubUnitPropertyDefinitions :
        public armarx::ComponentPropertyDefinitions
    {
    public:
        TrajectoryControllerSubUnitPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineRequiredProperty<std::string>("KinematicUnitName",
                                                "Name of the KinematicUnit. Only needed for "
                                                "returning to zeroFramePose while resetting.");
            defineOptionalProperty<float>("Kp",
                                          1.f,
                                          "Proportional gain value of PID Controller",
                                          PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>("Ki",
                                          0.0f,
                                          "Integral gain value of PID Controller",
                                          PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>("Kd",
                                          0.0f,
                                          "Differential gain value of PID Controller",
                                          PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>(
                "absMaximumVelocity",
                80.0f,
                "The PID will never set a velocity above this threshold (degree/s)");
            defineOptionalProperty<std::string>("CustomRootNode",
                                                "",
                                                "If this value is set, the root pose of the motion "
                                                "is set for this node instead of the root joint.");
 
            defineOptionalProperty<bool>("EnableRobotPoseUnit",
                                         false,
                                         "Specify whether RobotPoseUnit should be used to move the "
                                         "robot's position. Only useful in simulation.");
            defineOptionalProperty<std::string>(
                "RobotPoseUnitName",
                "RobotPoseUnit",
                "Name of the RobotPoseUnit to which the robot position should be sent");
        }
    };
 
    TYPEDEF_PTRS_HANDLE(TrajectoryControllerSubUnit);
 
    class TrajectoryControllerSubUnit :
        virtual public RobotUnitSubUnit,
        virtual public TrajectoryPlayerInterface,
        virtual public Component
    {
        // TrajectoryControllerSubUnitInterface interface
    public:
        bool startTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
        bool pauseTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
        bool stopTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
        bool resetTrajectoryPlayer(bool moveToFrameZeroPose,
                                   const Ice::Current& = Ice::emptyCurrent) override;
 
        void loadJointTraj(const TrajectoryBasePtr& jointTraj,
                           const Ice::Current& = Ice::emptyCurrent) override;
        void loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj,
                              const Ice::Current& = Ice::emptyCurrent) override;
 
        void setLoopPlayback(bool loop, const Ice::Current& = Ice::emptyCurrent) override;
        Ice::Double getEndTime(const Ice::Current& = Ice::emptyCurrent) override;
        Ice::Double getTrajEndTime(const Ice::Current& = Ice::emptyCurrent) override;
        Ice::Double getCurrentTime(const Ice::Current& = Ice::emptyCurrent) override;
        void setEndTime(Ice::Double, const Ice::Current& = Ice::emptyCurrent) override;
 
        // Within the RobotUnit the NJointTrajectoryController is always in VelocityControl
        void
        setIsVelocityControl(bool, const Ice::Current& = Ice::emptyCurrent) override
        {
        }
 
        void setIsPreview(bool, const Ice::Current& = Ice::emptyCurrent) override;
        bool
        setJointsInUse(const std::string&, bool, const Ice::Current& = Ice::emptyCurrent) override;
        void enableRobotPoseUnit(bool, const Ice::Current& = Ice::emptyCurrent) override;
 
        void considerConstraints(bool, const Ice::Current& = Ice::emptyCurrent) override;
 
        // RobotUnitSubUnit interface
        void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
 
        void setup(RobotUnit* rUnit);
 
        // KinematicUnitListener interface
        void
        reportControlModeChanged(const NameControlModeMap&,
                                 Ice::Long,
                                 bool,
                                 const Ice::Current&) override
        {
        }
 
        void
        reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
        {
        }
 
        void
        reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
        {
        }
 
        void
        reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
        {
        }
 
        void
        reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
        {
        }
 
        void
        reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
        {
        }
 
        void
        reportJointMotorTemperatures(const NameValueMap&,
                                     Ice::Long,
                                     bool,
                                     const Ice::Current&) override
        {
        }
 
        void
        reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override
        {
        }
 
        void
        setOffset(const ::Eigen::Matrix4f&, const ::Ice::Current& = ::Ice::emptyCurrent) override
        {
        }
 
        // ManagedIceObject interface
    protected:
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
 
        std::string
        getDefaultName() const override
        {
            return "TrajectoryPlayer";
        }
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr
        createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr{
                new TrajectoryControllerSubUnitPropertyDefinitions{getConfigIdentifier()}};
        }
 
    private:
        NJointTrajectoryControllerPtr
        createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist);
        void assureTrajectoryController();
        bool controllerExists();
        void previewRun();
 
        RobotUnit* robotUnit = NULL;
        NJointTrajectoryControllerPtr jointTrajController;
        std::string controllerName = this->getName() + "_" + "JointTrajectory";
 
        TrajectoryPtr jointTraj;
 
        // so far no usage -> need to implement usage of robotPoseUnit (only for simulation) -> does it really belong here?
        TrajectoryPtr basePoseTraj;
        bool robotPoseUnitEnabled;
        std::string robotPoseUnitName;
 
        double endTime;
        double trajEndTime;
        std::vector<std::string> usedJoints;
 
        std::string kinematicUnitName;
        KinematicUnitInterfacePrx kinematicUnit;
        std::string customRootNode;
 
        float kp;
        float ki;
        float kd;
        float maxVelocity;
        bool considerConstraintsForTrajectoryOptimization = false;
 
        bool isPreview = false;
        DebugDrawerInterfacePrx debugDrawer;
        PeriodicTask<TrajectoryControllerSubUnit>::pointer_type previewTask;
 
        std::mutex dataMutex;
        std::recursive_mutex controllerMutex;
        bool recreateController = true;
 
        // Component interface
    public:
        virtual void
        componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
    };
} // namespace armarx