The slack variable becomes negative.

[sm3304love]

hello. I'm trying to implement soft inequality constraints using slack variables. Using inequality constraints, the slack variable will always be set greater than 0, but if an obstacle approaches the robot within the safety margin distance, the slack variable within the setIneqConFunction function will be negative.

Below are the setIneqConFunction function and setObjectiveFunction function that use slack variable.

void NonlinearMPC::set_obj()
{
    mpc_solver.setObjectiveFunction([&](const mpc::mat<pred_hor + 1, num_states> &x,
                                        const mpc::mat<pred_hor + 1, num_outputs> &,
                                        const mpc::mat<pred_hor + 1, num_inputs> &u, const double &slack) {
        double cost = 0;
        for (int i = 0; i < pred_hor; i++)
        {
            Eigen::VectorXd q = x.row(i).head(dof);
            Eigen::VectorXd x_base = x.row(i).tail(3);

            T_w_b.linear() = Eigen::AngleAxisd(x_base(2), Eigen::Vector3d::UnitZ()).toRotationMatrix();
            T_w_b.translation() << x_base(0), x_base(1), 0.0;

            pinocchio::framesForwardKinematics(model, data, q);
            const pinocchio::SE3 &effector_tf = data.oMf[frame_id];
            Eigen::Affine3d T_w_ee;
            T_w_ee.matrix() = T_w_b.matrix() * T_b_arm.matrix() * effector_tf.toHomogeneousMatrix();

            Eigen::Quaterniond ee_ori(T_w_ee.rotation());
            Eigen::Vector3d ee_pos = T_w_ee.translation();

            Eigen::VectorXd pose_error = ee_pos - ee_pos_ref;
            Eigen::VectorXd ori_error = (ee_ori_ref * ee_ori.inverse()).vec();
            double pose_cost = pose_error.transpose() * Q_trans * pose_error;
            double ori_cost = ori_error.transpose() * Q_ori * ori_error;

            // Modified weight of input (velocity)
            Eigen::Affine3d T_w_arm;
            T_w_arm.matrix() = T_w_b.matrix() * T_b_arm.matrix();

            if ((T_w_arm.translation() - ee_pos_ref).norm() > 1.75)
            {
                R.diagonal() << 10, 10, 10, 10, 10, 10, 0.2, 0.2, 0.2;
            }
            else
            {
                R.diagonal() << 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 10, 10, 0.2;
            }

            double input_cost = u.row(i).dot(R * u.row(i).transpose());

            cost += 0.5 * (pose_cost + ori_cost + input_cost);
        }

        // // final terminal cost
        Eigen::VectorXd q_final = x.row(pred_hor).head(dof);
        Eigen::VectorXd x_base_final = x.row(pred_hor).tail(3);

        T_w_b.linear() = Eigen::AngleAxisd(x_base_final(2), Eigen::Vector3d::UnitZ()).toRotationMatrix();
        T_w_b.translation() << x_base_final(0), x_base_final(1), 0.0;

        pinocchio::framesForwardKinematics(model, data, q_final);
        const pinocchio::SE3 &effector_tf_final = data.oMf[frame_id];
        Eigen::Affine3d T_w_ee;
        T_w_ee.matrix() = T_w_b.matrix() * T_b_arm.matrix() * effector_tf_final.toHomogeneousMatrix();

        Eigen::Quaterniond ee_ori_final(T_w_ee.rotation());
        Eigen::Vector3d ee_pos_final = T_w_ee.translation();

        Eigen::VectorXd pose_error_final = ee_pos_final - ee_pos_ref;
        Eigen::VectorXd ori_error_final = (ee_ori_ref * ee_ori_final.inverse()).vec();

        double final_vel_cost = u.row(pred_hor).dot(Qf_vel * u.row(pred_hor).transpose());
        double final_pose_cost = pose_error_final.transpose() * Qf_trans * pose_error_final;
        double final_ori_cost = ori_error_final.transpose() * Qf_ori * ori_error_final;

        std::cout << "slack : " << slack << std::endl;

        cost += 0.5 * (final_pose_cost + final_ori_cost + final_vel_cost) + slack + 0.5 * slack * slack;

        return cost;
    });
}

