data logging outside of module control

Author: Elise-J

Diff for: dual_arm_control/include/dual_arm_control_iam/DualArmControlSim.hpp

@@ -98,8 +98,6 @@ struct StateMachine {
   Eigen::Vector3f deltaPos;
   float trackingFactor;
   bool adaptationActive;
-
-  bool startlogging;
 };
 
 struct SphericalPosition {
@@ -130,6 +128,72 @@ struct tossingTaskVariables {
   Eigen::Vector4f restOrientation;
 };
 
+struct DataToSave {
+  // Robot
+  Eigen::Vector3f robotX[NB_ROBOTS];
+  Eigen::Vector4f robotQ[NB_ROBOTS];
+  Vector6f robotVelDesEE[NB_ROBOTS];
+  Vector6f robotVelEE[NB_ROBOTS];
+  Eigen::Vector3f robotVDes[NB_ROBOTS];
+  Eigen::Vector3f robotOmegaDes[NB_ROBOTS];
+  Vector6f robotFilteredWrench[NB_ROBOTS];
+  Vector7f robotJointsPositions[NB_ROBOTS];
+  Vector7f robotJointsVelocities[NB_ROBOTS];
+  Vector7f robotJointsAccelerations[NB_ROBOTS];
+  Vector7f robotJointsTorques[NB_ROBOTS];
+
+  // Object
+  Eigen::Matrix4f objectWHGpSpecific[NB_ROBOTS];
+  Eigen::Matrix4f objectWHDo;
+  Eigen::Vector3f objectXo;
+  Eigen::Vector4f objectQo;
+  Eigen::Vector3f objectVo;
+  Eigen::Vector3f objectWo;
+  Vector6f objVelDes;
+
+  // Target
+  Eigen::Vector3f targetXt;
+  Eigen::Vector4f targetQt;
+  Eigen::Vector3f targetVt;
+  Eigen::Vector3f targetXdLanding;
+  Eigen::Vector3f targetXIntercept;
+  Eigen::Vector3f targetXtStateToGo;
+
+  // Tossing
+  tossingTaskVariables tossVar;
+
+  // Task
+  Eigen::Vector3f taskXPlacing;
+  float desiredVelImp;
+  float betaVelMod;
+  Eigen::Vector2f dualPathLenAvgSpeed;
+
+  // Cooperative control
+  Vector6f cooperativeCtrlForceApplied[NB_ROBOTS];
+
+  // Free motion control
+  float freeMotionCtrlActivationProximity;
+  float freeMotionCtrlActivationNormal;
+  float freeMotionCtrlActivationTangent;
+  float freeMotionCtrlActivationRelease;
+  float freeMotionCtrlActivationRetract;
+
+  // DS throwing
+  float dsThrowingActivationProximity;
+  float dsThrowingActivationNormal;
+  float dsThrowingActivationTangent;
+  float dsThrowingActivationToss;
+
+  // State machine
+  bool goHome;
+  bool goToAttractors;
+  bool releaseAndretract;
+  bool isThrowing;
+  bool isPlacing;
+  bool isContact;
+  float desVtoss;
+};
+
 class DualArmControlSim {
 
 private:
@@ -372,4 +436,7 @@ class DualArmControlSim {
   void keyboardReferenceObjectControl();
   StateMachine getStateMachine();
   void updateStateMachine(StateMachine stateMachine);
-};
+
+  // ---- Get data for log
+  DataToSave getDataToSave();
+};

Diff for: dual_arm_control/include/dual_arm_control_iam/tools/PdfGMR.hpp

@@ -27,7 +27,7 @@
 
 #include "eigen3/Eigen/Core"
 
-// #include "dual_arm_control_iam/DataLogging.hpp"
+#include "dual_arm_control_iam/tools/Utils.hpp"
 
 using namespace std;
 class PdfGMR {
@@ -43,7 +43,7 @@ class PdfGMR {
   ~PdfGMR(){};
 
   bool init(std::string fileGMM[]) {
-    // datalog_.loadGMMParam(fileGMM, priorGMMToss_, meanGMMToss_, covMxGMMToss_);
+    Utils<double>::loadGMMParam(fileGMM, priorGMMToss_, meanGMMToss_, covMxGMMToss_);
     return true;
   }
 

Diff for: dual_arm_control/include/dual_arm_control_iam/tools/Utils.hpp

@@ -28,6 +28,7 @@
 #include "Eigen/Eigen"
 #include <float.h>
 #include <fstream>
+#include <iostream>
 
 template<typename T = float>
 class Utils {
@@ -798,6 +799,64 @@ class Utils {
   static Eigen::Matrix<T, 3, 1> getAbs3D(Eigen::Matrix<T, 4, 4> H[2]) {
     return 0.5 * (H[0].block(0, 3, 3, 1) + H[1].block(0, 3, 3, 1));
   }
+
+  // Function to load data from file
+  static bool loadDataFromFile(std::string fileName, Eigen::VectorXf& dataAllVal) {
+    // Eigen::Matrix<T, Eigen::Dynamic, 1> &data_all_val
+    std::ifstream inFile;
+    inFile.open(fileName);
+    if (!inFile) {
+      std::cout << "Unable to open file \n" << std::endl;
+      exit(1);// terminate with error
+    }
+
+    std::vector<float> dataVal;
+    float x;
+    while (inFile >> x) { dataVal.push_back(x); }
+
+    int sizeDataVal = dataVal.size();
+    dataAllVal.resize(sizeDataVal);
+    for (int i = 0; i < sizeDataVal; i++) dataAllVal(i) = dataVal[i];
+
+    return true;
+  }
+
+  static bool
+  loadGMMParam(std::string fileName[], Eigen::VectorXf& priors, Eigen::MatrixXf& means, Eigen::MatrixXf& covars) {
+
+    std::string priorsFileName = fileName[0];
+    std::string meansFileName = fileName[1];
+    std::string covarFileName = fileName[2];
+    Eigen::VectorXf priorsAllVal;
+    Eigen::VectorXf meansAllVal;
+    Eigen::VectorXf covarsAllVal;
+
+    Utils<double>::loadDataFromFile(priorsFileName, priorsAllVal);
+    Utils<double>::loadDataFromFile(meansFileName, meansAllVal);
+    Utils<double>::loadDataFromFile(covarFileName, covarsAllVal);
+
+    // Priors
+    priors = priorsAllVal;
+
+    int nbStates = priorsAllVal.rows();
+    int dataDim = int(meansAllVal.rows() / nbStates);
+
+    // Means
+    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> meansMx(meansAllVal.data(),
+                                                                                              dataDim,
+                                                                                              nbStates);
+    means = meansMx;
+
+    int rowCov = dataDim * nbStates;
+
+    // Covariance
+    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> covarMx(covarsAllVal.data(),
+                                                                                              rowCov,
+                                                                                              dataDim);
+    covars = covarMx;
+
+    return true;
+  }
 };
 
 template class Utils<float>;