TrajectoryPlayer.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    RobotComponents::ArmarXObjects::TrajectoryPlayer
 * @author     zhou ( derekyou dot zhou at gmail dot com )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <mutex>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
 
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
#include <RobotAPI/interface/units/RobotPoseUnitInterface.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
#include <RobotAPI/libraries/core/Trajectory.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx
{
    /**
     * @class TrajectoryPlayerPropertyDefinitions
     * @brief
     */
    class TrajectoryPlayerPropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        TrajectoryPlayerPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineRequiredProperty<std::string>(
                "KinematicUnitName",
                "Name of the KinematicUnit to which the joint values should be sent");
            defineRequiredProperty<std::string>("KinematicTopicName",
                                                "Name of the KinematicUnit reporting topic");
 
            defineOptionalProperty<std::string>(
                "ArmarXProjects",
                "Armar4",
                "Comma-separated list with names of ArmarXProjects (e.g. 'Armar3,Armar4'). The MMM "
                "XML File can be specified relatively to a data path of one of these projects.");
            defineOptionalProperty<float>(
                "FPS",
                100.0f,
                "FPS with which the recording should be executed. Velocities will be adapted.");
            defineOptionalProperty<bool>(
                "LoopPlayback", false, "Specify whether motion should be repeated after execution");
            defineOptionalProperty<bool>(
                "isVelocityControl",
                true,
                "Specify whether the PID Controller should be used (==velocity control)");
            defineOptionalProperty<float>("Kp", 1.f, "Proportional gain value of PID Controller");
            defineOptionalProperty<float>("Ki", 0.00001f, "Integral gain value of PID Controller");
            defineOptionalProperty<float>("Kd", 0.0f, "Differential gain value of PID Controller");
            defineOptionalProperty<float>(
                "absMaximumVelocity",
                120.0f,
                "The PID will never set a velocity above this threshold (degree/s)");
            defineOptionalProperty<std::string>(
                "RobotNodeSetName",
                "RobotState",
                "The name of the robot node set to be used (only need for PIDController mode)");
            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");
            defineOptionalProperty<float>(
                "Offset.x", 0.f, "x component of initial robot position (mm)");
            defineOptionalProperty<float>(
                "Offset.y", 0.f, "y component of initial robot position (mm)");
            defineOptionalProperty<float>(
                "Offset.z", 0.f, "z component of initial robot position (mm)");
            defineOptionalProperty<float>(
                "Offset.roll", 0.f, "Initial robot pose: roll component of RPY angles (radian)");
            defineOptionalProperty<float>(
                "Offset.pitch", 0.f, "Initial robot pose: pitch component of RPY angles (radian)");
            defineOptionalProperty<float>(
                "Offset.yaw", 0.f, "Initial robot pose: yaw component of RPY angles (radian)");
        }
    };
 
    /**
     * @defgroup Component-TrajectoryPlayer TrajectoryPlayer
     * @ingroup RobotComponents-Components
     * A description of the component TrajectoryPlayer.
     *
     * @class TrajectoryPlayer
     * @ingroup Component-TrajectoryPlayer
     * @brief Brief description of class TrajectoryPlayer.
     *
     * Detailed description of class TrajectoryPlayer.
     */
    class TrajectoryPlayer :
        virtual public armarx::Component,
        public armarx::TrajectoryPlayerInterface
 
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "TrajectoryPlayer";
        }
 
        // interface
        bool startTrajectoryPlayer(const Ice::Current&) override;
        bool pauseTrajectoryPlayer(const Ice::Current&) override;
        bool stopTrajectoryPlayer(const Ice::Current&) override;
        bool resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current&) override;
        void setLoopPlayback(bool loop, const Ice::Current&) override;
        void setIsVelocityControl(bool isVelocity,
                                  const ::Ice::Current& = Ice::emptyCurrent) override;
 
        //        virtual void load(const TrajSource& trajs,
        //                          double start = 0.0,
        //                          double end = 1.0,
        //                          double timestep = 0.01,
        //                          const ::Ice::StringSeq& joints = std::vector<std::string>(),
        //                          const ::Ice::Current& = Ice::emptyCurrent);
 
        //        virtual void load(const TrajectoryBasePtr& trajs,
        //                          double start = 0.0,
        //                          double end = 1.0,
        //                          double timestep = 0.01,
        //                          const ::Ice::StringSeq& joints = std::vector<std::string>(),
        //                          const ::Ice::Current& = Ice::emptyCurrent);
 
        void loadJointTraj(const TrajectoryBasePtr& trajs,
                           const ::Ice::Current& = Ice::emptyCurrent) override;
        void loadBasePoseTraj(const TrajectoryBasePtr& trajs,
                              const ::Ice::Current& = Ice::emptyCurrent) override;
 
        double
        getCurrentTime(const Ice::Current&) override
        {
            return currentTime;
        }
 
        double
        getEndTime(const Ice::Current&) override
        {
            return endTime;
        }
 
        void
        setEndTime(double end, const Ice::Current&) override
        {
            if (endTime == end)
            {
                return;
            }
 
            endTime = end;
            jointTraj = jointTraj->normalize(0, endTime);
            basePoseTraj = basePoseTraj->normalize(0, endTime);
        }
 
        double
        getTrajEndTime(const Ice::Current&) override
        {
            return trajEndTime;
        }
 
        void
        setIsPreview(bool isPreview, const Ice::Current&) override
        {
            this->isPreview = isPreview;
        }
 
        void
        enableRobotPoseUnit(bool isRobotPose, const Ice::Current&) override
        {
            robotPoseUnitEnabled = isRobotPose;
        }
 
        void
        considerConstraints(bool, const Ice::Current&) override
        {
            ARMARX_WARNING << "NYI TrajectoryPlayer::considerConstraints()";
        }
 
        bool setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&) override;
 
        // KinematicUnitListener interface
        void
        reportControlModeChanged(const NameControlModeMap&,
                                 Ice::Long,
                                 bool,
                                 const Ice::Current&) override
        {
        }
 
        void reportJointAngles(const NameValueMap& angles,
                               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& offset, const Ice::Current&) override
        {
            this->offset = offset;
        }
 
        void updateTargetValues();
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
 
        ::armarx::TrajectoryPtr jointTraj;
        ::armarx::TrajectoryPtr basePoseTraj;
 
 
        KinematicUnitInterfacePrx kinematicUnit;
        DebugObserverInterfacePrx debugObserver;
        DebugDrawerInterfacePrx debugDrawer;
 
        std::string armarxProject;
        bool robotPoseUnitEnabled;
 
        Eigen::Matrix4f offset;
 
        PeriodicTask<TrajectoryPlayer>::pointer_type task;
        Ice::StringSeq jointNames;
        std::map<std::string, bool> jointNamesUsed;
        std::map<std::string, bool> limitlessMap;
 
        bool isVelocityControl;
 
        double currentTime;
        double runningTime;
        IceUtil::Time startTime;
        double timeOffset;
        double endTime;
 
        bool loopPlayback;
        bool isPreview;
        std::map<std::string, PIDControllerPtr> PIDs;
        NameValueMap targetPositionValues;
        NameValueMap targetVelocityValues;
        PosePtr targetRobotPose;
        NameValueMap nullVelocities;
        IceUtil::Time motionStartTime;
        //        float start, end;
        int direction;
        NameValueMap jointOffets;
        float maxVel;
 
        std::string modelFileName;
 
 
        void run();
        bool checkJointsLimit();
        bool checkSelfCollision();
 
        VirtualRobot::RobotPtr localModel; // to analyze the availability of the trajectory point.
 
    private:
        bool start();
        bool pause();
        bool stop();
        void updatePIDController(const NameValueMap& angles);
 
        double trajEndTime;
 
        IceUtil::Time startCal;
        double frozenTime;
 
        std::recursive_mutex motionMutex;
        NameValueMap latestJointAngles;
        std::mutex jointAnglesMutex;
 
        bool paused;
        bool firstRound;
        VirtualRobot::RobotNodePtr customRootNode;
    };
 
    using TrajectoryPlayerPtr = ::IceInternal::Handle<::armarx::TrajectoryPlayer>;
 
} // namespace armarx