WholeBodyTrajectoryController.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/>.
 *
 * @author     Fabian Reister (fabian dot reister at kit dot edu)
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <cstddef>
#include <mutex>
#include <vector>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.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/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
#include <RobotAPI/libraries/core/MultiDimPIDController.h>
#include <RobotAPI/libraries/core/PIDController.h>
 
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/control/njoint_controller/core/ConfigurableNJointControllerBase.h>
#include <armarx/control/njoint_controller/joint_space/aron/WholeBodyTrajectoryControllerConfig.aron.generated.h>
 
namespace armarx
{
    class ControlTargetHolonomicPlatformVelocity;
    class SensorValueHolonomicPlatformWithAbsolutePosition;
} // namespace armarx
 
namespace armarx::control::njoint_controller::joint_space::whole_body_trajectory_controller
{
    struct Target
    {
    };
 
    class Trajectory
    {
    public:
        using TrajectoryPoint = armarx::control::njoint_controller::joint_space::
            whole_body_trajectory_controller::arondto::TrajectoryPoint;
 
        using TimestampUS = std::int64_t;
 
        Trajectory(const std::vector<TrajectoryPoint>& traj) : trajectory_(traj)
        {
        }
 
        const TrajectoryPoint&
        initialPoint() const
        {
            return trajectory_.front();
        }
 
        void
        setStartTime(const TimestampUS& timestamp)
        {
            startTimestampUS_ = timestamp;
        }
 
        struct TimeInterval
        {
            TimestampUS min; // [s]
            TimestampUS max; // [s]
 
            bool
            isWithin(const TimestampUS timestamp) const
            {
                return timestamp >= min and timestamp <= max;
            }
        };
 
        std::optional<TrajectoryPoint>
        at(const TimestampUS& timestamp)
        {
            ARMARX_CHECK_NOT_NULL(startTimestampUS_);
 
            // check if timestamp is in range
            if (not timeBounds().isWithin(timestamp))
            {
                if (timestamp <= timeBounds().min)
                {
                    return trajectory_.front();
                }
 
                if (timestamp >= timeBounds().max)
                {
                    return trajectory_.back();
                }
 
                ARMARX_WARNING << "Should not get here!";
                return std::nullopt;
            }
 
            // relative timestamp wrt to start of trajectory
            const TimestampUS relativeTimeUS = timestamp - startTimestampUS_.value();
 
            // we assume that our trajectory is sorted (monotonically increasing timestamps)
 
            // we identify the first element which is timewise after the current time
 
            const auto compareFn = [](const TrajectoryPoint& lhs, const TimestampUS& val) -> bool
            { return lhs.timestampMicroSeconds < val; };
 
            const auto it =
                std::lower_bound(trajectory_.begin(), trajectory_.end(), relativeTimeUS, compareFn);
 
            // if match is first element, return it without interpolation
            if (it == trajectory_.begin())
            {
                return *it;
            }
 
            const auto itBefore = std::prev(it);
 
            // interpolation factor [0,1]
            const double f = static_cast<double>(relativeTimeUS - itBefore->timestampMicroSeconds) /
                             (it->timestampMicroSeconds - itBefore->timestampMicroSeconds);
 
 
            const auto linearInterpolate = [](const std::vector<double>& a,
                                              const std::vector<double>& b,
                                              const double f) -> std::vector<double>
            {
                // FIXME consider 2 pi bounds
                ARMARX_CHECK_EQUAL(a.size(), b.size());
                ARMARX_CHECK_GREATER_EQUAL(f, 0);
                ARMARX_CHECK_LESS_EQUAL(f, 1);
 
                const std::size_t n = a.size();
 
                std::vector<double> c(n);
 
                for (std::size_t i = 0; i < n; i++)
                {
                    c.at(i) = (1 - f) * a.at(i) + f * b.at(i);
                }
 
                return c;
            };
 
            // const auto noInterpolate = [](const std::vector<double>& a,
            //                               const std::vector<double>& b,
            //                               const double f) -> std::vector<double> { return b; };
 
 
            TrajectoryPoint tp;
            tp.timestampMicroSeconds = timestamp;
            tp.jointValues = linearInterpolate(itBefore->jointValues, it->jointValues, f);
            tp.jointValuesPlatform =
                linearInterpolate(itBefore->jointValuesPlatform, it->jointValuesPlatform, f);
            tp.handJointValues =
                linearInterpolate(itBefore->handJointValues, it->handJointValues, f);
 
            tp.jointVelocities =
                linearInterpolate(itBefore->jointVelocities, it->jointVelocities, f);
            tp.jointVelocitiesPlatform = linearInterpolate(
                itBefore->jointVelocitiesPlatform, it->jointVelocitiesPlatform, f);
 
 
            return tp;
        }
 
