TCPControlUnit.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    ArmarX::
* @author     Mirko Waechter (mirko.waechter at kit dot edu)
* @date       2013
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <RobotAPI/interface/units/TCPControlUnit.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
 
#include <memory>
 
namespace armarx
{
    /**
     * \class TCPControlUnitPropertyDefinitions
     * \brief
     */
    class TCPControlUnitPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        TCPControlUnitPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit Proxy");
            defineOptionalProperty<std::string>("RobotStateTopicName", "RobotState", "Name of the RobotComponent State topic.");
            defineOptionalProperty<float>("MaxJointVelocity", 30.f / 180 * 3.141, "Maximal joint velocity in rad/sec");
            defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
            //            defineOptionalProperty<float>("MaximumCommandDelay", 20000, "Delay after which the TCP Control unit releases itself if no new velocity have been set.");
            defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
 
            defineOptionalProperty<float>("LambdaDampedSVD", 0.1f, "Parameter used for smoothing the differential IK near singularities.");
        }
    };
 
    /**
     * \defgroup Component-TCPControlUnit TCPControlUnit
     * \ingroup RobotAPI-SensorActorUnits
     * \brief Unit for controlling a tool center point (TCP).
     *
     * This class implements the interface to control a node of a robot (e.g a TCP)
     * in cartesian coordinates in velocity control mode. It takes velocities in mm/s for
     * translations and rad/s for orientation. Several nodessets can be controlled simultaneously.
     *
     * Before the TCPControlUnit can be used, request() needs to be called. Since a cartesian velocity needs
     * is only correct for the current joint configuration, it needs to recalculated as fast as possible.
     * Thus, after request is called the TCPControlUnit recalculates the velocity in a given cycle time and
     * updates the joint velocities. To set another cycle time use setCycleTime().
     * To set the velocity for a node use setTCPVelocity. Calling setTCPVelocity again with another nodeset
     * will add this nodeset to the list of currently controlled TCPs.
     *
     * \note After usage release() **must** be called to stop the recalcuation and setting of joint velocities.
     */
 
    /**
     * @ingroup Component-TCPControlUnit
     * @brief The TCPControlUnit class
     */
    class TCPControlUnit :
        virtual public Component,
        virtual public TCPControlUnitInterface
    {
    public:
        TCPControlUnit();
 
        // TCPControlUnitInterface interface
 
        /**
         * \brief Sets the cycle time with which the joint velocities are recalculated.
         * \param milliseconds New cycle time.
         * \param c Ice Context, leave blank.
         */
        void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * \brief Sets the cartesian velocity of a node in a nodeset for translation and/or orientation.
         * It is best to provide the data in global coordinates. Otherwise the coordinates frame transformation is done in place
         * on the current robot state, which might not be the same as when the command was given. Additionally, execution inaccurracies
         * might propagate if local coordinate frames are used.
         * \param nodeSetName Nodeset that should be used for moving the node, i.e. tcp
         * \param tcpName Name of the VirtualRobot node that should be moved
         * \param translationVelocity Target cartesian translation velocity in mm/s, but might not be reached. If NULL the translation is ommitted in the calculation.
         * Thus the translation behaviour is undefined und the node/tcp position might change.
         * \param orientationVelocityRPY Target cartesian orientation velocity in rad/s in roll-pitch-yaw, but might not be reached. If NULL the orientation is ommitted in the calculation.
         * Thus the orientation behaviour is undefined und the node/tcp orientation might change.
         * \param c Ice Context, leave blank.
         *
         * \see request(), release()
         */
        void setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const::armarx::FramedDirectionBasePtr& translationVelocity, const::armarx::FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c = Ice::emptyCurrent) override;
 
        // UnitExecutionManagementInterface interface
        /**
         * \brief Does not do anything at the moment.
         * \param c
         */
        void init(const Ice::Current& c = Ice::emptyCurrent) override;
        /**
         * \brief Does not do anything at the moment.
         * \param c
         */
        void start(const Ice::Current& c = Ice::emptyCurrent) override;
        /**
         * \brief Does not do anything at the moment.
         * \param c
         */
        void stop(const Ice::Current& c = Ice::emptyCurrent) override;
        UnitExecutionState getExecutionState(const Ice::Current& c = Ice::emptyCurrent) override;
 
        // UnitResourceManagementInterface interface
        /**
         * \brief Triggers the calculation loop for using cartesian velocity. Call once before/after setting a tcp velocity with SetTCPVelocity.
         * \param c Ice Context, leave blank.
         */
        void request(const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * \brief Releases and stops the recalculation and updating of joint velocities.
         * Call always when finished with cartesian control. The target velocity values of
         * all node set will be deleted in this function.
         * \param c Ice Context, leave blank.
         */
        void release(const Ice::Current& c = Ice::emptyCurrent) override;
 
        bool isRequested(const Ice::Current& c = Ice::emptyCurrent) override;
 
    protected:
 
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
        std::string getDefaultName() const override
        {
            return "TCPControlUnit";
        }
 
        // PropertyUser interface
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        static Eigen::VectorXf CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse);
        static Eigen::VectorXf CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse, Eigen::VectorXf desiredJointValues = Eigen::VectorXf());
        void calcAndSetVelocities();
    private:
        static void ContinuousAngles(const Eigen::AngleAxisf& oldAngle, Eigen::AngleAxisf& newAngle, double& offset);
        void periodicExec();
        void periodicReport();
        Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, VirtualRobot::RobotNodePtr refFrame, VirtualRobot::IKSolver::CartesianSelection mode);
        Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity, float maxJointVelocity);
 
        float maxJointVelocity;
        int cycleTime;
        Eigen::Matrix4f lastTCPPose;
 
        struct TCPVelocityData
        {
            FramedDirectionPtr translationVelocity;
            FramedDirectionPtr orientationVelocity;
            std::string nodeSetName;
            std::string tcpName;
        };
 
        using TCPVelocityDataMap = std::map<std::string, TCPVelocityData>;
        TCPVelocityDataMap tcpVelocitiesMap, localTcpVelocitiesMap;
 
        using DIKMap = std::map<std::string, VirtualRobot::DifferentialIKPtr>;
        DIKMap dIKMap, localDIKMap;
 
 
        PeriodicTask<TCPControlUnit>::pointer_type execTask;
        PeriodicTask<TCPControlUnit>::pointer_type topicTask;
        std::string robotName;
        RobotStateComponentInterfacePrx robotStateComponentPrx;
        KinematicUnitInterfacePrx kinematicUnitPrx;
        VirtualRobot::RobotPtr jointExistenceCheckRobot;
        VirtualRobot::RobotPtr localRobot;
        VirtualRobot::RobotPtr localReportRobot;
        VirtualRobot::RobotPtr localVelReportRobot;
        TCPControlUnitListenerPrx listener;
 
        DebugObserverInterfacePrx debugObs;
 
        bool requested;
        std::map<std::string, double> oriOffsets;
 
        std::vector<VirtualRobot::RobotNodePtr > nodesToReport;
        FramedPoseBaseMap lastTCPPoses;
        IceUtil::Time lastTopicReportTime;
 
        std::mutex dataMutex;
        std::mutex taskMutex;
        std::mutex reportMutex;
        bool calculationRunning;
        double syncTimeDelta;
 
        std::string topicName;
 
 
 
        // KinematicUnitListener interface
    protected:
        void reportControlModeChanged(const NameControlModeMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
        void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool, const Ice::Current&) override;
        void reportJointVelocities(const NameValueMap& jointVel, Ice::Long timestamp, bool, const Ice::Current&) override;
        void reportJointTorques(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
        void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
        void reportJointCurrents(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
        void reportJointMotorTemperatures(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
        void reportJointStatuses(const NameStatusMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
 
    };
 
    class EDifferentialIK : public VirtualRobot::DifferentialIK, virtual public Logging
    {
    public:
        typedef Eigen::VectorXf(EDifferentialIK::*ComputeFunction)(float);
 
        EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot:: RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
 
        VirtualRobot::RobotNodePtr getRefFrame()
        {
            return coordSystem;
        }
        int getJacobianRows()
        {
            return nRows;
        }
 
        Eigen::MatrixXf calcFullJacobian();
 
        void clearGoals();
        void setRefFrame(VirtualRobot::RobotNodePtr coordSystem);
 
        void setGoal(const Eigen::Matrix4f& goal, VirtualRobot::RobotNodePtr tcp, VirtualRobot::IKSolver::CartesianSelection mode, float tolerancePosition, float toleranceRotation, Eigen::VectorXf tcpWeight);
 
        void setDOFWeights(Eigen::VectorXf dofWeights);
        //        void setTCPWeights(Eigen::VectorXf tcpWeights);
        bool computeSteps(float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50) override;
        bool computeSteps(Eigen::VectorXf& resultJointDelta, float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50, ComputeFunction computeFunction = &DifferentialIK::computeStep);
        Eigen::VectorXf computeStep(float stepSize) override;
        void applyDOFWeightsToJacobian(Eigen::MatrixXf& Jacobian);
        void applyTCPWeights(VirtualRobot::RobotNodePtr tcp, Eigen::MatrixXf& partJacobian);
        void applyTCPWeights(Eigen::MatrixXf& invJacobian);
        float getWeightedError();
        float getWeightedErrorPosition(VirtualRobot::SceneObjectPtr tcp);
        Eigen::VectorXf computeStepIndependently(float stepSize);
        bool solveIK(float stepSize = 1, float minChange = 0.01f, int maxSteps = 50, bool useAlternativeOnFail = false);
    protected:
        bool adjustDOFWeightsToJointLimits(const Eigen::VectorXf& plannedJointDeltas);
 
        Eigen::VectorXf dofWeights;
        std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf> tcpWeights;
        Eigen::VectorXf tcpWeightVec;
    };
    using EDifferentialIKPtr = std::shared_ptr<EDifferentialIK>;
 
}