From 37cf60855a0883c6c8bf2ea1b9ac999e64d0b765 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Sun, 2 Aug 2015 20:10:03 +0200
Subject: [PATCH] Seems to work with tasks individually. it depends on VS
tunning. @home @simulation
---
src/quadarm_task_priority_ctrl.cpp | 12 +++++++-----
1 file changed, 7 insertions(+), 5 deletions(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 29a9655..60c76a9 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,10 +287,12 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
MatrixXd Vtotal(4+this->arm_.nj,1);
+ // Task 0 + 1 + 2
+ //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_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();
@@ -425,7 +427,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
// HT r.t. base link
T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
// 3rd column of rotation matrix
- IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).block(0,2,3,1);
+ IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).inverse().block(0,2,3,1);
// Origin of the link's frame r.t. base_link
link_origin.at(jj+1) = T_base_to_joint.at(jj+1).col(3);
// Distance to the middle of the link jj
--
GitLab