Questions about the IpOpt implementation

[alecive]

Hey @pattacini @matejhof ,

me and @brahmgardner are trying to go in-depth with the code of the reactIpOpt class, but we need some help because there is little to no documentation. So here is a list of the questions we have right now:

• What does He do?
• Can you enlighten us about what happens in computeQuantities? We are especially concerned about what happens from line 319 to 323
• What does skew do?
• What does v2m do?
• Hr is the matrix related to the particle position and orientation (as far as we understood). Why it is not used to compute err_xyz (but only err_ang)?

[pattacini]

Hi @alecive ,

Unfortunately, there's quite heavy maths underlying all those computations and I can only point you to some useful resources:

• For the Skew Matrices used to compute rotations.
• v2m simply gives back the 4-by-4 homogeneous matrix corresponding to the 6-by-1 vector that accounts for translation (the first 3 components) and rotation (the last 3 components) expressed in compact axis-angle notation (only 3 coordinates instead of 4).
• computeQuantities computes at each solver iteration the prediction of next pose of the end-effector He in terms of translation and rotation given the joint velocities v. The maths is explained in [1].
• err_xyz=pr-pe;, where pr=xr.subVector(0,2); and Hr=v2m(xr);; therefore, Hr is implicit in the computation of err_xyz too.

[1] Robotics, Modelling, Planning and Control, Siciliano & Sciavicco, page 139.

[alecive]

Point 1: v2m

That makes totally sense. You go from a 6x1 pose to a 4x4 transform matrix

Point 2 : skew

So what we have understood is that you go from 3D angular velocities to its corresponding rotation matrix via the skew matrix computation. You do it e.g. here where you retrieve the orientation component of x_dot (with J0_ang* v). Is that correct? If so, the reason for which you use skew instead of e.g. axis2dcm is that here you're dealing with angular velocities rather than static orientations. Is this correct? If this is correct, since we're using ROS and we'll deal with quaternions, we just need to convert the quaternion initially to o_x,y,z and than use that internally.

Point 3: computeQuantities

We know what the method does. We know more or less what the method is supposed to do, but we find very difficult to track it down because there's no documentation / comments on variable names and code in general. E.g. this is what we have in reactIpOpt.h:

    Vector xr,pr;
    Matrix Hr,skew_nr,skew_sr,skew_ar;
    Matrix q_lim,v_lim;
    Vector q0,v0,v,p0;
    Matrix H0,R0,He,J0_xyz,J0_ang,Derr_ang;
    Vector err_xyz,err_ang;
    Matrix bounds;
    double dt;

as you can see, no documentation is given. So we spent a lot of time tracking down things, and this is what we understood, but we would need feedback on this:

• xr is the 6x1 pose vector that represents the next step, i.e. the pose that the particle thread computed for the end-effector trajectory.
• pr is the 3x1 position vector extracted from xr (i.e. xr[0,1,2])
• Hr is the 4x4 transform matrix extracted from xr
• q0,v0,p0 are initial joint configuration, joint speed, and end effector position but no orientation, because:
• differently from xr, orientation for the initial ee pose is coded in a matrix, ie H0, whose rotational component is R0
• He is the computed 4x4 transform matrix that is computed internally by ipopt to solve the task. So why the hell did you call it like that? Is it something like "expected H" or something? Similarly, why Hr is called like that?
• skew_nr, skew_sr, skew_ar are out of our comprehension, so we would need a bit of a help with them. Our understanding is that they encode the transform matrix of the individual rotations about x,y,z in Hr. And this makes sense. What it does not make sense to us is: i) their name ii) how they are used to compute L in computeQuantities iii) more importantly, what does L do ?

[pattacini]

Hi @alecive I understand you found the maths a bit complicated but, actually, it is and nothing can make it easier...

The code just implements the instructions reported in the book - as I said. Did you have the chance to get your copy? Here's the page, which I guess is quite enlighting 😉, comprising all the quantities you're seeking information about (L, skews...):

Terminology

Very simply, He accounts for the predicted end-effector pose (the one the end-effector will reach at next time instant given the velocity found out by Ipopt), whereas Hr accounts for the reference (desired) pose to be attained.

Therefore:

• the subscript e stands for end-effector;
• the subscript r stands for reference.

[alecive]

Haha ok great thanks for the tip! I missed the reference to the book at the end of your previous comment. :+1:

[alecive]

I found some major bugs in our IPOPT controller that prevented it from working with orientation control ON. So I took it as a chance to restructure the class significantly in many directions. We are now sourcing from Eigen classes more than what we were doing before, and our code is farther away from yours in terms of implementation.

This did not fix the issue: after a one-day debugging, I still had failures in the derivative checker (each grad_f was offset by a constant value that was depending on the joint configuration), and the solver was not converging. Then I realized that I was not correctly computing the angular error, ie eq 3.83 in the book: instead of r sin (theta) I was doing r theta. Changing this fixed the derivative checker errors. I still have to test on the robot, but I wanted to write this here for the following reason: we were incorrectly computing err_ang because that is what you were basically doing here https://github.com/robotology/react-control/blob/master/modules/reactController/src/reactIpOpt.cpp#L441 (and @brahmgardner was not thorough in his porting).

