Update 1. "Shelving" GCS!

At this point, not sure if leaving continuous variables up until the last moment gives us anything.

Context for the "next" project.

What kind of limitations are there in the current RRT through contact approach?

1. Contact sampling: we assume the fingers "teleport"
2. Quasidynamic model error
3. Anitescu model error (friction hydroplaning artifacts). 
4. No easy way to do closed-loop feedback control afterwards.
5. Randomness of RRT

Context for the "next" project.

Context for the "next" project.

What kind of limitations are there in the current RRT through contact approach?

1. Contact sampling: we assume the fingers "teleport"
2. Quasidynamic model error
3. Anitescu model error (friction hydroplaning artifacts). 
4. No easy way to do closed-loop feedback control afterwards.
5. Randomness of RRT

Started by thinking: let's address the first issue at least, and come up with a better contact sampling algorithm.

Context for the "next" project.

What kind of limitations are there in the current RRT through contact approach?

1. Contact sampling: we assume the fingers "teleport"
2. Quasidynamic model error
3. Anitescu model error (friction hydroplaning artifacts). 
4. No easy way to do closed-loop feedback control afterwards.
5. Randomness of RRT

Started by thinking: let's address the first issue at least, and come up with a better contact sampling algorithm.

But first, what is our RRT algorithm fundamentally doing?

Why do we expect better contact samplers to help RRT greatly?

Context for the "next" project.

What kind of limitations are there in the current RRT through contact approach?

1. Contact sampling: we assume the fingers "teleport"
2. Quasidynamic model error
3. Anitescu model error (friction hydroplaning artifacts). 
4. No easy way to do closed-loop feedback control afterwards.
5. Randomness of RRT

Started by thinking: let's address the first issue at least, and come up with a better contact sampling algorithm.

RRT Projection as Greedy Descent Down a CLF

Suppose an RRT algorithm that chooses a goal as the subgoal 100% of the time.

Then, I could propose a distance metric from the current object position 

to the goal position.

Is this a control Lyapunov function for a contact-rich system?

In other words, at q, can we always find some u such that 

(i.e. can we always choose some action to decrease this distance metric at the next step)

Is this a CLF?

Is this a control Lyapunov function for a contact-rich system?

In other words, at q, can we always find some u such that 

(i.e. can we always choose some action to decrease this distance metric at the next step)

This would be a ridiculous proposition, otherwise contact problems would be way too easy :)

Steepest Descent Controller

But for some "local" problems where trajopt is likely to succeed, this greedy descent down the CLF candidate does pretty well. 

RRT Projection as Greedy Descent Down a CLF

Is this a CLF?

But why not? what are the cases where we can't choose one-step actions that decrease the distance?

Is this a control Lyapunov function for a contact-rich system?

In other words, at q, can we always find some u such that 

(i.e. can we always choose some action to decrease this distance metric at the next step)

This would be a ridiculous proposition, otherwise contact problems would be way too easy :)

But why not? what are the cases where we can't choose one-step actions that decrease the distance?

Is this a CLF?

Short answer: We can't always choose such a u because we lose actuation in the direction of the goal. 

In this case, kinematic limits manifest as loss of actuation.

But why not? what are the cases where we can't choose one-step actions that decrease the distance?

Why isn't this a CLF?

Short answer: We can't always choose such a u because we lose actuation in the direction of the goal. 

Long answer: 

1. Kinematic limits manifest as loss of actuation. 

2. The unilateral nature of contact (can only push, can't pull) can also result in loss      of actuation.

3. Other effects (friction) may appear.

Motivating joint relocation

So far we know that for all q =/ qg, it is not possible to find u s.t.

But what if we change the action space slightly?

"Find an actuation to decrease the current cost."

"Jointly find an actuation and a joint configuration to decrease the current cost."

High-Level Skeleton of an Algorithm

While True:

find u s.t. 

if [we need to relocate the joints]:

    [relocate the joints.].

1. When do we know we have to relocate the joints automatically?

2. How do we relocate the joints? To which configuration?

High-Level Skeleton of an Algorithm

While True:

find u s.t. 

if [we need to relocate the joints]:

    [relocate the joints.].

1. When do we know we have to relocate the joints automatically?

     When we know that there aren't any actions that will change the cost.

2. How do we relocate the joints? To which configuration?

     Choose a joint location that gives us maximum actuation.

     But also make sure this relocation of joints is feasible. 

High-Level Skeleton of an Algorithm

2. How do we relocate the joints? To which configuration?

     Choose a joint location that gives us maximum actuation.

Find a joint location such that choosing a minimum actuation from this joint location results in lowest cost among all the other joint locations.

Current algorithm in <200 lines of code.

While True:

find u s.t. 

periodically: 

1. When do we know we have to relocate the joints automatically?

     When we know that there aren't any actions that will change the cost.

2. How do we relocate the joints? To which configuration?

     Choose a joint location that gives us maximum actuation.

     But also make sure this relocation of joints is feasible. 

Next Steps

1. Come up with a good answer for when we should relocate contact

2. Come up with relocation constraints based on static stability analysis.

     (not necessary for planar demos)