bug in c++ exponential simulator

[hammoudbilal]

hi @andreadelprete

I finally managed to pin down the source of part of the code with the bug but i still can't figure out what is causing it, so maybe you could take a look ?

the bug appears at any change in the number of active contacts, so I turned to printing things at contact switches, I start by tracking change in number of active contacts here.

I print each active contact position, velocity and force here, and up till this point things still match the python code,

then I try to populate the jacobian matrix here , at this part of the code, there seems to be a miss match of which contacts are active, this prints out the following results

0.251 Number of active contacts changed from 4 to 3 this is the print from computeContactForces() HL_FOOT p [ -0.195408 0.145106 -6.2304e-05] v [ 0.0227138 -0.00558171 0.0454345] f [0 0 0] HR_FOOT p [ -0.19545 -0.145132 -0.000144996] v [-0.0180531 -0.0047408 -0.0212274] f [6.69764 3.07576 15.6711] FL_FOOT p [ 0.195432 0.145123 -0.000199007] v [ 0.0124034 0.00010649 -0.017879] f [-4.37274 -1.8537 15.3091]

this is the print from computeIntegrationTerms(), only two active contacts, even though Jc_ is resized with the correct dimensions, i.e the last three rows remain zero jacobian of contact HR_FOOT 1 8.2631e-05 -0.000400766 7.73157e-05 -0.205176 0.150616 0 0 0 0 0 0 0 0 0 3.53956e-05 -0.20523 -0.0915469 -8.21861e-05 0.999999 0.00110999 0.204925 0.000248528 -0.208728 0 0 0 0 0 0 0 0 0 0.205041 9.08537e-05 0.000526424 0.000400858 -0.00110996 0.999999 -0.150861 0.208633 0.000292048 0 0 0 0 0 0 0 0 0 -0.0462609 0.0186329 0.131221 jacobian of contact FL_FOOT 1 8.2631e-05 -0.000400766 -3.90305e-05 -0.204908 -0.139638 2.88968e-06 -0.202932 -0.106201 0 0 0 0 0 0 0 0 0 -8.21861e-05 0.999999 0.00110999 0.205136 -0.000185331 0.182155 0.20502 0.000425632 -0.00621826 0 0 0 0 0 0 0 0 0 0.000400858 -0.00110996 0.999999 0.139425 -0.182225 -0.000258153 0.0348255 0.00776463 -0.11951 0 0 0 0 0 0 0 0 0

you can see that computeContactForce() can see all three active contacts where as the function right after it, computeIntegrationTerms() seems to only see two active contacts, I still can't pinpoint what is changing the status of one of the active contacts, could you please take a look ?

[hammoudbilal]

the bug was here, contactModel.computeContactForce() was changing the active status after constructing the matrices, resulting in zero elements in some case, problem solved by commenting the line. another option would be changing the status and reducing the number of active contacts. Maybe leave it open for now to see it brings any accuracy/speedup improvements later ?

[andreadelprete]

Sorry I introduced this bug. Indeed I had some doubts it was correct to do that because the activation/deactivation of contacts should only occur at the collision-checking phase. I think it's better to leave it as you did. So now you get same results with python and c++ simulator for quadrupedal trotting?

[hammoudbilal]

yes everything works fine now,

I will leave the printing at contact each switch in the code for now and close this issue