From b053713442d815713cef7c5e138ae7cfa4c310c0 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Sun, 2 Aug 2015 14:03:03 +0200
Subject: [PATCH] working with arm joint limits
---
src/quadarm_task_priority_ctrl.cpp | 11 ++++-------
1 file changed, 4 insertions(+), 7 deletions(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 44205c5..29a9655 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -266,8 +266,8 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
double lambdaL = this->ctrl_params_.jntlim_gain;
// task velocity
- //this->ctrl_params_.jntlim_vel = NIR_VS * JL_pseudo * lambdaL * sigmaL; // As secondary
- this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;
+ this->ctrl_params_.jntlim_vel = NIR_VS * JL_pseudo * lambdaL * sigmaL; // As secondary
+ //this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;
// Total velocities _______________________________
@@ -287,12 +287,10 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
MatrixXd Vtotal(4+this->arm_.nj,1);
- // Task 0 + 1
- //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel;
// Task 0 + 1 + 3
- //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel;
+ Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel;
// All tasks
- Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel;
+ //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel;
Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
@@ -484,7 +482,6 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
MatrixXd cog_arm_in_qi = Rib * this->arm_cog_data_.arm_cog.block(0,0,3,1);
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)
MatrixXd CoG_partial;
MatrixXd Jj_cog(3,this->arm_.nj);
--
GitLab