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 <memory>
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
 
#include <RobotAPI/interface/units/TCPControlUnit.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
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>;
 
} // namespace armarx