Closed andreadelprete closed 4 years ago
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
.computeContactState
and computeContactForces
are always called one after the other, then it should be just one method.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!step
is weird and should be reversed, i.e. first collision detection, then compute forces, then compute accelerations and integratethanks for the tips, I will clean it up
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 ft 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 ?
ExponentialSimulator
differ a bit from EulerSimulator
, this was first discussed in #1 . Following your code, you computedJv_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.
* `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.
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.
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)
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
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.
i switched back to second order forward kinematics with zero acceleration. Yes it is solved, I will close it
Hi @hammoudbilal
as we already started discussing, the code could probably use some refactoring because at the moment it's a bit messy. I haven't had the time to think about it properly, but I have recently developed a simple simulator in Python for a class I am teaching. I've tried to make it as simple as possible so that student could understand it easily. I'm sharing it with you here. Maybe this code could be a starting point to think of the new code
If you prefer I can push it in the repo, together with a simple test and the additional code for visualization with gepetto-viewer. I don't know if all of this is necessary though if we wanna use it just as inspiration for the design.