shivesh1210 / 2nd_order_closed_form_time_derivatives_eom Goto Github PK

I feel very very sorry to contact with you about one hard issue I encountered by this way. I have read the paper " On the closed form computation of the dynamic matrices and their differentiations" . I almost spent my entire holiday to implement these algorithm equations eq1 ~ eq32, I almost get all passed except eq24b.

my python code results showed that $$ \dot C \neq \frac{\partial C} {\partial q} \dot q + \frac{\partial C}{\partial\dot q} \ddot q$$ but when I set velcotity of all joints zeros , $ \dot C = \frac{\partial C} {\partial q} \dot q + \frac{\partial C}{\partial\dot q} \ddot q$

therefore I think only eq24b i.e. $\frac{\partial C} {\partial q}$ computed errorly, but I do cannot find the mistake

I have checked the following equations from python result all correct, I think all equations eq1 ~ eq32 computed correctly except eq24b

$$ \dot M = \frac{\partial C} {\partial q} \dot q $$

$ \dot G = \frac{\partial G} {\partial q} \dot q $

$ \dot J = \frac{\partial J} {\partial q} \dot q $

$ \ddot J = \frac{\partial J} {\partial q} \ddot q + \frac{\partial \dot J} {\partial q} \dot q$

$ \dot M = C + C^T$

$M \ddot q + C \dot q + g = Y \pi$

Could you give me some advice and help?