DSRTBimanualController.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::DSRTBimanualController
 * @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_DSRTBimanualController_H
#define _ARMARX_LIB_DSController_DSRTBimanualController_H
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Tools/Gravity.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.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
{
    class DSRTBimanualControllerControlData
    {
    public:
        Eigen::Vector3f left_tcpDesiredLinearVelocity;
        Eigen::Vector3f left_tcpDesiredAngularError;
 
        Eigen::Vector3f right_tcpDesiredLinearVelocity;
        Eigen::Vector3f right_tcpDesiredAngularError;
 
        Eigen::Vector3f left_right_distanceInMeter;
 
    };
 
    typedef boost::shared_ptr<GMRDynamics> BimanualGMMPtr;
 
    struct BimanualGMRParameters
    {
        int K_gmm_;
        int dim_;
        std::vector<double> Priors_;
        std::vector<double> Mu_;
        std::vector<double> Sigma_;
        std::vector<double> attractor_;
        double dt_;
    };
 
 
    class BimanualGMMMotionGen
    {
    public:
        BimanualGMMMotionGen() {}
 
        BimanualGMMMotionGen(const std::string& fileName)
        {
            getGMMParamsFromJsonFile(fileName);
        }
 
        BimanualGMMPtr  leftarm_gmm;
        BimanualGMMPtr  rightarm_gmm;
 
        BimanualGMRParameters leftarm_gmmParas;
        BimanualGMRParameters rightarm_gmmParas;
 
        Eigen::Vector3f leftarm_Target;
        Eigen::Vector3f rightarm_Target;
 
        Eigen::Vector3f left_DS_DesiredVelocity;
        Eigen::Vector3f right_DS_DesiredVelocity;
        Eigen::Vector3f left_right_position_errorInMeter;
 
 
 
        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>() };
            //            StructuralJsonParser jsonParser(objDefs);
            //            jsonParser.parse();
            //            JPathNavigator jpnav(jsonParser.parsedJson);
            //            float k = jpnav.selectSingleNode("left/K").asFloat();
            //            ARMARX_INFO << "k: " << k ;
            //            jpnav.selectSingleNode("left")
            JSONObjectPtr json = new JSONObject();
            json->fromString(objDefs);
 
            //            leftarm_gmmParas.K_gmm_ = AbstractObjectSerializerPtr::dynamicCast<JSONObjectPtr>(json->getElement("left"))->getInt("K");
            //            leftarm_gmmParas.dim_ = json->getElement("left")->getInt("dim");
            //            boost::dynamic_pointer_cast<JSONObjectPtr>(json->getElement("left"))->getArray<double>("Priors", leftarm_gmmParas.Priors_);
 
            //            json->getElement("left")->getArray<double>("Mu", leftarm_gmmParas.Mu_);
            //            json->getElement("left")->getArray<double>("attractor", leftarm_gmmParas.attractor_);
            //            json->getElement("left")->getArray<double>("Sigma", leftarm_gmmParas.Sigma_);
 
            //            rightarm_gmmParas.K_gmm_ = json->getElement("right")->getInt("K");
            //            rightarm_gmmParas.dim_ = json->getElement("right")->getInt("dim");
            //            json->getElement("right")->getArray<double>("Priors", rightarm_gmmParas.Priors_);
            //            json->getElement("right")->getArray<double>("Mu", rightarm_gmmParas.Mu_);
            //            json->getElement("right")->getArray<double>("attractor", rightarm_gmmParas.attractor_);
            //            json->getElement("right")->getArray<double>("Sigma", rightarm_gmmParas.Sigma_);
 
 
            leftarm_gmmParas.K_gmm_ = json->getInt("leftK");
            leftarm_gmmParas.dim_ = json->getInt("leftDim");
            json->getArray<double>("leftPriors", leftarm_gmmParas.Priors_);
            json->getArray<double>("leftMu", leftarm_gmmParas.Mu_);
            json->getArray<double>("leftAttractor", leftarm_gmmParas.attractor_);
            json->getArray<double>("leftSigma", leftarm_gmmParas.Sigma_);
 
 
            rightarm_gmmParas.K_gmm_ = json->getInt("rightK");
            rightarm_gmmParas.dim_ = json->getInt("rightDim");
            json->getArray<double>("rightPriors", rightarm_gmmParas.Priors_);
            json->getArray<double>("rightMu", rightarm_gmmParas.Mu_);
            json->getArray<double>("rightAttractor", rightarm_gmmParas.attractor_);
            json->getArray<double>("rightSigma", rightarm_gmmParas.Sigma_);
 
 
            scaling = json->getDouble("Scaling");
            v_max = json->getDouble("MaxVelocity");
 
            leftarm_gmm.reset(new GMRDynamics(leftarm_gmmParas.K_gmm_, leftarm_gmmParas.dim_, leftarm_gmmParas.dt_, leftarm_gmmParas.Priors_, leftarm_gmmParas.Mu_, leftarm_gmmParas.Sigma_));
            leftarm_gmm->initGMR(0, 2, 3, 5);
 
            rightarm_gmm.reset(new GMRDynamics(rightarm_gmmParas.K_gmm_, rightarm_gmmParas.dim_, rightarm_gmmParas.dt_, rightarm_gmmParas.Priors_, rightarm_gmmParas.Mu_, rightarm_gmmParas.Sigma_));
            rightarm_gmm->initGMR(0, 2, 3, 5);
 
 
            //            if (leftarm_gmmParas.attractor_.size() < 3)
            //            {
            //                ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
            //            }
 
            //            if (rightarm_gmmParas.attractor_.size() < 3)
            //            {
            //                ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
            //            }
 
            std::cout << "line 162." << std::endl;
 
 
            for (int i = 0; i < 3; ++i)
            {
                leftarm_Target(i) = leftarm_gmmParas.attractor_.at(i);
                rightarm_Target(i) = rightarm_gmmParas.attractor_.at(i);
            }
 
            std::cout << "Finished GMM." << std::endl;
 
        }
 
 
        void updateDesiredVelocity(
            const Eigen::Vector3f& leftarm_PositionInMeter,
            const Eigen::Vector3f& rightarm_PositionInMeter,
            float positionErrorToleranceInMeter,
            float desiredZ,
            float correction_x,
            float correction_y,
            float correction_z)
        {
            MathLib::Vector position_error;
            position_error.Resize(3);
 
            MathLib::Vector desired_vel;
            desired_vel.Resize(3);
 
 
 
            Eigen::Vector3f leftarm_Target_corrected = leftarm_Target;
            Eigen::Vector3f rightarm_Target_corrected = rightarm_Target;
 
            leftarm_Target_corrected(0) += correction_x;
            rightarm_Target_corrected(0) += correction_x;
 
            leftarm_Target_corrected(1) += correction_y;
            rightarm_Target_corrected(1) += correction_y;
 
            leftarm_Target_corrected(2)  = desiredZ + correction_z;
            rightarm_Target_corrected(2) = desiredZ +  correction_z;
 
 
 
            // for the left arm
            Eigen::Vector3f leftarm_PositionError = leftarm_PositionInMeter - leftarm_Target_corrected;
            if (leftarm_PositionError.norm() < positionErrorToleranceInMeter)
            {
                leftarm_PositionError.setZero();
            }
 
            for (int i = 0; i < 3; ++i)
            {
                position_error(i) = leftarm_PositionError(i);
            }
 
            desired_vel = leftarm_gmm->getVelocity(position_error);
 
            Eigen::Vector3f leftarm_desired_vel;
            leftarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
 
            leftarm_desired_vel = scaling * leftarm_desired_vel;
 
            float lenVec = leftarm_desired_vel.norm();
 
            if (std::isnan(lenVec))
            {
                leftarm_desired_vel.setZero();
            }
 
            if (desiredZ < 1.5)
            {
                v_max = 0.2;
            }
            else
            {
                v_max = 0.5;
            }
 
            if (lenVec > v_max)
            {
                leftarm_desired_vel = (v_max / lenVec) * leftarm_desired_vel;
            }
 
            left_DS_DesiredVelocity = leftarm_desired_vel;
 
 
            // for the right arm
            Eigen::Vector3f rightarm_PositionError = rightarm_PositionInMeter - rightarm_Target_corrected;
            if (rightarm_PositionError.norm() < positionErrorToleranceInMeter)
            {
                rightarm_PositionError.setZero();
            }
 
            for (int i = 0; i < 3; ++i)
            {
                position_error(i) = rightarm_PositionError(i);
            }
 
            desired_vel = rightarm_gmm->getVelocity(position_error);
 
            Eigen::Vector3f rightarm_desired_vel;
            rightarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
 
            rightarm_desired_vel = scaling * rightarm_desired_vel;
 
            lenVec = rightarm_desired_vel.norm();
            if (std::isnan(lenVec))
            {
                rightarm_desired_vel.setZero();
            }
 
            if (lenVec > v_max)
            {
                rightarm_desired_vel = (v_max / lenVec) * rightarm_desired_vel;
            }
 
            right_DS_DesiredVelocity = rightarm_desired_vel;
 
            left_right_position_errorInMeter = leftarm_PositionError - rightarm_PositionError;
 
        }
 
 
 
    };
 
    typedef boost::shared_ptr<BimanualGMMMotionGen> BimanualGMMMotionGenPtr;
 
 
 
 
    /**
     * @defgroup Library-DSRTBimanualController DSRTBimanualController
     * @ingroup armarx_control
     * A description of the library DSRTBimanualController.
     *
     * @class DSRTBimanualController
     * @ingroup Library-DSRTBimanualController
     * @brief Brief description of class DSRTBimanualController.
     *
     * Detailed description of class DSRTBimanualController.
     */
    class DSRTBimanualController : public NJointControllerWithTripleBuffer<DSRTBimanualControllerControlData>, public DSBimanualControllerInterface
    {
 
        // ManagedIceObject interface
    protected:
        void onInitNJointController();
        void onDisconnectNJointController();
 
 
        void controllerRun();
 
 
 
        // NJointControllerInterface interface
    public:
        using ConfigPtrT = DSRTBimanualControllerConfigPtr;
 
        DSRTBimanualController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
 
 
        std::string getClassName(const Ice::Current&) const
        {
            return "DSRTBimanualController";
        }
 
        // NJointController interface
        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
 
        void setToDefaultTarget(const Ice::Current&);
    private:
 
        float deadzone(float currentValue, float targetValue, float threshold);
        Eigen::Quaternionf quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
 
        //        PeriodicTask<DSRTBimanualController>::pointer_type controllerTask;
        BimanualGMMMotionGenPtr gmmMotionGenerator;
        struct DSRTBimanualControllerSensorData
        {
            Eigen::Matrix4f left_tcpPose;
            Eigen::Matrix4f right_tcpPose;
 
            //            Eigen::Vector3f left_linearVelocity;
            //            Eigen::Vector3f right_linearVelocity;
 
            Eigen::Vector3f left_force;
            Eigen::Vector3f right_force;
 
 
            double currentTime;
 
 
        };
        TripleBuffer<DSRTBimanualControllerSensorData> controllerSensorData;
 
        struct DSCtrlDebugInfo
        {
            float desredZ;
            float force_error_z;
            float guardXYHighFrequency;
            float guard_mounting_correction_x;
            float guard_mounting_correction_y;
            float guard_mounting_correction_z;
        };
        TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
 
        struct DSRTDebugInfo
        {
            StringFloatDictionary desired_torques;
            float desiredForce_x;
            float desiredForce_y;
            float desiredForce_z;
            float tcpDesiredTorque_x;
            float tcpDesiredTorque_y;
            float tcpDesiredTorque_z;
 
            float tcpDesiredAngularError_x;
            float tcpDesiredAngularError_y;
            float tcpDesiredAngularError_z;
 
            float currentTCPAngularVelocity_x;
            float currentTCPAngularVelocity_y;
            float currentTCPAngularVelocity_z;
 
            float currentTCPLinearVelocity_x;
            float currentTCPLinearVelocity_y;
            float currentTCPLinearVelocity_z;
 
            // force torque sensor in root
            float left_realForce_x;
            float left_realForce_y;
            float left_realForce_z;
            float right_realForce_x;
            float right_realForce_y;
            float right_realForce_z;
            float left_force_error;
            float right_force_error;
 
            float left_tcpDesiredTorque_x;
            float left_tcpDesiredTorque_y;
            float left_tcpDesiredTorque_z;
            float right_tcpDesiredTorque_x;
            float right_tcpDesiredTorque_y;
            float right_tcpDesiredTorque_z;
 
        };
        TripleBuffer<DSRTDebugInfo> debugDataInfo;
 
 
        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;
        Eigen::MatrixXf left_jacobip;
        Eigen::MatrixXf left_jacobio;
 
        VirtualRobot::DifferentialIKPtr right_ik;
        Eigen::MatrixXf right_jacobip;
        Eigen::MatrixXf right_jacobio;
 
        Eigen::Quaternionf left_desiredQuaternion;
        Eigen::Vector3f left_oldPosition;
        Eigen::Matrix3f left_oldOrientation;
 
        Eigen::Quaternionf right_desiredQuaternion;
        Eigen::Vector3f right_oldPosition;
        Eigen::Matrix3f right_oldOrientation;
 
        Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
        Eigen::Vector3f left_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;
 
        Eigen::Vector3f leftForceOffset;
        Eigen::Vector3f rightForceOffset;
 
        float left_contactForceZ_correction;
        float right_contactForceZ_correction;
 
        float smooth_startup;
 
        DSRTBimanualControllerConfigPtr cfg;
 
        Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
        Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
 
        float filterTimeConstant;
 
        std::vector<std::string> left_jointNames;
        std::vector<std::string> right_jointNames;
 
        float posiKp;
        float v_max;
        std::vector<float> Damping;
        float torqueLimit;
        float null_torqueLimit;
 
        float Coupling_stiffness;
        float Coupling_force_limit;
 
        float forward_gain;
 
        float oriKp;
        float oriDamping;
 
        float nullspaceKp;
        float nullspaceDamping;
 
        float contactForce;
 
        float guardTargetZUp;
        float guardTargetZDown;
        float guardDesiredZ;
        float guard_mounting_correction_z;
 
        float guardXYHighFrequency;
        Eigen::Vector3f left_force_old;
        Eigen::Vector3f right_force_old;
 
        Eigen::VectorXf left_qnullspace;
        Eigen::VectorXf right_qnullspace;
 
        Eigen::Quaternionf left_oriUp;
        Eigen::Quaternionf left_oriDown;
        Eigen::Quaternionf right_oriUp;
        Eigen::Quaternionf right_oriDown;
 
        //        std::vector<BimanualGMMMotionGenPtr> BimanualGMMMotionGenList;
 
 
        float forceFilterCoeff;
 
        float positionErrorTolerance;
        bool controllerStopRequested = false;
        bool controllerRunFinished = false;
 
        bool isDefaultTarget = true;
 
        /// intermediate variables
        Eigen::Vector6f left_tcptwist;
        Eigen::Vector6f right_tcptwist;
 
        Eigen::Matrix4f left_currentTCPPose;
        Eigen::Matrix4f right_currentTCPPose;
 
        unsigned int nJointLeft = 0;
        unsigned int nJointRight = 0;
 
        Eigen::MatrixXf left_jacobi;
        Eigen::MatrixXf right_jacobi;
 
        Eigen::VectorXf left_qpos;
        Eigen::VectorXf left_qvel;
 
        Eigen::VectorXf right_qpos;
        Eigen::VectorXf right_qvel;
 
        Eigen::Vector3f left_forceInRoot;
        Eigen::Vector3f right_forceInRoot;
 
        double deltaT = 0.0;
 
        Eigen::Vector3f left_DS_force;
        Eigen::Vector3f right_DS_force;
        Eigen::Vector3f coupling_force;
 
        Eigen::Vector3f left_tcpDesiredForce;
        Eigen::Vector3f right_tcpDesiredForce;
        Eigen::Vector3f left_tcpDesiredTorque;
        Eigen::Vector3f right_tcpDesiredTorque;
 
        Eigen::VectorXf left_nullspaceTorque;
        Eigen::VectorXf right_nullspaceTorque;
 
        Eigen::MatrixXf I;
 
        Eigen::Vector6f left_tcpDesiredWrench;
        Eigen::Vector6f right_tcpDesiredWrench;
 
        Eigen::VectorXf left_jointDesiredTorques;
        Eigen::VectorXf right_jointDesiredTorques;
 
        Eigen::MatrixXf left_jtpinv;
        Eigen::MatrixXf right_jtpinv;
 
        Eigen::VectorXf left_nullqerror;
        Eigen::VectorXf right_nullqerror;
 
        // NJointController interface
    protected:
        void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        // NJointController interface
    protected:
        void rtPreActivateController();
        void rtPostDeactivateController();
    };
 
}
 
#endif