DSJointCarryController.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    DSController::ArmarXObjects::DSJointCarryController
 * @author     Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#ifndef _ARMARX_LIB_DSController_DSJointCarryController_H
#define _ARMARX_LIB_DSController_DSJointCarryController_H
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
 
#include <armarx/control/ds_controller/DSControllerInterface.h>
 
#include "GMRDynamics.h"
#include "MathLib.h"
 
namespace armarx::control::ds_controller
{
 
    typedef boost::shared_ptr<GMRDynamics> JointCarryGMMPtr;
 
    struct JointCarryGMRParameters
    {
        int K_gmm_;
        int dim_;
        std::vector<double> Priors_;
        std::vector<double> Mu_;
        std::vector<double> Sigma_;
        std::vector<double> attractor_;
        double dt_;
    };
 
    class JointCarryGMMMotionGen
    {
    public:
        JointCarryGMMMotionGen()
        {
        }
 
        JointCarryGMMMotionGen(const std::string& fileName)
        {
            getGMMParamsFromJsonFile(fileName);
        }
 
        JointCarryGMMPtr guard_gmm;
        JointCarryGMRParameters guard_gmmParas;
        Eigen::Vector3f guard_target;
        Eigen::Vector3f guard_desiredVelocity;
 
        float scaling;
        float v_max;
 
        void
        getGMMParamsFromJsonFile(const std::string& fileName)
        {
            std::ifstream infile{fileName};
            std::string objDefs = {std::istreambuf_iterator<char>(infile),
                                   std::istreambuf_iterator<char>()};
            JSONObjectPtr json = new JSONObject();
            json->fromString(objDefs);
            guard_gmmParas.K_gmm_ = json->getInt("K");
            guard_gmmParas.dim_ = json->getInt("Dim");
            json->getArray<double>("Priors", guard_gmmParas.Priors_);
            json->getArray<double>("Mu", guard_gmmParas.Mu_);
            json->getArray<double>("Attractor", guard_gmmParas.attractor_);
            json->getArray<double>("Sigma", guard_gmmParas.Sigma_);
            scaling = json->getDouble("Scaling");
            v_max = json->getDouble("MaxVelocity");
 
            guard_gmm.reset(new GMRDynamics(guard_gmmParas.K_gmm_,
                                            guard_gmmParas.dim_,
                                            guard_gmmParas.dt_,
                                            guard_gmmParas.Priors_,
                                            guard_gmmParas.Mu_,
                                            guard_gmmParas.Sigma_));
            guard_gmm->initGMR(0, 2, 3, 5);
            std::cout << "line 162." << std::endl;
 
 
            for (int i = 0; i < 3; ++i)
            {
                guard_target(i) = guard_gmmParas.attractor_.at(i);
            }
 
            std::cout << "Finished GMM." << std::endl;
        }
 
        void
        updateDesiredVelocity(const Eigen::Vector3f& positionInMeter,
                              float positionErrorToleranceInMeter)
        {
            MathLib::Vector position_error;
            position_error.Resize(3);
 
            MathLib::Vector desired_vel;
            desired_vel.Resize(3);
 
            Eigen::Vector3f positionError = positionInMeter - guard_target;
            if (positionError.norm() < positionErrorToleranceInMeter)
            {
                positionError.setZero();
            }
 
            for (int i = 0; i < 3; ++i)
            {
                position_error(i) = positionError(i);
            }
 
            desired_vel = guard_gmm->getVelocity(position_error);
 
            guard_desiredVelocity << desired_vel(0), desired_vel(1), desired_vel(2);
 
            guard_desiredVelocity = scaling * guard_desiredVelocity;
 
            float lenVec = guard_desiredVelocity.norm();
 
            if (std::isnan(lenVec))
            {
                guard_desiredVelocity.setZero();
            }
 
            if (lenVec > v_max)
            {
                guard_desiredVelocity = (v_max / lenVec) * guard_desiredVelocity;
            }
        }
    };
 
    typedef boost::shared_ptr<JointCarryGMMMotionGen> JointCarryGMMMotionGenPtr;
 
    class DSJointCarryControllerControlData
    {
    public:
        Eigen::Vector3f leftDesiredLinearVelocity;
        Eigen::Vector3f leftDesiredAngularError;
        Eigen::Vector3f rightDesiredLinearVelocity;
        Eigen::Vector3f rightDesiredAngularError;
    };
 
    /**
     * @defgroup Library-DSJointCarryController DSJointCarryController
     * @ingroup armarx_control
     * A description of the library DSJointCarryController.
     *
     * @class DSJointCarryController
     * @ingroup Library-DSJointCarryController
     * @brief Brief description of class DSJointCarryController.
     *
     * Detailed description of class DSJointCarryController.
     */
    class DSJointCarryController :
        public NJointControllerWithTripleBuffer<DSJointCarryControllerControlData>,
        public DSJointCarryControllerInterface
    {
 
        // ManagedIceObject interface
    protected:
        void onInitNJointController();
        void onDisconnectNJointController();
 
 
        void controllerRun();
 
 
        // NJointControllerInterface interface
    public:
        using ConfigPtrT = DSJointCarryControllerConfigPtr;
 
        DSJointCarryController(const RobotUnitPtr& robotUnit,
                               const NJointControllerConfigPtr& config,
                               const VirtualRobot::RobotPtr&);
 
        std::string
        getClassName(const Ice::Current&) const
        {
            return "DSJointCarryController";
        }
 
        // NJointController interface
        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                   const IceUtil::Time& timeSinceLastIteration);
 
