MoveJointPlatformTrajectory.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    RobotSkillTemplates::MotionAndPlatformControlGroup
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <chrono>
 
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
#include <ArmarXCore/statechart/xmlstates/XMLState.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
 
#include <RobotSkillTemplates/statecharts/MotionAndPlatformControlGroup/MoveJointPlatformTrajectory.generated.h>
 
#include "MotionAndPlatformControlGroupStatechartContext.generated.h"
 
namespace armarx::MotionAndPlatformControlGroup
{
    class MoveJointPlatformTrajectory :
        public MoveJointPlatformTrajectoryGeneratedBase < MoveJointPlatformTrajectory >
    {
    public:
        using DataFieldIdentifierBaseLists = std::pair<DataFieldIdentifierBaseList, DataFieldIdentifierBaseList>;
        using TimedVariantBaseLists = std::pair<TimedVariantBaseList, TimedVariantBaseList>;
 
        MoveJointPlatformTrajectory(const XMLStateConstructorParams& stateData):
            XMLStateTemplate < MoveJointPlatformTrajectory > (stateData), MoveJointPlatformTrajectoryGeneratedBase < MoveJointPlatformTrajectory > (stateData)
        {
        }
 
        // inherited from StateBase
        void onEnter() override;
        void run() override;
        void onBreak() override;
        void onExit() override;
        void waitForDone(bool stopAtCurrentPos);
 
        // static functions for AbstractFactory Method
        static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
        static SubClassRegistry Registry;
 
        DataFieldIdentifierBaseLists getDataFieldIdentifiers(const std::string& channelName)
        {
            DataFieldIdentifierBaseLists datafields;
            datafields.first.reserve(3);
            datafields.second.reserve(jointNames.size() - 3);
            {
                const std::string platformObserverName = context->getProperty<std::string>("PlatformUnitObserverName").getValue();
                if (channelName == "jointangles")
                {
                    datafields.first.emplace_back(new DataFieldIdentifier {platformObserverName, "platformPose", "positionX"});
                    datafields.first.emplace_back(new DataFieldIdentifier {platformObserverName, "platformPose", "positionY"});
                    datafields.first.emplace_back(new DataFieldIdentifier {platformObserverName, "platformPose", "rotation"});
                }
                else
                {
                    datafields.first.emplace_back(new DataFieldIdentifier {platformObserverName, "platformVelocity", "velocityX"});
                    datafields.first.emplace_back(new DataFieldIdentifier {platformObserverName, "platformVelocity", "velocityY"});
                    datafields.first.emplace_back(new DataFieldIdentifier {platformObserverName, "platformVelocity", "velocityRotation"});
                }
            };
            const std::string observerName = context->getProperty<std::string>("KinematicUnitObserverName").getValue();
            for (size_t i = 3; i < jointNames.size(); ++i)
            {
                datafields.second.emplace_back(new DataFieldIdentifier {observerName, channelName, jointNames.at(i)});
            }
            return datafields;
        }
        void ctrlInterpol();
        void ctrlNative();
 
        //helper
        bool doingLastConfig() const
        {
            return currentCfgIt == lastCfgIt;
        }
        bool doneAllConfigs() const
        {
            return currentCfgIt == t->end();
        }
 
        void cacheValues()
        {
            cachedValues.first  = context->getPlatformUnitObserver()->getDataFields(datafieldsValues.first);
            cachedValues.second = context->getKinematicUnitObserver()->getDataFields(datafieldsValues.second);
        }
        void cacheVelocities()
        {
            cachedVelocities.first  = context->getPlatformUnitObserver()->getDataFields(datafieldsVelocities.first);
            cachedVelocities.second = context->getKinematicUnitObserver()->getDataFields(datafieldsVelocities.second);
        }
 
        static float getFromList(const TimedVariantBaseLists& lists, std::size_t i)
        {
            if (i < 3)
            {
                return lists.first.at(i)->getFloat();
            }
            return lists.second.at(i - 3)->getFloat();
        }
        float getValue(std::size_t i) const;
        float getVelocity(std::size_t i) const;
 
        void setVelocity(NameValueMap& vels)
        {
            const auto x = vels.at("x");
            const auto y = vels.at("y");
            const auto a = vels.at("alpha");
            context->getPlatformUnit()->move(x, y, a);
            ARMARX_INFO << "VelocitiesPlatform: x= " << x << " y=" << y << " alpha=" << a;
            vels.erase("x");
            vels.erase("y");
            vels.erase("alpha");
            ARMARX_ON_SCOPE_EXIT
            {
                vels["x"] = x;
                vels["y"] = y;
                vels["alpha"] = a;
            };
            context->getKinematicUnit()->setJointVelocities(vels);
        }
        void setValues(NameValueMap& values)
        {
            const auto x = values.at("x");
            const auto y = values.at("y");
            const auto a = values.at("alpha");
            context->getPlatformUnit()->moveTo(x, y, a, translationTargetTolerance, jointTargetTolerance);
            values.erase("x");
            values.erase("y");
            values.erase("alpha");
            ARMARX_ON_SCOPE_EXIT
            {
                values["x"] = x;
                values["y"] = y;
                values["alpha"] = a;
            };
            context->getKinematicUnit()->setJointAngles(values);
        }
        void setMaxVelocity(NameValueMap& vels)
        {
            const auto x = vels.at("x");
            const auto y = vels.at("y");
            const auto a = vels.at("alpha");
            context->getPlatformUnit()->setMaxVelocities((x + y) / 2.0, a);
            vels.erase("x");
            vels.erase("y");
            vels.erase("alpha");
            ARMARX_ON_SCOPE_EXIT
            {
                vels["x"] = x;
                vels["y"] = y;
                vels["alpha"] = a;
            };
            context->getKinematicUnit()->setJointVelocities(vels);
        }
        void setControlMode(ControlMode mode)
        {
            NameControlModeMap controlModes;
            for (size_t i = 3; i < jointNames.size(); i++)
            {
                controlModes[jointNames.at(i)] = mode;
            }
            context->getKinematicUnit()->switchControlMode(controlModes);
        }
        bool validJointNames() const
        {
            return jointNames.size() >= 3
                   && jointNames.at(0) == "x"
                   && jointNames.at(1) == "y"
                   && jointNames.at(2) == "alpha";
        }
 
        float getMaxVel(std::size_t joint) const
        {
            return (joint < jointMaxSpeeds.size() ? jointMaxSpeeds.at(joint) : in.getJointMaxSpeed());
        }
        float getMaxAcc(std::size_t joint) const
        {
            return (joint < maxAccs.size() ? maxAccs.at(joint) : in.getJointMaxAccel());
        }
 
        float getTargetTolerance(std::size_t i) const
        {
            return (i < 2 ? translationTargetTolerance : jointTargetTolerance);
        }
 
        float getConfigChangeTolerance(std::size_t i) const
        {
            if (doingLastConfig())
            {
                return getTargetTolerance(i);
            }
            return i < 2 ? in.getNextConfigTranslationTolerance() : in.getNextConfigTolerance();
        }
 
 
 
        static int sign(double val);
 
        // DO NOT INSERT ANY CLASS MEMBERS,
        // use stateparameters instead,
        // if classmember are neccessary nonetheless, reset them in onEnter
        TrajectoryPtr t;
        std::vector<std::string> jointNames;
        std::vector<float> maxAccs;
        std::vector<float> jointMaxSpeeds;
        MotionAndPlatformControlGroupStatechartContext* context;
 
        DataFieldIdentifierBaseLists datafieldsValues;
        TimedVariantBaseLists cachedValues;
 
        DataFieldIdentifierBaseLists datafieldsVelocities;
        TimedVariantBaseLists cachedVelocities;
 
        const std::chrono::milliseconds period
        {
            10
        };
        const float secondsPerPeriod = static_cast<float>(period.count()) / 1000.f;
        float jointTargetTolerance;
        float translationTargetTolerance;
        typename Trajectory::ordered_view::const_iterator lastCfgIt;
        typename Trajectory::ordered_view::const_iterator currentCfgIt;
 
        ConditionIdentifier targetReachedCondition;
        ActionEventIdentifier timeoutEvent;
    };
}