Cylinder Update

[gbaraban]

Makes cylinder work for 3D Rigid Body States

---

This change is 

[marinkobi]

Reviewed 6 of 6 files at r1.
Reviewable status: all files reviewed, 12 unresolved discussions (waiting on @gbaraban)

---

lib/systems/constraints/cylinder.h, line 77 at r1 (raw file):

        //DO NOT TRUST 
        if (g[0] > 0)
          dgdx->row(0).segment(3,3) = -v/v.norm();

you could maybe check the type of the state T and based on that use different indices, e.g.
if (typeid(T) == typeid(Body3dState) {

} else ....

---

if this is not meant to be used to other objects then you can add
assert(typeid(T) == typeid(Body3dState))

---

lib/systems/constraints/groundplane.h, line 8 at r1 (raw file):

namespace gcop {
  
  template <typename T = VectorXd,

it'd be nice to add some documentation what this does

---

lib/systems/constraints/groundplane.h, line 60 at r1 (raw file):

      double d = v - cr;  // distance from center of cylinder to system boundary
      
      //if (d < 0) {

best not to leave commented out code unless it'll be useful in the future (in which case you leave a note about it), e.g.
// NOTE(Gabe): leaving this commented out for future debugging use

---

lib/systems/constraints/groundplane.h, line 72 at r1 (raw file):

        //It assumes that the position indices of the x vector are 3,4,5, but other states use 0,1,2
        //DO NOT TRUST 
        dgdx->row(0)[5] = -1;

same comment as above regarding checking the type

---

lib/systems/constraints/yawvelfollow.h, line 14 at r1 (raw file):

    int _nu = Dynamic, 
    int _np = Dynamic>
    class YawVelFollow : public Constraint<T, _nx, _nu, _np> {

A more accurate name would include the word constraint at the end, e.g. YawVelocityConstraint

---

lib/systems/constraints/yawvelfollow.h, line 71 at r1 (raw file):

        if (d < -M_PI){
          d += 2*M_PI;
        }

it'd be good to add an assert that d is between -PI and PI here

---

lib/systems/constraints/yawvelfollow.h, line 74 at r1 (raw file):

      }

      g[0] = d*d; // must be 0 for non-collision

the math is somewhat cleaner if the cost if 0.5dd

---

lib/systems/constraints/yawvelfollow.h, line 98 at r1 (raw file):

        double partial_dx = 2*d;
        Vector3d dThetadeR = eR/theta;
        Vector3d temp(0,2*eR[1],2*eR[2]);

I'm not double-checking this in detail since I think we decided to go with a cost.
In this case is it useful to add this file to the repo?

---

lib/systems/costs/yawcost.h, line 82 at r1 (raw file):

                                          Vectormd *Lp, Matrixmd *Lpp,
                                          Matrixmnd *Lpx) {
    cout << "Yaw L Called" << endl;

remove all these printouts before committing to master

or add them behind a debug flat which is off by default

---

lib/systems/costs/yawcost.h, line 83 at r1 (raw file):

                                          Matrixmnd *Lpx) {
    cout << "Yaw L Called" << endl;
    Vector3d e(0,0,1);

can make this static const to avoid creating it every time
also better name would be e3 or ez

hmm... actually should this be (1,0,0) if you're trying to get the heading vector?

---

lib/systems/costs/yawcost.h, line 94 at r1 (raw file):

    double dx = df(0);
    double dy = df(1);
    df = df/df.norm();

check if close to zero here

---

lib/systems/costs/yawcost.h, line 105 at r1 (raw file):

             0, 0, 0;
    MatrixXd d2dfdp2(9,3);
    d2dfdp2 << (Matrix2d()<<(3*dx/(dnorm*dnorm*dnorm) - 3*(dx*dx*dx)/(dnorm*dnorm*dnorm*dnorm*dnorm)),

that's fine ... but you could easily write all of this using matrix-vector notation

the fractions below can be simplified by using the normalize df instead of dx and y

[gbaraban]

Reviewable status: all files reviewed, 12 unresolved discussions (waiting on @gbaraban and @marinkobi)

---

lib/systems/constraints/cylinder.h, line 77 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

you could maybe check the type of the state T and based on that use different indices, e.g.
if (typeid(T) == typeid(Body3dState) {

} else ....

---

if this is not meant to be used to other objects then you can add
assert(typeid(T) == typeid(Body3dState))

I added a check for if Body3dState is the state.

---

lib/systems/constraints/groundplane.h, line 8 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

it'd be nice to add some documentation what this does

Done.

---

lib/systems/constraints/groundplane.h, line 60 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

best not to leave commented out code unless it'll be useful in the future (in which case you leave a note about it), e.g.
// NOTE(Gabe): leaving this commented out for future debugging use

Done.

---

lib/systems/constraints/groundplane.h, line 72 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

same comment as above regarding checking the type

Done.

---

lib/systems/constraints/yawvelfollow.h, line 14 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

A more accurate name would include the word constraint at the end, e.g. YawVelocityConstraint

Done.

---

lib/systems/constraints/yawvelfollow.h, line 71 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

it'd be good to add an assert that d is between -PI and PI here

Done.

---

lib/systems/constraints/yawvelfollow.h, line 74 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

the math is somewhat cleaner if the cost if 0.5dd

Done.

---

lib/systems/constraints/yawvelfollow.h, line 98 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

I'm not double-checking this in detail since I think we decided to go with a cost.
In this case is it useful to add this file to the repo?

To be honest, I didn't mean to commit this to this branch. I'll switch it over to a new one.

---

lib/systems/costs/yawcost.h, line 82 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

remove all these printouts before committing to master

or add them behind a debug flat which is off by default

I'll get rid of them when it's working.

---

lib/systems/costs/yawcost.h, line 83 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

can make this static const to avoid creating it every time
also better name would be e3 or ez

hmm... actually should this be (1,0,0) if you're trying to get the heading vector?

My mistake.

---

lib/systems/costs/yawcost.h, line 94 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

check if close to zero here

will do.

---

lib/systems/costs/yawcost.h, line 105 at r1 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

that's fine ... but you could easily write all of this using matrix-vector notation

the fractions below can be simplified by using the normalize df instead of dx and y

working on it.

[gbaraban]

Reviewable status: 1 of 2 files reviewed, 12 unresolved discussions (waiting on @gbaraban and @marinkobi)

---

lib/systems/constraints/cylinder.h, line 77 at r1 (raw file):

Previously, gbaraban wrote…

I added a check for if Body3dState is the state.

Done.

---

lib/systems/costs/yawcost.h, line 83 at r1 (raw file):

Previously, gbaraban wrote…

My mistake.

Done.

---

lib/systems/costs/yawcost.h, line 94 at r1 (raw file):

Previously, gbaraban wrote…

will do.

Done.

[marinkobi]

BUG
o

Reviewed 3 of 5 files at r2.
Reviewable status: 2 of 3 files reviewed, 10 unresolved discussions (waiting on @gbaraban and @marinkobi)

---

lib/systems/constraints/cylinder.h, line 73 at r2 (raw file):

        dgdx->resize(1, _nx);
        dgdx->setZero();
        //THIS IS A BUG.  For states that are not Body3dState, it won't necessarily work.

remove comment

---

lib/systems/costs/yawcost.h, line 69 at r1 (raw file):

  template <typename T, int _nx, int _nu, int _np> 
    YawCost<T, _nx, _nu, _np>::YawCost(System<T, _nx, _nu, _np> &sys, 
                                           double tf, Vector3d t) : Cost<T, _nx, _nu, _np>(sys, tf) {

target

---

lib/systems/costs/yawcost.h, line 82 at r1 (raw file):

Previously, gbaraban wrote…

I'll get rid of them when it's working.

OK, then you should not be targeting master but a temporary debug branch;
or
you will need one final commit where you remove them, before merging. so I'm leaving this comment as "blocking"

---

lib/systems/costs/yawcost.h, line 101 at r1 (raw file):

    SO3::Instance().hat(ehat, e);
    Matrix3d ddfdp;
    ddfdp << ((-1/dnorm) + dx*dx/(dnorm*dnorm*dnorm)), (dx*dy)/(dnorm*dnorm*dnorm), 0,

this looks correct

---

lib/systems/costs/yawcost.h, line 122 at r1 (raw file):

    cout << "Yaw Lx Called" << endl;
      Lx->setZero();
      Vector3d dLdR = c*(ehat.transpose()*x.R.transpose()*df);

I believe this should be:

Vector3d dLdR = -gain*(ehat.transpose()*x.R.transpose()*df);

---

lib/systems/costs/yawcost.h, line 123 at r1 (raw file):

      Lx->setZero();
      Vector3d dLdR = c*(ehat.transpose()*x.R.transpose()*df);
      Vector3d dLdp = -c*(ddfdp.transpose()*x.R*e);

I believe this should be

-gain*(ddfdp.transpose()x.Re);

---

lib/systems/costs/yawcost.h, line 129 at r1 (raw file):

    if (Lxx){
    cout << "Yaw Lxx Called" << endl;
      Lxx->setZero();

up to you to double-check Hessians, but
for debugging purposes you could always set the Hessian to the identity (you should still get some convergence-- although not very balanced).

---

lib/systems/costs/yawcost.h, line 39 at r3 (raw file):

   * @param tf time horizon: when the cost function L is called it will internally check whether its argument t is equation to tf and return the terminal cost
   */
  YawCost(System<T, _nx, _nu, _np> &sys, double tf, const Vector3d &targ);

targ -> target
or even better
targ -> target_position

---

lib/systems/costs/yawcost.h, line 62 at r3 (raw file):

  Vector3d target;
  const Vector3d e = Vector3d::UnitX();

const reference
or
static const

[marinkobi]

Reviewed 1 of 1 files at r3.
Reviewable status: all files reviewed, 10 unresolved discussions (waiting on @gbaraban and @marinkobi)

[marinkobi]

Reviewed 1 of 1 files at r4.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @gbaraban)

[gbaraban]

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @gbaraban)

---

lib/systems/constraints/cylinder.h, line 73 at r2 (raw file):

Previously, marinkobi (Marin Kobilarov) wrote…

remove comment

Done.