Solution for Linear Motion Only

ℹ Introduction

We aim to find the angular positions and the magnitudes of the two normal forces F1 and F2 that − when summed up with F0 − will guarantee that the net force acting on the object equates to 0, as stated by Newton's first law. This will allow attaining stationarity for what concerns linear motion.

☝️ At this stage, we are NOT taking into account:

• the tangential components of F1 and F2, thus dealing only with normal forces.
• the torques generated by F1 and F2. As a result, we do expect that we will not achieve stationarity for rotational motion.

Thereby, we need 4 independent variables to describe our solution:

• x[0] is the angular position in radians of the force F1 on the object's perimeter: x[0] increases anticlockwise.
• x[1] is the angular position in radians of the force F2 on the object's perimeter: x[1] increases anticlockwise.
• x[2] is the factor multiplying the normal force F1: x[2]>0 for F1 pointing inward.
• x[3] is the factor multiplying the normal force F2: x[3]>0 for F2 pointing inward.

Given that, we will try to minimize the square norm of the net force. Hence, we do not have to deal with any particular constraints other than the variables' ranges.

From this assumption, it stems immediately that:

bool Grasp::get_nlp_info(...)
{
    n = 4;
    m = 0;
    nnz_jac_g = 0;
    nnz_h_lag = 0;
    index_style = TNLP::C_STYLE;
    return true;
}

Regarding, the variables' bounds, we will let the angular positions x[0] and x[1] vary indefinitely from -inf to +inf. This is a better approach than limiting the angular positions within the obvious range [0,2π] as it will allow the optimizer to exploit the whole domain.

⚠️ Remember that the optimizer is unaware that a wrapping mechanism is active here. Unless we implement some sort of smart and smooth (in the derivative sense) description of the wrapping, we will do a better job by just letting the optimizer run unbounded on the entire span of radians 😉

In addition, we ought to constrain the forces' magnitudes within a positive range as the forces are required to point inward. Since we would like to apply always Fi≠0 we use a tiny nonzero value as the lower bound. On the other hand, we should avoid generating huge forces, thus a value of 2 seems a good candidate for being the upper bound (F0 will not overcome 1).

bool Grasp::get_bounds_info(...)
{
    x_l[0] = -numeric_limits<double>::infinity();
    x_u[0] = +numeric_limits<double>::infinity();
    
    x_l[1] = -numeric_limits<double>::infinity();
    x_u[1] = +numeric_limits<double>::infinity();

    x_l[2] = .001;
    x_u[2] = 2.;

    x_l[3] = .001;
    x_u[3] = 2.;
    return true;
}

Also, a possible starting point may be the one below:

bool Grasp::get_starting_point(...)
{
    x[0] = 0.;
    x[1] = M_PI/2.;
    x[2] = .5;
    x[3] = .5;
    return true;
}

🍎 Implement Newton's First Law

F0 can be readily retrieved from the Problem API using the function Problem::get_F(). Remember that F0 is of type Force, which is characterized by its angular position t on the perimeter along with 2 forces components, normal and tangential to the perimeter in t, whose magnitudes are specified by fn and ft, respectively. The latter fields are only real numbers, thus one needs to get the directions of such forces in our 2D world recruiting the services Problem::get_N() and Problem::get_T().

We have all the ingredients to compute the square norm of the net force Ftot that the algorithm will attempt to minimize in order to satisfy Newton's first law.

We finally recall that the square norm can be obtained by relying on the dot product yarp::math::dot().

bool Grasp::eval_f(...)
{
    auto F = problem.get_F();

    auto Ftot = F.fn*problem.get_N(F.t) + F.ft*problem.get_T(F.t) +
                x[2]*problem.get_N(x[0]) + x[3]*problem.get_N(x[1]);
    obj_value = yarp::math::dot(Ftot,Ftot);
    
    assert(!isnan(obj_value));
    return true;
}

👀 Note how we check if we generate a NaN by mistake. This is a good practice as it prevents iterations from going on as soon as a NaN is detected. NaN's are peculiar in that they spread and thrive very quickly making debugging quite tough.

The gradient of the cost function can be computed quite easily by resorting to the function Problem::get_dN().

bool Grasp::eval_grad_f(...)
{
    auto F = problem.get_F();

    auto Ftot = F.fn*problem.get_N(F.t) + F.ft*problem.get_T(F.t) +
                x[2]*problem.get_N(x[0]) + x[3]*problem.get_N(x[1]);

    grad_f[0] = 2. * yarp::math::dot(Ftot, x[2]*problem.get_dN(x[0]));
    grad_f[1] = 2. * yarp::math::dot(Ftot, x[3]*problem.get_dN(x[1]));
    grad_f[2] = 2. * yarp::math::dot(Ftot, problem.get_N(x[0]));
    grad_f[3] = 2. * yarp::math::dot(Ftot, problem.get_N(x[1]));

    for (Ipopt::Index i = 0; i < n; i++) {
        assert(!isnan(grad_f[i]));
    }
    return true;
}

👍 Finalize the Solution

This is a straightforward task. Just let the result make more sense by wrapping the angles in [0,2π].

void Grasp::finalize_solution(...)
{
    result[0].t = problem.wrap_angle(x[0]);
    result[0].fn = x[2];
    result[0].ft = 0.;
    result[1].t = problem.wrap_angle(x[1]);
    result[1].fn = x[3];
    result[1].ft = 0.;
}

📈 Example Output

Here's a typical solution to the problem as provided by the implementation above:

Clearly, the object is stationary in terms of linear motion (i.e. F=0), but it will rotate because of a nonzero net torque. This is somehow expected as we did not compensate for it.

⏭ Moving Forward

We are now equipped with a solid baseline from where to kick off the following activities:

1. Extend the present solution in order to incorporate Newton's first law for rotational motion.
2. Consider friction cones and thus deal with tangential components of the forces, while avoiding slippage.