Code refactoring about consim HOT 9 CLOSED

Computing dJv_local_ = alocal_ + w.cross(v) when computing alocal_ causes the simulation to diverge. As a sanity check I switched to computing
dJv_local_ = dJdt_local_ * v_ ;

this method produces results similar to computing
dJv_local_ = w.cross(v)
where the simulation converges with some error relative to the Euler Simulator , this I still find a bit strange, maybe we should move this to a separate issue.

Also ExponentialSimulator::checkFrictionCone() now updates f_projected = f_average in all cases assuming that ExponentialSimulator::computeSlipping() will have to update it, I would rather add the qp implementation in a different pull request though.

let me know if I need to address more refactoring issues before merging this pull request

from consim.

Hi @hammoudbilal

I like the idea of getting rid of the Contact class and moving the anchor point inside ContactPoint. Most of the problems with the new code are with the Simulator classes. Maybe it's because you're trying to share too much code and structure between the Euler and Exponential simulator, which are too different in terms of implementation to share that much code.

I think that this should roughly be the logic flow of a simulation step:

compute all kinematics-dynamic terms based on current state (q,v)
for cp in ContactPointList: 
	cp.update_pos_vel()
	for co in ObjectList: 
		collision = co.check_collision(cp)
		# check_collision updates normal-tangential directions stored in cp

Then for the EULER SIMULATOR we should have:

tau_contact_forces.setZero()
for cp in ContactPointList: 
	if cp.active: 
		cp.object.contact_model.computeForce(cp)
		# the computed contact force is stored in cp
		tau_contact_forces += cp.get_tau_contact_forces()

While for the EXPONENTIAL SIMULATOR:

for cp in ContactPointList: 
	if cp.active: 
		fill matrices J, dJv, p0, p, dp, Kpo
Compute A and b
Compute tau_contact_forces via matrix exponential
Saturate forces at friction cones
Update anchor points

And finally both simulators should finish with

Compute joint accelerations based on tau_contact_forces

With this idea in mind, these are some issues I've found with the code of your PR.

• checkContact should rather be called detectContacts.
• If computeContactState and computeContactForces are always called one after the other, then it should be just one method.
• In ExponentialSimulator, the method computeContactForces doesn't really compute the contact forces but it just computes the quantities needed for computing later the contact forces in step. This is really confusing!
• In both simulators, the order of operations inside step is weird and should be reversed, i.e. first collision detection, then compute forces, then compute accelerations and integrate
• In general the names of the methods are often misleading which make the code hard to read and understand

from consim.

thanks for the tips, I will clean it up

from consim.

Hi @andreadelprete

there are few issues remaining