        // DSJointCarryControllerInterface interface
    public:
        void setGuardInHandPosition(const Ice::FloatSeq& guardPositionToHandInMeter,
                                    const Ice::Current&);
        void setGuardOrientation(const Ice::FloatSeq& guardOrientationInRobotBase,
                                 const Ice::Current&);
        void setDesiredGuardOri(const Ice::FloatSeq& desiredOrientationInRobotBase,
                                const Ice::Current&);
        void setRotationStiffness(const Ice::FloatSeq& rotationStiffness, const Ice::Current&);
        void setGuardObsAvoidVel(const Ice::FloatSeq& guardVel, const Ice::Current&);
        float getGMMVel(const Ice::Current&);
 
    private:
        mutable MutexType interface2CtrlDataMutex;
 
        float deadzone(float currentValue, float targetValue, float threshold);
        Eigen::Quaternionf
        quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
        JointCarryGMMMotionGenPtr gmmMotionGenerator;
 
        struct DSJointCarryControllerSensorData
        {
            Eigen::Matrix4f left_tcpPose;
            Eigen::Matrix4f right_tcpPose;
            Eigen::Vector3f left_force;
            Eigen::Vector3f right_force;
            double currentTime;
        };
 
        TripleBuffer<DSJointCarryControllerSensorData> controllerSensorData;
 
        struct DSCtrlDebugInfo
        {
            Eigen::Vector3f leftDesiredLinearVelocity;
            Eigen::Vector3f rightDesiredLinearVelocity;
        };
 
        TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
 
        struct Interface2CtrlData
        {
            Eigen::Vector3f guardToHandInMeter;
            Eigen::Quaternionf guardOriInRobotBase;
            Eigen::Quaternionf desiredGuardOriInRobotBase;
            Eigen::Vector3f guardRotationStiffness;
            Eigen::Vector3f guardObsAvoidVel;
        };
 
        TripleBuffer<Interface2CtrlData> interface2CtrlData;
 
        struct DSRTDebugInfo
        {
            StringFloatDictionary desired_torques;
        };
 
        TripleBuffer<DSRTDebugInfo> debugDataInfo;
 
        struct Ctrl2InterfaceData
        {
            float guardZVel;
        };
 
        TripleBuffer<Ctrl2InterfaceData> ctrl2InterfaceData;
 
        std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
        std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
 
        std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
        std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
 
        const SensorValueForceTorque* leftForceTorque;
        const SensorValueForceTorque* rightForceTorque;
 
        std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
        std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
 
        VirtualRobot::RobotNodePtr left_arm_tcp;
        VirtualRobot::RobotNodePtr right_arm_tcp;
 
        VirtualRobot::RobotNodePtr left_sensor_frame;
        VirtualRobot::RobotNodePtr right_sensor_frame;
 
        VirtualRobot::DifferentialIKPtr left_ik;
        VirtualRobot::DifferentialIKPtr right_ik;
 
        Eigen::Quaternionf left_desiredQuaternion;
        Eigen::Quaternionf right_desiredQuaternion;
 
        Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
        Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
        Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
        Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
 
        Eigen::VectorXf left_jointVelocity_filtered;
        Eigen::VectorXf right_jointVelocity_filtered;
 
        Eigen::VectorXf left_desiredTorques_filtered;
        Eigen::VectorXf right_desiredTorques_filtered;
        Eigen::Vector3f left_tcpDesiredTorque_filtered;
        Eigen::Vector3f right_tcpDesiredTorque_filtered;
 
        float smooth_startup;
 
        DSJointCarryControllerConfigPtr cfg;
        float filterTimeConstant;
 
        std::vector<std::string> left_jointNames;
        std::vector<std::string> right_jointNames;
 
        float posiKp;
        float v_max;
        std::vector<float> Damping;
        float oriKp;
        float oriDamping;
        float torqueLimit;
        float null_torqueLimit;
        float nullspaceKp;
        float nullspaceDamping;
        Eigen::VectorXf left_qnullspace;
        Eigen::VectorXf right_qnullspace;
 
 
        Eigen::Quaternionf desired_guardOri;
 
        float positionErrorTolerance;
        bool controllerStopRequested = false;
        bool controllerRunFinished = false;
 
        // NJointController interface
    protected:
        void onPublish(const SensorAndControl&,
                       const DebugDrawerInterfacePrx&,
                       const DebugObserverInterfacePrx&);
 
        // NJointController interface
    protected:
        void rtPreActivateController();
        void rtPostDeactivateController();
    };
 
} // namespace armarx::control::ds_controller
 
#endif