Added current robot positions previous code openloop

FindKDL.cmake

@@ -4,7 +4,7 @@
 # KDL_INSTALL where to find include, lib, bin, etc.
 # KDL_FOUND, is set to true
 
-#INCLUDE (${PROJECT_SOURCE_DIR}/config/FindPkgConfig.cmake)
+INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
 
 IF ( CMAKE_PKGCONFIG_EXECUTABLE )
 

src/CMakeLists.txt

@@ -7,11 +7,11 @@ SET(headers quadarm_task_priority_ctrl.h)
 # locate the necessary dependencies
 
 # KDL #########
-INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
+#INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
 #SET( KDL_INSTALL ${CMAKE_INSTALL_PREFIX} CACHE PATH "The KDL installation directory.")
 INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
 #find_package(Orocos-KDL REQUIRED)
-find_package(KDL REQUIRED)
+#find_package(KDL REQUIRED)
 
 # Boost #######
 set(Boost_USE_STATIC_LIBS ON) 

src/quadarm_task_priority_ctrl.cpp

@@ -48,10 +48,10 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
   uam_control();
 
   // Arm positions increment
-  this->arm_.jnt_pos = this->arm_.jnt_pos+(this->uam_.vel.block(6,0,this->arm_.nj,1) * this->ctrl_params_.dt);
+  //this->arm_.jnt_pos = this->arm_.jnt_pos+(this->uam_.vel.block(6,0,this->arm_.nj,1) * this->ctrl_params_.dt);
 
   // Quadrotor positions increment
-  this->quad_.pos = this->quad_.pos+(this->uam_.vel.block(0,0,6,1) * this->ctrl_params_.dt);
+  //this->quad_.pos = this->quad_.pos+(this->uam_.vel.block(0,0,6,1) * this->ctrl_params_.dt);
 
   // return values
   HT = this->T_; // DEBUG. TODO: make it constant and do not use this trans. outside.
@@ -242,6 +242,7 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
   
   // Gain
   double lambdaG = this->ctrl_params_.cog_gain;
+
   // task velocity
   this->ctrl_params_.cog_vel = NIR_VS * JG_pseudo * lambdaG * sigmaG;
 
@@ -295,6 +296,13 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
 
   Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
 
+  // // Debug
+  // std::cout << "Task velocities" << std::endl;
+  // std::cout << this->ctrl_params_.ir_vel.transpose() << std::endl;
+  // std::cout << this->ctrl_params_.vs_vel.transpose() << std::endl;
+  // std::cout << this->ctrl_params_.cog_vel.transpose() << std::endl;
+  // std::cout << this->ctrl_params_.jntlim_vel.transpose() << std::endl;
+
   // Check singular configurations 
   if (isnan(Vtotal(0,0)))
     Vtotal=MatrixXd::Zero(4+this->arm_.nj,1);
@@ -482,7 +490,8 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
   Rib = MatrixXd::Identity(3,3);
 
   // X and Y values of CoG in quadrotor inertial frame
-  MatrixXd cog_arm_in_qi = Rib * this->arm_cog_data_.arm_cog.block(0,0,3,1);
+  this->ctrl_params_.cog_arm = this->arm_cog_data_.arm_cog.block(0,0,3,1);
+  MatrixXd cog_arm_in_qi = Rib * this->ctrl_params_.cog_arm;
   this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
 
   // Partial CoG and jacobian of each link (augmented links: from current to end-effector)
@@ -668,12 +677,19 @@ void CQuadarm_Task_Priority_Ctrl::write_to_file(const string& folder_name, const
   folder << folder_name;
   path+= folder.str();
 
-  // mkdir(path.c_str(), S_IRWXU|S_IRGRP|S_IXGRP);
-  // string command = "mkdir -p ";
-  // stringstream ss;
-  // ss << command << path;
-  // string com = ss.str();
-  // system(com.c_str());
+  // Check if path exists, and create folder if needed
+  struct stat sb;
+  if (stat(path.c_str(), &sb) != 0)
+  {
+    mkdir(path.c_str(), S_IRWXU|S_IRGRP|S_IXGRP);
+    string command = "mkdir -p ";
+    stringstream ss;
+    ss << command << path;
+    string com = ss.str();
+    int ret = system(com.c_str());
+    if (WEXITSTATUS(ret)!=0)
+      std::cout << "Error creating directory." << std::endl;
+  }
 
   path+="/";
 

src/quadarm_task_priority_ctrl.h

@@ -124,10 +124,10 @@ class CQuadarm_Task_Priority_Ctrl
 
     quadrotor quad_; // Quadrotor
 
-    ht T_; // Homogeneous Transforms
-
     ctrl_params ctrl_params_; // Control Law parameters.
 
+    ht T_; // Homogeneous Transforms
+
     MatrixXd cam_vel_; // Camera velocities
 
     double target_dist_; // Euclidean distance to target.
@@ -234,6 +234,8 @@ class CQuadarm_Task_Priority_Ctrl
 
   public:
 
+    //TODO: these variables are public for debugging purposes
+    // they must be private and set get functions created.
     cog_data arm_cog_data_; // Arm CoG data to debug in higher levels.
 
   	/**