NJointTrajectoryController.h

Go to the documentation of this file.

#pragma once
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXGui/interface/StaticPlotterInterface.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
#include <RobotAPI/libraries/core/TrajectoryController.h>
 
#include "NJointControllerWithTripleBuffer.h"
 
namespace armarx
{
    TYPEDEF_PTRS_HANDLE(NJointTrajectoryController);
 
    class NJointTrajectoryControllerControlData
    {
    public:
        TrajectoryControllerPtr trajectoryCtrl;
    };
 
    /**
     * @brief The NJointTrajectoryController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointTrajectoryController :
        public NJointControllerWithTripleBuffer<NJointTrajectoryControllerControlData>,
        public NJointTrajectoryControllerInterface
    {
    public:
        NJointTrajectoryController(RobotUnit* prov,
                                   const NJointControllerConfigPtr& config,
                                   const VirtualRobot::RobotPtr&);
 
        ~NJointTrajectoryController();
 
        // NJointControllerInterface interface
        std::string getClassName(const Ice::Current&) const override;
        void onInitNJointController() override;
        void onConnectNJointController() override;
 
        // NJointController interface
        void rtPreActivateController() override;
        void rtPostDeactivateController() override;
        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                   const IceUtil::Time& timeSinceLastIteration) override;
 
        // NJointTrajectoryControllerInterface interface
        void setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&) override;
        bool isFinished(const Ice::Current&) override;
        double getCurrentTimestamp(const Ice::Current&) override;
        float getCurrentProgressFraction(const Ice::Current&) override;
 
        double getCurrentTrajTime() const;
        void setLooping(bool looping);
        double getTrajEndTime() const;
        TrajectoryPtr getTrajectoryCopy() const;
 
    private:
        IceUtil::Time startTime;
        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
        std::vector<const SensorValue1DoFActuatorPosition*> sensors;
        LimitlessStateSeq limitlessStates;
        StaticPlotterInterfacePrx plotter;
        DebugObserverInterfacePrx dbgObs;
 
        Eigen::VectorXf currentPos;
        bool looping = false;
        float direction = 1.0;
        double trajEndTime;
        TrajectoryControllerPtr trajectoryCtrl;
        NJointTrajectoryControllerConfigPtr cfg;
        bool finished = false;
        double currentTimestamp = 0;
 
        double startTimestamp = 0, endTimestamp = 1;
    };
} // namespace armarx