void NonlinearMPC::set_constraints()
{
    mpc_solver.setIneqConFunction([&](mpc::cvec<ineq_c> &in_con, const mpc::mat<pred_hor + 1, num_states> &x,
                                      const mpc::mat<pred_hor + 1, num_outputs> &,
                                      const mpc::mat<pred_hor + 1, num_inputs> &u, const double &slack) {
        int index = 0;

        obs_transform.setTranslation(hpp::fcl::Vec3f(obs_pos(0), obs_pos(1), obs_pos(2)));
        obs_transform.setQuatRotation(hpp::fcl::Quaternion3f(obs_ori.w(), obs_ori.x(), obs_ori.y(), obs_ori.z()));
        obs->setTransform(obs_transform);

        for (int i = 0; i < pred_hor + 1; i++)
        {
            in_con(index++) = 0 - slack; // 0 <= slack

            for (size_t j = 0; j < dof; j++)
            {
                in_con(index++) = u(i, j) - V_arm_Ub(j);  // u <= u_max
                in_con(index++) = -u(i, j) + V_arm_lb(j); // u_min <= u
            }
            for (size_t j = dof; j < num_inputs; j++)
            {
                in_con(index++) = u(i, j) - V_base_Ub(j - dof);  // u <= u_max
                in_con(index++) = -u(i, j) + V_base_lb(j - dof); // u_min <= u
            }
            for (int j = 0; j < num_states; j++) //
            {
                in_con(index++) = x(i, j) - x_Ub(j);  // q <= q_max
                in_con(index++) = -x(i, j) + x_lb(j); // q_min <= q
            }

            Eigen::VectorXd q = x.row(i).head(dof);
            Eigen::VectorXd x_base = x.row(i).tail(3);

            pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);

            double min_distance = std::numeric_limits<double>::max();

            for (int j = 0; j < num_arm_links; j++)
            {
                const pinocchio::GeometryObject &go = geomModel.geometryObjects[j];
                hpp::fcl::Transform3f arm_transform;
                arm_transform.setTranslation(hpp::fcl::Vec3f(geomData.oMg[j].translation()));
                arm_transform.setQuatRotation(hpp::fcl::Quaternion3f(geomData.oMg[j].rotation()));

                hpp::fcl::Transform3f world_base_transform;
                world_base_transform.setTranslation(hpp::fcl::Vec3f(x_base[0], x_base[1], 0.5));
                world_base_transform.setQuatRotation(
                    hpp::fcl::Quaternion3f(Eigen::AngleAxisd(x_base[2], Eigen::Vector3d::UnitZ())));

                hpp::fcl::Transform3f world_transform = world_base_transform * arm_transform;

                auto go_collision_object = std::make_shared<hpp::fcl::CollisionObject>(go.geometry, world_transform);

                hpp::fcl::DistanceRequest arm_request;
                hpp::fcl::DistanceResult arm_result;
                hpp::fcl::distance(obs.get(), go_collision_object.get(), arm_request, arm_result);

                if (arm_result.min_distance < min_distance)
                {
                    min_distance = arm_result.min_distance;
                }
            }
            mobile_collision->setTranslation(hpp::fcl::Vec3f(x_base[0], x_base[1], 0.25));
            hpp::fcl::Quaternion3f quat_mobile(Eigen::AngleAxisd(x_base[2], Eigen::Vector3d::UnitZ()));
            mobile_collision->setRotation(quat_mobile.toRotationMatrix());

            hpp::fcl::DistanceRequest mobile_request;
            hpp::fcl::DistanceResult mobile_result;

            if (obs_pos(2) > 0.02)
            {
                mobile_result.min_distance = 100;
            }
            else
            {
                auto obs_copy = obs;
                auto mobile_collision_copy = mobile_collision;
                obs_copy->setTranslation(hpp::fcl::Vec3f(obs->getTranslation()[0], obs->getTranslation()[1], 0));
                mobile_collision_copy->setTranslation(
                    hpp::fcl::Vec3f(mobile_collision->getTranslation()[0], mobile_collision->getTranslation()[1], 0));

                hpp::fcl::distance(obs_copy.get(), mobile_collision_copy.get(), mobile_request, mobile_result);
            }
            if (mobile_result.min_distance < min_distance)
            {
                min_distance = mobile_result.min_distance;
            }

            in_con(index++) = -min_distance + 0.1 + slack;
        }
    });
}

How can I solve this problem?

[nicolapiccinelli]

I see you set in_con(index++) = 0 - slack; // 0 <= slack, which should force the slack to be positive, but then in_con(index++) = -min_distance + 0.1 + slack;. If the slack is forced to be always positive, this does not make the constraint softer; it's forcing the robot to stay farthest from the obstacles.

Given such a soft constraint, I expect the slack variable to reduce the 0.1 safety margin, but as you state the problem, it's working exactly in the opposite direction.

[sm3304love]

I found out what I did wrong. thank you

I have another question about Slack variables.

The argument type of the Slack variable in the callback function is double. What if I want to use slack variables in more than one inequality constraint?

[nicolapiccinelli]

You can use it, but it’s value will result in a trade off to make your problem feasible among multiple constraints.