Commit 14b1437d authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

VS working with IBVS. no secondary tasks. No Weighted Pseudoinverse. No saturations

parent 1dd81f9c
......@@ -291,7 +291,10 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
// Task 0 + 1 + 3
//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;
//TODO
Vtotal = JVS_wpseudo * sigmaVS;
Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
......@@ -360,10 +363,10 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
JVS = Jqa1;
// task jacobian pseudo inverse
//JVS_pseudo = calcPinv(Jqa1);
JVS_pseudo = weight_jacvs_inverse(Jqa1);
JVS_pseudo = calcPinv(Jqa1);
//JVS_pseudo = weight_jacvs_inverse(Jqa1);
//std::cout << "No weighted pseudo inverse: @line:" << __LINE__ << std::endl;
//TODO
// task velocity vector
sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment