From bdea307907605af84fdf89fc9fda37021e161b15 Mon Sep 17 00:00:00 2001
From: asantamaria <somriu@gmail.com>
Date: Sat, 31 Oct 2015 09:43:36 +0100
Subject: [PATCH] added ir_gain. to be tested with real robot
---
src/quadarm_task_priority_ctrl.cpp | 8 ++++++--
src/quadarm_task_priority_ctrl.h | 1 +
2 files changed, 7 insertions(+), 2 deletions(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index f9fa156..47ef9d9 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -210,8 +210,11 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
MatrixXd JIR,JIR_pseudo,sigmaIR;
task_infrad(JIR,JIR_pseudo,sigmaIR);
+ // Gain
+ double lambdaIR = this->ctrl_params_.ir_gain;
+
// task velocity
- this->ctrl_params_.ir_vel = JIR_pseudo * sigmaIR;
+ this->ctrl_params_.ir_vel = JIR_pseudo * lambdaIR * sigmaIR;
// TASK 1: Visual Servo _________________________________________
@@ -323,7 +326,8 @@ void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo
inf_radius(0,0) = this->ctrl_params_.inf_radius;
// Task vector
- sigmaIR = d_obs.transpose()*d_obs-inf_radius;
+ Eigen::MatrixXd euc_d_obs = (d_obs.transpose()*d_obs).array().sqrt();
+ sigmaIR = inf_radius-euc_d_obs;
// Task Jacobian
JIR = MatrixXd::Zero(1,4+this->arm_.nj);
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index 543a7fe..dcf60fa 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -97,6 +97,7 @@ typedef struct{
MatrixXd vs_vel; // Primary task velocity.
double vs_alpha_min; // Alpha value for gain matrix pseudo inverse.
MatrixXd vs_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse W.
+ double ir_gain; // Gain of Inflation radius task.
double inf_radius; // Security distance to an obstacle (Inflation radius) to prevent Collisions.
double cog_gain; // Gain of CoG alignment secondary task.
MatrixXd cog_vel; // Secondary task velocity.
--
GitLab