Relative orientation formulation

[zicodasilva]

@alknemeyer Do you have any guidance/tips for supporting both the absolute and relative angle formulations? This would be a nice add-on so that we have full control on the formulation of the problem.

[alknemeyer]

I just pushed a branch that starts on it. It's untested and written while tired, so don't trust it!

Given that most things just expect a link to have an orientation in inertial coordinates, it shouldn't matter that Rb_I is made up of more rotations (the rotations of the previous link, plus some extra rotations). Likewise it should be fine that your q vector is shorter?

The main thing is that the relative angle links will have fewer symbolic angles defined. I think you'll either have to define fewer angles in the pyomo bits, or keep it as is and just let those extra (unused) variables as zero. I would start by skimming through the uses of is_base, q and Rb_I to see if you can surface assumptions of absolute orientation

[zicodasilva]

Great, thanks for adding the new branch. I will take a look at it and test it on the monoped model. Will let you know if I find any issues.

[zicodasilva]

I'm getting a little bit confused about the notation used for the rotations. For orientation of the body in the inertial frame, you use euler-321 (which is a yaw-pitch-roll sequence):

def euler_321(phi: SymOrFloat, theta: SymOrFloat, psi: SymOrFloat) -> Mat:
    return rot_x(phi) @ rot_y(theta) @ rot_z(psi)

which I presume to be phi - roll, theta - pitch, psi - yaw. In the example diagrams and example code (it seems) you use theta to be the rotation around the x-axis and phi the rotation around the y-axis? Am I missing something?

Also, while we on the topic. In your example quadraped3D.py, you have the following:

  # input torques to tail - pitch and yaw
  body_B.add_hookes_joint(tail0, about='xy')
  add_torque(body_B, tail0, about='xy', **params['motor']['spine-tail0'])

  # torques in the middle of the tail - pitch and yaw
  tail0.add_hookes_joint(tail1, about='xy')
  add_torque(tail0, tail1, about='xy', **params['motor']['tail0-tail1'])

should it not be about='yz'? I'm free to have a chat about this, because I am a bit confused at the moment.

[alknemeyer]

In the example diagrams and example code (it seems) you use theta to be the rotation around the x-axis and phi the rotation around the y-axis? Am I missing something?

Ah, I may have switched the x- and y-axes after the coordinates modelling chapter, during which time those diagrams were made. I think it would be better to stick to the example in chapter 4 of my dissertation.

should it not be about='yz'?

Yes, it should! I can't believe I never noticed that mistake. In fact, I wonder if that contributed to my struggling to get useful/consistent results in the final chapter of my write up. Sorry! And good working spotting this stuff!

[zicodasilva]

Ya, it is always good to have code reviews because it is so easy to make silly mistakes.

So I have something almost working for the relative angle formulation (without dynamics) but there is something odd happening when I compare the NLP produced for AcinoSet vs physical_education: the number of non-zeros in "Hessian for the Lagrangian function" for AcinoSet formulation is 0, but for physical education formulation it is like 40000. The Pyomo setup is pretty much the same, only kinematic constraints and assumption of constant acceleration. The only difference seems to be the formulation of the kinematic equations (i.e. different euler angles and slightly different lengths of links). Any idea what is going on here? Not sure exactly what the Hessian of the lagrangian function means.

[alknemeyer]

"Lagrangian" in general refers to a few things, but in this case, the Lagrangian is the one used in constrained optimisation problems of the form:

minimize/maximize f(x), subject to g(x) = 0

To solve these problems, you can use the Lagrangian, defined as

L(x, λ) = f(x) - λ*g(x)

When Ipopt solves the optimisation problem it uses the Hessian of the Lagrangian -- ie, it finds

H[i, j] = L.deriv(x[i]).deriv(x[j]) for i=1..n, j=1..n where n=length(x)

So for the Hessian to be non-zero, there must be elements in your objective or constraints function that don't disappear after taking that double derivative. That happens with stuff like f(x) = x^2 (in which case H = 2), or f(x1, x2) = x1*x2, in which case

H = [0 1
     1 0]

If you have f(x1, x2) = 2*x1 + 5*x2 then the Hessian will be all zeros. I don't exactly know why the AcinoSet version has no non-zero elements, but physical_education could have non-zero elements because the timestep is non-constant? eg hessian(position + time*velocity where those are all variables, will have non-zero elements.

