Bug: a task starting disasble may stay disabled even after setting the activation state

[lucarossini-iit]

Currently, the activation state works setting to zero the _A matrix and storing the old value in _A_last_active. The algorithm supposes that the user computes the _A matrix at each task->update(), which is then given to _A_last_active, and then set to zero:

OpenSoT/include/OpenSoT/Task.h

Lines 380 to 388 in 4329964

	for(typename std::list< ConstraintPtr >::iterator i = this->getConstraints().begin();
	i != this->getConstraints().end(); ++i) (*i)->update(x);
	this->_update(x);
	if(!_is_active){
	_A_last_active = _A;
	_A.setZero(_A.rows(), _A.cols());
	return;
	}

Problems may arise if the task start non active and the user forgets to compute the _A matrix in the update function, as it is happening in the CentauroAnkleSteering task:

OpenSoT/src/tasks/velocity/CentauroAnkleSteering.cpp

Lines 371 to 388 in 4329964

	void CentauroAnkleSteering::_update(const Eigen::VectorXd& x)
	{
	// get robot state
	_model->getJointPosition(_q);
	// compute wheel desired velocity
	Eigen::Vector6d wheel_vel;
	_model->getVelocityTwist(_steering.getWheelName(), wheel_vel);
	double q_steering = _steering.computeSteeringAngle(wheel_vel.head<3>());
	double q_current = _q(_steering_dof_idx);
	double dq = _lambda*(q_steering - q_current);
	dq = std::min(std::max(dq, -_max_steering_dq), _max_steering_dq);
	_b(0) = dq;
	}

In this case, the _A matrix keeps being a zero matrix even after setting the activation state as Enabled. We should fix this behaviour to avoid future similar bugs. @alaurenzi @EnricoMingo

[EnricoMingo]

Yes, this is a good point. Indeed as the framework works right now is not easy to force the computation of the A matrix (we could add the update() in the base class but we should force the call of the update() of the derived one, hmmm...). Another point is that the _A_last_active is used as an approximation of the real A matrix, in fact, in the documentation is written that after the reactivation of a task it should be always called the update() of the task in order to recompute correctly the A matrix:

OpenSoT/include/OpenSoT/Task.h

Lines 228 to 240 in 4329964

	/**
	* @brief Activated / deactivates the task by setting the A matrix to zero.
	* Important note: after activating a task, call the update() function in
	* order to recompute a proper A matrix.
	*/
	void setActive(const bool active_flag){
	if(!_is_active && active_flag && _A_last_active.rows() > 0){
	_A = _A_last_active;
	}
	_is_active = active_flag;
	}

However should be nice to find a solution to this issue, only one thing: since it is a core aspect, I would suggest to open a dedicated branch.

[lucarossini-iit]

The problem is not whether we call update or not before the setActivatioState() because it is done at line 384 of Task.h

OpenSoT/include/OpenSoT/Task.h

Lines 380 to 388 in 4329964

	for(typename std::list< ConstraintPtr >::iterator i = this->getConstraints().begin();
	i != this->getConstraints().end(); ++i) (*i)->update(x);
	this->_update(x);
	if(!_is_active){
	_A_last_active = _A;
	_A.setZero(_A.rows(), _A.cols());
	return;
	}

I think the main problem is that the user is forced to calculate the _A matrix in each task->update(). We need to write this in the documentation or force the user somehow to add the matrix calculation.

[EnricoMingo]

I know, but when you reactivate the task, the _A_last_active is assigned to the _A. However, if you call update() AFTER the reactivation, the _A will be updated again with the correct actual values.
Despite this, I think we should correct the documentation ensuring that in the constructor all the task related quantities (the _A and _W matrices, and the _b vector) are initialized. Furthermore, we could use the checkConsistency() method in Task.h to check if the task has been correctly initialized or not.