added DSJointCarryController

source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice

@@ -134,4 +134,44 @@ module armarx
 
     };
 
+    class DSJointCarryControllerConfig extends NJointControllerConfig
+    {
+        float posiKp = 5;
+        float v_max = 0.15;
+        Ice::FloatSeq posiDamping;
+        float oriDamping;
+        float oriKp;
+
+        float filterTimeConstant = 0.01;
+        float torqueLimit = 1;
+        float NullTorqueLimit = 0.2;
+
+        Ice::FloatSeq left_desiredQuaternion;
+        Ice::FloatSeq right_desiredQuaternion;
+        Ice::FloatSeq leftarm_qnullspaceVec;
+        Ice::FloatSeq rightarm_qnullspaceVec;
+
+        float nullspaceKp;
+        float nullspaceDamping;
+
+        string gmmParamsFile = "";
+        float positionErrorTolerance;
+        float desiredTorqueDisturbance;
+        float TorqueFilterConstant;
+        float guardLength;
+
+        Ice::FloatSeq defaultGuardOri;
+        float handVelLimit;
+        Ice::FloatSeq defaultRotationStiffness;
+    };
+
+
+    interface DSJointCarryControllerInterface extends NJointControllerInterface
+    {
+        void setGuardPosition(Ice::FloatSeq guardCenterToHandsCenter);
+        void setGuardOrientation(Ice::FloatSeq guardOrientationInRobotBase);
+        void setDesiredGuardOri(Ice::FloatSeq desiredGuardOriInRobotBase);
+        void setRotationStiffness(Ice::FloatSeq guardRotationStiffness);
+    };
+
 };

source/RobotAPI/libraries/DSControllers/CMakeLists.txt

@@ -36,6 +36,7 @@ set(LIBS ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
 
 set(LIB_FILES
 ./DSRTBimanualController.cpp
+./DSJointCarryController.cpp
 ./DSRTController.cpp
 ./GMRDynamics.cpp
 ./Gaussians.cpp
@@ -43,6 +44,7 @@ set(LIB_FILES
 )
 set(LIB_HEADERS
 ./DSRTBimanualController.h
+./DSJointCarryController.h
 ./DSRTController.h
 ./GMRDynamics.h
 ./Gaussians.h

source/RobotAPI/libraries/DSControllers/DSJointCarryController.h

+/*
+ * 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 <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.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 <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Tools/Gravity.h>
+
+#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
+#include "GMRDynamics.h"
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include "MathLib.h"
+//#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+//#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+
+
+namespace armarx
+{
+
+    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 DSController
+        * 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 setGuardPosition(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&);
+
+    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;
+        };
+        TripleBuffer<Interface2CtrlData> interface2CtrlData;
+
+        struct DSRTDebugInfo
+        {
+            StringFloatDictionary desired_torques;
+        };
+        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;
+        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();
+
+
+
+    };
+
+}
+
+#endif