If you're reading up on this, don't get mixed up with Lagrangian mechanics, which is totally different and just confusingly named. Also, disclaimer, haven't brushed up on this stuff in a while so I may be wrong!

[zicodasilva]

Thanks for that explanation. I figured out the issue, there was a bug with parsing the OF_hessian_approximation='limited-memory' value in the pe.utils.default_solver function. I assumed that it was working but it actually wasn't using that value, and so the optimisation was super slow and "shit". When I fixed that, the The number of non-zeros in Hessian for the Lagrangian function becomes 0 which matches the AcinoSet stuff :). I will push that fix along with the fix for the relative angle formulation. I'm currently doing:

 if rotations is not None and parent_orientation is not None:
          self.relative_orientation = True
          q_list, dq_list, ddq_list = [], [], []
          for rotation in rotations:
              q_temp, dq_temp, ddq_temp = symdef.make_ang_sym(rotation, name)
              q_list.append([q_temp])
              dq_list.append([dq_temp])
              ddq_list.append([ddq_temp])
          self.q = Mat(q_list)
          self.dq = Mat(dq_list)
          self.ddq = Mat(ddq_list)
          self.Rb_I = parent_orientation
          self.rel_q_set = []
          for (rotation, angle) in zip(rotations, self.q):
              if rotation == 'x':
                  self.Rb_I = utils.rot_x(angle) @ self.Rb_I
                  self.rel_q_set.append('phi')
              elif rotation == 'y':
                  self.Rb_I = utils.rot_y(angle) @ self.Rb_I
                  self.rel_q_set.append('theta')
              elif rotation == 'z':
                  self.Rb_I = utils.rot_z(angle) @ self.Rb_I
                  self.rel_q_set.append('psi')
              else:
                  raise ValueError(f"Can't rotate about '{rotation}'")
      else:
          self.relative_orientation = False

in the setup of a link as relative to the parent.

[alknemeyer]

Wow, that's so bizarre! I have absolutely no idea why the OF_hessian_approximation argument isn't used. I definitely used it in my optimisations. I must have somehow removed it, or forgot to commit it? Sorry!

That code block looks good. I'm keen to see your results! Have you run an optimisation using relative angles yet?

[zicodasilva]

Yes, I'm busy running it on a dynamical model to test whether it is working as expected. I have tested it with a pure kinematic model and it works :) I'm wondering if you agree that we should prevent the following from happening when using relative angle formulation:

    def add_hookes_joint(self, otherlink: 'Link3D', about: str) -> 'Link3D':
        """Add a hooke's joint about axis `about` of `self`
        >>> link_body.add_hookes_joint(link_UFL, about='xy')
        """
        assert all(ax in ('x', 'y', 'z') for ax in about) and len(about) == 2 and not self.relative_orientation

        axes = Mat([1, 0, 0]), Mat([0, 1, 0]), Mat([0, 0, 1])
        vec1 = axes['xyz'.index(about[0])]
        vec2 = axes['xyz'.index(about[1])]

        ang_constr = (
            (self.Rb_I @ vec1).dot(otherlink.Rb_I @ vec2)
        )
        self.angle_constraints.append(ang_constr)

        self.constraint_forces.append(
            sp.Symbol('F_{r/%s/%s}' % (self.name, otherlink.name))
        )

        return self

    def add_revolute_joint(self, otherlink: 'Link3D', about: str) -> 'Link3D':
        """Adds a revolute connection about axis `about` of `self`

        >>> link_UFL.add_revolute_joint(link_LFL, about='y')
        """
        assert about in ('x', 'y', 'z') and not self.relative_orientation

I've added and not self.relative_orientation to the assert to prevent someone from adding angle and force contraints between links in a relative model where that is implicitly implied.

[alknemeyer]

Awesome, I'm so glad to hear!

Good idea for the assertions :+1: I also like the idea of printing out what the argument was, to make debugging a little simpler. Eg:

assert about in ('x', 'y', 'z') and not self.relative_orientation, f"Got invalid axis: {about}"

[zicodasilva]

That is a good point, and with that, I will close this issue :)