• computeContactForces for EulerSimulator is done at the end of the integration loop for convenience, this way when reading state x_{t+1} at the end of the step, the code also returns the current forces f_{t+1} instead of f_t at the beginning of the step. for ExponentialSimulator, computeContactForces could be called right after computing the integral intxt to return f_{t+1} then the joint accelerations/velocities and positions could be updated. (maybe i'm wrong about this?)

• updating the anchor points through the qp is still missing, this function is supposed to handle it, but I avoided pushing it with this pull request #11 . However I wrote down the QP for n contact points all together in the pdf file below. The QP optimizes for dp_0, then the delta_p0 could be integrated. Can you please check if what I wrote makes sense? looking at the constraints, the matrices are block diagonal, it seems that the only important factor is the matrix I defined as \Bar{A}
  anchor_point_qp.pdf
  I am guessing I could exploit this and re-write checkFrictionCone based on on the transformation that comes from the corresponding block in \Bar{A}? maybe I could write the QP for each contact point independently ?

• I spent sometime trying to understand why the trajectories of ExponentialSimulator differ a bit from EulerSimulator, this was first discussed in #1 . Following your code, you compute

dJv_local_ = alocal_ + w.cross(v);
dJv_ = Adjoint * dJv_local_;

in order to get 'alocal_' the second order pinocchio::forwardKinematics must be called.
I get a segmentation fault the case where i use

pinocchio::computeAllTerms();
pinocchio::updateFramePlacements();
pinocchio:: forwardKinematics();

if I do the opposite, alocal_ becomes zero

pinocchio:: forwardKinematics();
pinocchio::computeAllTerms();
pinocchio::updateFramePlacements();

I will have to dig deeper into this, I still don't have an answer.

from consim.

* `computeContactForces` for `EulerSimulator` is done at the end of the integration loop for convenience, this way when reading state x_{t+1} at the end of the step, the code also returns the current forces f_{t+1} instead of f_t at the beginning of the step. 

I see. Let's leave it like this for now, but let's make this consistent with the exponential simulator.

for ExponentialSimulator, computeContactForces could be called right after computing the integral intxt to return f_{t+1} then the joint accelerations/velocities and positions could be updated. (maybe i'm wrong about this?)

The force computed using int_xt is a sort of "predicted force" obtained via the linear dynamical system. This is gonna be different from the real force you get from the nonlinear system after updating q and v. Beware of this difference. I think that from python we wanna be able to access both these forces to analyze what's going on inside the simulator. If the time step we use is not too large, then the two forces should be rather close.

I wrote down the QP for n contact points all together in the pdf file below. The QP optimizes for dp_0, then the delta_p0 could be integrated.  Can you please check if what I wrote makes sense? 

At a first glance it all makes sense. I'll take a better look later.

looking at the constraints, the matrices are block diagonal, it seems that the only important factor is the matrix I defined as \Bar{A}
anchor_point_qp.pdf
I am guessing I could exploit this and re-write checkFrictionCone based on on the transformation that comes from the corresponding block in \Bar{A}? maybe I could write the QP for each contact point independently ?

I think the constraint matrix in the QP is dense, because it contains the matrix exponential, which is a dense matrix, so unfortunately you cannot solve a separate QP for each contact (but please double check as I may be wrong).

* I spent sometime trying to understand why the trajectories of `ExponentialSimulator`  differ a bit from `EulerSimulator`, this was first discussed in #1 . Following your code, you compute

dJv_local_ = alocal_ + w.cross(v);
dJv_ = Adjoint * dJv_local_;

in order to get 'alocal_' the second order pinocchio::forwardKinematics must be called.
I get a segmentation fault the case where i use

pinocchio::computeAllTerms();
pinocchio::updateFramePlacements();
pinocchio:: forwardKinematics();

if I do the opposite, alocal_ becomes zero

pinocchio:: forwardKinematics();
pinocchio::computeAllTerms();
pinocchio::updateFramePlacements();

I will have to dig deeper into this, I still don't have an answer.

Ok, I'll also take a look to see if I can help with this. At the moment, this doesn't ring a bell.

from consim.

Regarding the QP (Eq. 28 in the paper), you don't need the constraint on the normal forces being positive because the other 4 constraints on the tangential forces, when put together, automatically admit only solutions with positive normal forces.

from consim.

Regarding the computation of pinocchio, computeAllTerms is equivalent to calling:

            pinocchio::forwardKinematics
            pinocchio::crba
            pinocchio::nonLinearEffects
            pinocchio::computeJointJacobians
            pinocchio::centerOfMass
            pinocchio::jacobianCenterOfMass
            pinocchio::kineticEnergy
            pinocchio::potentialEnergy

This is too much for our needs, so we call only the functions we need, including those for the frame kinematics and the computation of dJv:

        pin.forwardKinematics(self.model, self.data, q, v, np.zeros(self.model.nv))
        pin.computeJointJacobians(self.model, self.data)
        pin.updateFramePlacements(self.model, self.data)
        pin.crba(self.model, self.data, q)
        pin.nonLinearEffects(self.model, self.data, q, v)

from consim.

This issue seems to be solved now, including the computation of dJv (how did you solve it by the way?), so I think we can close this.

from consim.

i switched back to second order forward kinematics with zero acceleration. Yes it is solved, I will close it

from consim.