From 9c80938f4abdb12725f1fc3537df337ae3a612f3 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Fri, 21 Aug 2015 19:24:15 +0200
Subject: [PATCH] Added current robot positions previous code openloop
---
FindKDL.cmake | 2 +-
src/CMakeLists.txt | 4 ++--
src/quadarm_task_priority_ctrl.cpp | 34 ++++++++++++++++++++++--------
src/quadarm_task_priority_ctrl.h | 6 ++++--
4 files changed, 32 insertions(+), 14 deletions(-)
diff --git a/FindKDL.cmake b/FindKDL.cmake
index 1a48ac0..6f367b7 100644
--- a/FindKDL.cmake
+++ b/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 )
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 29b40b3..c48ff2d 100644
--- a/src/CMakeLists.txt
+++ b/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)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 82f40c9..0b6bec2 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/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+="/";
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index da96887..5ebb18c 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/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.
/**
--
GitLab