Bug in manipulability constraint

[PasMarra]

Here, the improve_manip_dyn_ is set to zero but never goes back to the original value.

    yDebug() << "gain: " << improve_manip_dyn_ ;
    yDebug() << "gain: " << improve_manip_th_;
    if (manip <= improve_manip_th_)
    {
        std::cout << "\n\n\nmanip <= improve_manip_th_ "<< manip << " <= " << improve_manip_th_ <<"\n\n\n";
        improve_manip_dyn_= 0.0;
    }

    G_l.block(2 * joints.size(), 0, 1, joints.size()) = -sampling_time_ * dmdq.transpose();
    h_l(2 * joints.size()) = improve_manip_dyn_* (manip - improve_manip_th_) + dmdq.dot(joints_vel_);

Instead, we should do something like:

    G_l.block(2 * joints.size(), 0, 1, joints.size()) = -sampling_time_ * dmdq.transpose();
    if (manip < improve_manip_th_)
        h_l(2 * joints.size()) = dmdq.dot(joints_vel_);
    else
        h_l(2 * joints.size()) = improve_manip_dyn_* (manip - improve_manip_th_) + dmdq.dot(joints_vel_);

TO DO: Check also the bimanual