constraints in cpp

[marvin-ad-martianum]

constraints i currently implement like this , ex:

auto constraint_aligned = [this](const Eigen::VectorXd& u) -> Eigen::VectorXd {
    Eigen::VectorXd constrained_u = u;
    //std::cout << "u size " << u.size() << std::endl;
    //std::cout << "constrained " << constrained_u << std::endl;
    for (int i = 0; i < u.size(); ++i) {
        // Apply constraints to each row
        if(i < u.size()/2)
            constrained_u(i) = std::min(std::max(u(i), min_value_rpm), max_value_rpm);
        else
            constrained_u(i) = std::min(std::max(u(i), min_value_vel), max_value_vel);
    }
    return constrained_u;
};

problem is, this gets successively harder with multiple constraints. more consistent notation with the input would be nice.

another problem is that most systems have some inputs which are not controllable, hence they will need to be entered as fully constrained inputs. here it would be useful if i could give specific dimensions of the constraints as disturbances, meaning i give a trajecotry of inputs u, tagging them as disturbance, which then automatically limits them in a bound eps << 1 along this trajecotory

[dominichofer]

I added clamp(VectorXd, VectorXd, VectorXd) -> VectorXd with #30. I hope it allows for a more consistent notation with multi-dimensional constraints. Does it? The second part, I don't understand. It puzzles me why you'd put inputs in a controller that are not controllable, that sounds paradoxical. Can you elaborate?

[marvin-ad-martianum]

Yes, it allows for more consistent notation, but the best thing i could come up with is this

auto constraint_inputs = [this](const Eigen::VectorXd& u, const Eigen::VectorXd& min_value_vel, const Eigen::VectorXd& max_value_vel) -> Eigen::VectorXd {
    Eigen::VectorXd low(u.size());
    Eigen::VectorXd high(u.size());
    int half_size = u.size() / 2;

    // Set the constant constraints for the first half
    low.head(half_size).setConstant(min_value_rpm);
    high.head(half_size).setConstant(max_value_rpm);

    // Use vector constraints for the second half
    low.tail(u.size() - half_size) = min_value_vel;
    high.tail(u.size() - half_size) = max_value_vel;

    // Apply the clamp function
    return clamp(u, low, high);
};

in this example i do not change rpm min and max but will continuously change the velocity trajectories.

i still have the problem of dealing with index stuff such as: int half_size = u.size() / 2; It just seems like an unnecessarily complicated way of doing this, do you have a more elegant solution?

About the second part. Each linear model has inputs and outputs. In the best case the input is one i control. However, in the case of several inputs, with one or more, that I do not control (in German Störgrösse), I have a problem. I will need to add this to the model for the optimizer to consider it, but still tell the controller that it is uncontrollable. Hence I add this input as a fully constrained input (sometimes with a small eps parameter of wiggle room for numerical stability). Since in some cases i know the future trajectory of this input i will add this fully constrained future input trajectory to my problem. For example: I control the rotation of a motor in an end-effector mounted on a robot arm moving closer to an object. Assuming I dont control the movement of the arm with this algorithm, I might still need this information to optimize the end-effector control.

[dominichofer]

Maybe vector concatenation makes it more compact.

int size_a, size_b;
double low_a, low_b;
double high_a, high_b;
Eigen::VectorXd low(size_a + size_b);
Eigen::VectorXd high(size_a + size_b);

low << Eigen::VectorXd::Constant(size_a, low_a), Eigen::VectorXd::Constant(size_b, low_b);
high << Eigen::VectorXd::Constant(size_a, high_a), Eigen::VectorXd::Constant(size_b, high_b);

auto constraint_inputs = [this](const Eigen::VectorXd& u) { return clamp(u, low, high); }

[dominichofer]

To constrain a value relatively to itself, you could write your own helper functions

Eigen::VectorXd clamp_with_absolute_slack(const Eigen::VectorXd& value, double slack) {
    auto delta = Eigen::VectorXd::Constant(u.size(), slack);
    return clamp(u, u - delta, u + delta);
}

Eigen::VectorXd clamp_with_relative_slack(const Eigen::VectorXd& value, double slack) {
    auto low = Eigen::VectorXd::Constant(u.size(), 1 - slack);
    auto high = Eigen::VectorXd::Constant(u.size(), 1 + slack);
    return clamp(u, u * low, u * high);
}

If your slack should be only 1 representable value, maybe have a look at https://en.cppreference.com/w/cpp/numeric/math/nextafter.

[marvin-ad-martianum]

Hmm. Looks good. absolute slack seems the stable one around zero. I would do that. Now, to continuously update lets say values of b i would just edit this part of the vector in every loop? Something like this?

double absolute_slack = 0.01;

auto constraint_inputs = [this, size_a, size_b, low_a, high_a, relative_slack](const Eigen::VectorXd& u) {

    Eigen::VectorXd constrained = Eigen::VectorXd::Zero(u.size());
    constrained.head(size_a) = clamp(u.head(size_a), Eigen::VectorXd::Constant(size_a, low_a), Eigen::VectorXd::Constant(size_a, high_a));

 constrained.tail(size_b) = clamp_with_absolute_slack(u.tail(size_b), absolute_slack);

    return constrained;
};