Comments (9)
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.
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 calleddetectContacts
.- If
computeContactState
andcomputeContactForces
are always called one after the other, then it should be just one method. - In
ExponentialSimulator
, the methodcomputeContactForces
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.
there are few issues remaining
-
computeContactForces
forEulerSimulator
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. forExponentialSimulator
,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-writecheckFrictionCone
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 fromEulerSimulator
, 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-writecheckFrictionCone
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.
Related Issues (20)
- First benchmark Exponential vs Euler HOT 9
- Bug in exponential integrator without double integral HOT 5
- Results with seldom updates of matrix exponential HOT 2
- gcc internal error segmentation fault
- Reset of contact state HOT 4
- New method for friction cone constraints HOT 1
- Sanity check: Sliding point mass HOT 3
- Integration accuracy without contacts HOT 7
- Accuracy of the Euler integration HOT 4
- Potential influcence of the cone projection HOT 2
- ExponentialSimulator accuracy with nonzero impact velocity HOT 5
- EulerSimulator discontinuity in friction force during sliding HOT 2
- bug in c++ exponential simulator HOT 3
- Quadruped Trot Results HOT 7
- Quadruped Jump Results HOT 3
- Romeo walking Results HOT 4
- Quadruped Squat Results
- missing reference trajectory HOT 1
- few issues with HalfPlane HOT 2
- Exponential VS Rigid-contacts HOT 9
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from consim.