So, first question: is that a bug on your side? Am I missing something else that is instead clear to you guys?

Second question (very minor since now it seems to work, but still): err_ang can be computed with either equation 3.84 or equation 3.85. @pattacini any reason for choosing the former over the latter?

EDIT Nevermind, the derivative checker still fails on the robot for some reason (but not in my unit tests apparently). There is still some work to be done apparently.

[pattacini]

Hi @alecive

Those relations are all equivalent expressions for err_ang. That said, we usually go for 3.84, which is implemented by the snippet you pointed out above. It's faster than 3.83 and 3.85.

Nonetheless, 3.85 is the starting point to obtain the expression for the derivative of err_ang.

Hope this clarifies the thing 😉

[matejhof]

So, just to make sure - the iCub code is bug free in this case?

[pattacini]

It should be, @matejhof We cannot be 100% certain when speaking about SW 😉

[pattacini]

Hold on, just to contradict myself, there might be an error. Let me better check.

[alecive]

I've been digging more on the issue. Still a lot needs to be doublechecked/taken care of, but I wanted to update you with the current state of our "side".

So, since we use ROS, we are dealing with quaternions as much as possible (because this is what ROS uses), and we then switch to rotation matrices / axis angle representations internally in IPOPT. But, all the checks and subsequent stuff are done in quaternions.

Eg, to check if an orientation is reached, I compute the dot product between the current orientation and the desired one and I test if it's sufficiently close to 1 (eg 0.99).

With that said, with the current codebase I've noticed the following. Sometimes IPOPT solves the task (err_ang is low) and the dot product is 1 , with the reference quaternion o_r equal to the estimated quaternion o_e, as in the following:

[ INFO][/baxter_react_controller::finalize_solution]:   o_r: -0.018541 0.969472 0.011161 0.244246
[ INFO][/baxter_react_controller::finalize_solution]:   o_e: -0.018526  0.969471 0.0111361 0.244252
[ INFO][/baxter_react_controller::finalize_solution]:   ori err [quaternion dot product]: 1 IPOPT_err_ang 3.57434e-09

but some other times, and this happened for example the 10ms after what I've shown above, IPOPT succeeds, but the quaternions are very different and the dot product is close to 0, as in here:

[ INFO][/baxter_react_controller::finalize_solution]:   o_r: -0.018541 0.969472 0.011161 0.244246
[ INFO][/baxter_react_controller::finalize_solution]:   o_e:  0.872821 0.0302584  0.481157 -0.0758635
[ INFO][/baxter_react_controller::finalize_solution]:   ori err [quaternion dot product]:0 -7.439e-06 IPOPT_err_ang 2.21355e-10

This may be due to gimball lock issues (that are not present if using quaternions). Again, I just wanted to update you on the issue, I still have to finish my testing.

[alecive]

Update: after we fixed the bug the derivative checker is having smaller errors, but the errors are still there:

* grad_f[          0] =  2.8145702459641879e+00    ~  2.8237029270350433e+00  [ 3.234e-03]
* grad_f[          1] =  1.5143918680459993e+00    ~  1.5175900536235076e+00  [ 2.107e-03]
* grad_f[          2] =  4.2864451644228136e+00    ~  4.2843281033988490e+00  [ 4.941e-04]
* grad_f[          3] =  1.4891315024257885e+00    ~  1.4979096296663386e+00  [ 5.860e-03]
* grad_f[          4] = -1.1613309217735690e+00    ~ -1.1699686774946372e+00  [ 7.383e-03]
* grad_f[          5] = -2.7427788199616518e+00    ~ -2.7333871166490553e+00  [ 3.436e-03]
* grad_f[          6] = -2.5966358424786971e+00    ~ -2.6063809333010113e+00  [ 3.739e-03]

@pattacini do you think this can be due to numerical approximations?

[pattacini]

@alecive it seems pretty ok to me One question: are you using the version with sin(theta) or that with only theta? Asking you 'cause I didn't have time yet to investigate it.

[alecive]

I am using with sin(theta). With theta the error in the derivative checker is much higher, i.e. in the ballpark of e02 instead of e-03.

[alecive]

Ok I give up. I cannot manage to make it work, there are too many things out of my control.

@pattacini when you have time, can you double check that my computeQuantities does the same things as yours? There are differences since I am using Eigen directly and not iKin, but to me they seem to perform the same things.

Here's yours:

for (size_t i=0; i<v.length(); i++)
    v[i]=x[i];

Vector w=J0_ang*v;
double theta=norm(w);
if (theta>0.0)
    w/=theta;
w.push_back(theta*dt);
He.setSubmatrix(axis2dcm(w).submatrix(0,2,0,2)*R0,0,0);

Vector pe=p0+dt*(J0_xyz*v);            
He(0,3)=pe[0];
He(1,3)=pe[1];
He(2,3)=pe[2];

err_xyz=pr-pe;
err_ang=dcm2axis(Hr*He.transposed());
err_ang*=err_ang[3];
err_ang.pop_back();

Matrix L=-0.5*(skew_nr*skew(He.getCol(0))+
               skew_sr*skew(He.getCol(1))+
               skew_ar*skew(He.getCol(2)));