        bool
        isValid(const TimestampUS& timestamp) const
        {
            ARMARX_CHECK_NOT_NULL(startTimestampUS_);
 
            return timeBounds().isWithin(timestamp);
        }
 
        TimeInterval
        timeBounds() const
        {
            ARMARX_CHECK_NOT_NULL(startTimestampUS_);
 
            return {.min = trajectory_.front().timestampMicroSeconds + startTimestampUS_.value(),
                    .max = trajectory_.back().timestampMicroSeconds + startTimestampUS_.value()};
        }
 
    protected:
    private:
        const std::vector<armarx::control::njoint_controller::joint_space::
                              whole_body_trajectory_controller::arondto::TrajectoryPoint>
            trajectory_;
 
        std::optional<std::int64_t> startTimestampUS_;
    };
 
    struct DebugData
    {
        std::int64_t localTimestamp;
 
        float platformPosDiff;
        float platformOriDiff;
 
        std::vector<float> jointPosDiff;
 
        arondto::TrajectoryPoint targetState;
 
        Eigen::Isometry3f currentTcpPose;
        Eigen::Isometry3f currentPlatformPose;
 
        bool isControllerActive;
    };
 
    class Controller :
        virtual public NJointControllerWithTripleBuffer<Target>,
        virtual public core::ConfigurableNJointControllerBase<arondto::Config>
    {
    public:
        Controller(const RobotUnitPtr& robotUnit,
                   const NJointControllerConfigPtr& config,
                   const VirtualRobot::RobotPtr& robot);
 
 
        std::string getClassName(const Ice::Current& iceCurrent = Ice::emptyCurrent) const override;
 
        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                   const IceUtil::Time& timeSinceLastIteration) override;
 
 
        void onPublish(const SensorAndControl&,
                       const DebugDrawerInterfacePrx&,
                       const DebugObserverInterfacePrx&) override;
 
 
    protected:
        void rtPreActivateController() override;
 
        virtual void onDisconnectNJointController();
 
        void
        updateConfig(const ConfigurableNJointControllerBaseT::AronConfigT& configData) override
        {
            // NYI
        }
 
    private:
        struct ControlTargets
        {
            ControlTargetHolonomicPlatformVelocity* platformTarget;
            std::vector<ControlTarget1DoFActuatorVelocity*> jointTargets;
            std::vector<ControlTarget1DoFActuatorPosition*> handTargets;
        } controlTargets_;
 
        const GlobalRobotLocalizationSensorDevice::SensorValueType* svGlobalRobotLocalization_;
        std::vector<const SensorValue1DoFActuatorPosition*> jointSensorValues_;
 
 
        std::optional<Trajectory> trajectory_;
 
        // std::size_t trajIdx_ = 0;
 
        MultiDimPIDController pidPlatform_;
        PIDController opidPlatform_;
 
        std::optional<MultiDimPIDController> pidArm_;
 
        std::vector<std::string> jointNames_;
        std::vector<std::string> fingerJointNames_;
 
        VirtualRobot::RobotPtr robot_;
        VirtualRobot::RobotNodePtr rtTcpNode_;
 
        VirtualRobot::RobotPtr debugRobot_;
        VirtualRobot::RobotNodePtr publishTcpNode_;
 
 
        TripleBuffer<DebugData> bufferRtToOnPublish_;
 
 
        std::vector<DebugData> whatever_;
        std::atomic_bool whateverFlag_ = true;
    };
 
} // namespace armarx::control::njoint_controller::joint_space::whole_body_trajectory_controller