Derr_ang=-dt*(L*J0_ang);

Here's mine:

// Let's update the estimated velocities
for (int i=0; i<v_e.size(); ++i)
{
    v_e[i]=x[i];
}

// Now, let's compute the position and orientation errors
Vector3d  w_e = J_0_ang*v_e;          // rotational (angular) speed
double theta =   w_e.norm();
if (theta > 0.0) { w_e /= theta; }

AngleAxisd w_e_aa(theta * dt, w_e);     // angular increment in axis angle representation

R_e = w_e_aa.toRotationMatrix() * R_0;
p_e = p_0 + dt * (J_0_xyz * v_e);

err_xyz = p_r-p_e;

AngleAxisd aa_err((R_r)*(R_e.transpose()));
err_ang = aa_err.axis() * sin(aa_err.angle());

MatrixXd L=-0.5*(skew_nr*skew(R_e.col(0))+
                            skew_sr*skew(R_e.col(1))+
                            skew_ar*skew(R_e.col(2)));

Derr_ang=-dt*(L*J_0_ang);

Notation is similar (_r means reference, _e means estimated), and I am not using full 4x4 matrices but 3x3 rotation matrices (hence, R_ instead of H_).

[alecive]

I finally found the bug, tracked here https://github.com/ScazLab/baxter_react_controller/issues/11

It does not make much sense, but using the same code that they are supposed to use internally, orientation computation is wrong starting from the first joint of the chain. I need to investigate. Let me close this comment with an hashtag: #death2closedsource

[alecive]

Obviously my kinematics is correct since there is no positional error that propagates up to the end-effector, it's just something that I am missing. What happens is that in some cases, if this is my chain (some segments are only fixed rototranslations):

[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H0: [0 0 0], [0 0 0 1]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H1: [0.0246579 -0.218297  0.108225], [-0.00276922 0.00101229    0.38369 0.923457]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H2: [0.0639101 -0.257813  0.119248], [-0.00267287 0.00124461   0.303593 0.952797]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H3: [ 0.119111 -0.298905  0.389645], [0.660562 0.246441 0.586588 0.398546]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H4: [ 0.138528 -0.313387  0.488727], [-0.0829376 -0.0861705  -0.124205 0.985022]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H5: [ 0.254329 -0.332777  0.733348], [ 0.578118  -0.27089 -0.480862 0.600974]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H6: [  0.29481 -0.305351  0.642024], [ -0.657392   0.713182 -0.0361792 -0.240621]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H7: [ 0.400395 -0.243231  0.400423], [ 0.826685 0.0239935   -0.5538 0.0965478]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H8: [ 0.445099 -0.226229  0.294769], [-0.0659556   0.97527 0.0615162 -0.201777]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H9: [ 0.488868 -0.209582  0.191325], [-0.0659556   0.97527 0.0615162 -0.201777]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H10: [ 0.498505 -0.205916   0.16855], [-0.0659556   0.97527 0.0615162 -0.201777]
[ INFO][/baxter_react_controller::Frame BaxterChain::JntToCart]: H11: [ 0.549655 -0.186462 0.0476595], [-0.0659556   0.97527 0.0615162 -0.201777]

H1 already starts to be wrong, in this case is [0.024658; -0.2183; 0.10822] [0.0027692; -0.0010123; -0.38369; 0.92346] (notice the minus sign in the z axis of the quaternion). This is only FYI (and for @matejhof when he'll start using the baxter :grin: )

[matejhof]

Excellent job, @alecive ! So you're saying the error comes from a Baxter kinematic library?

[alecive]

As it turned out, from KDL. We are using it internally because i) we cannot use iKin ii) we don't have time to port iKin iii) it provides a good set of utilities to work with urdfs. But I am trying to use it only where needed, and relying on Eigen instead. There was a bug in converting a KDL::Frame to an Eigen::Matrix4d because apparently the KDL::Rotation stores the matrix in a std::vector<double> column-by-column instead of row-by-row. No documentation was provided for this choice, that to me is totally counter-intuitive (eg Eigen uses a row-by-row convention, as well as YARP_sig::Matrix if I recall correctly).

I even have placed unit tests to check for that, but unfortunately for a variety of reasons they weren't able to catch the bug. And this made it even more difficult to catch since I was sure it wasn't there :sunglasses:

[pattacini]

👍 great catch 👎 for not using iKin 😞 😉

[alecive]

Closing for now since the issue has become too big. Still, two issues should be fixed:

1. We still don't know if to use theta (like in the code) or sin(theta) (like in the book). The issue is difficult to solve because sin is linear when theta is close to 0 (as in most of the cases when dealing with incremental reference poses). My hunch is to lean toward theta, but in my testing the derivative test's errors are at least one order of magnitude smaller when using sin(theta).
2. In my code, the derivative tests still have errors (although the solver now works). This means that i) IPOPT could be much faster and ii) the math (or the code) are still wrong. But this probably is on my side, so I will close this issue, investigate more, and when I'll find a solution I'll probably be able to fix 1.

[matejhof]

Fantastic progress! :+1: