Class Fourier may be err when caculate dq about dvrk_dynamics_identification

[dbdxnuliba]

you can change the fourier.py ffile accorrding
to the following cpp

double position = traj->q0;
double velocity = 0;
double acceleration = 0;

for(int i = 0; i < traj->a.size(); i++)
{
    double phase = traj->w_f*(i+1)*t;
    double s = sin(phase);
    double c = cos(phase);

    position += traj->a[i]/(traj->w_f*(i+1))*s - traj->b[i]/(traj->w_f*(i+1))*c;
    velocity += traj->a[i]*c + traj->b[i]*s;
    acceleration += -traj->a[i]*(traj->w_f*(i+1))*s + traj->b[i]*(traj->w_f*(i+1))*c;
}

[wangyanhit]

hey, thanks for the question. I have never tested if the position, velocity, and acceleration are corresponding to each other. But here I got one example from my previous experiment for the MTM

I drew 4 time lines. At the first and second times, the position reaches a peak, and at the same time the velocity is around 0; Similarly, the others also make sense to me.
And from the definition of q, dq, and ddq, I don't see any problem. Can you double-check it for me?

[wangyanhit]

And even though the trajectory generation is not correct, it won't affect the identified parameters. It will only affect the quality of the generated trajectory.

[dbdxnuliba]

you can test
a demo
rbtdef = sympybotics.RobotDef("selfRobot_2dof",
[(0, 0, 0.3055, "q"),
("-pi/2", 0, 0.1925, "q")],
dh_convention="mdh")
#rbtdef.frictionmodel = {}
rbtdef.frictionmodel = {'Coulomb','viscous'}

from trajectory_optimization import TrajOptimizer
from trajectory_optimization import TrajPlotter
from numpy import deg2rad

base_freq = 0.05
fourier_order = 6

joint_constraints = []
cartesian_constraints = []

joint_constraints = [(1, deg2rad(-180), deg2rad(180), deg2rad(-120), deg2rad(120)),
(2, deg2rad(-170), deg2rad(-10), deg2rad(-120), deg2rad(120))]

traj_optimizer = TrajOptimizer(rbt, fourier_order, base_freq,
joint_constraints=joint_constraints,
cartesian_constraints = cartesian_constraints)

traj_optimizer.optimize()

then it produce q,dq,ddq curve
and trajectory csv file containing only q
when you caculate dq_diff by ( q_next -q_now)/0.02 in the csv file
you can get dq_diff
then when you plot dq_diff curve
you will find the dq_diff curve shape caculated in csv file will didn't agree with the script plot dq curve

I can send you the detailed content if you can show me your email.

[dbdxnuliba]

this is script result q, dq ,ddq curve

[dbdxnuliba]

this is q and dq(caculated by q's diff)

[dbdxnuliba]

we can find q curve is the same, but dq_curve caculated by diff method is not very same with dq curve the script caculated

[wangyanhit]

yes, please send the things to me through [email protected]. thanks

[wangyanhit]

And where is the csv file from? And can you plot those two velocities in the same figure? that will make it easier to see.

[dbdxnuliba]

this is the q curve and dq curve (dq_curve caculated by diff method)

[dbdxnuliba]

ok ,I see ,but the Hb_mtrix formula is so complex ,
so it's not coveninet to produce Hb_code to C++ code .
do you have convient method

[wangyanhit]

yes, that matrix is super complex. if you want to use it on a robot with less than 4 DoFs I guess it's fine. But if you want to use it on a robot with higher DoFs I highly recommend you use the recursive Newton-Euler method, which is more computationally efficient.

[wangyanhit]

Can you push your code with the two-link arm to your GitHub such that I can check the trajectory problem using your simple case?

[dbdxnuliba]

ok ,I use https://github.com/kiwiwan/ur10_dyn_ident script
to create 2dof robot , then I found the problem

[dbdxnuliba]

I has sent you an email
you can try test.
thanks a lot

[wangyanhit]

I find the problem. My code is correct. The problem is that when you use the difference between the next q and the current q to calculate the velocity there will be a phase lag by the time interval. Since in my original program I only sampled 12 points in one period for the highest-frequency component it becomes very prominent when you do this. I select a number 12 becuase when it is larger the H matrix will become larger and it will cost more time for the optimization. You can try to make the two curves closer by increasing the sample_point in traj_optimizer.py

this is the result I get when sample_point is 50. They are very close to each other now.

[wangyanhit]

main_two_link_robot.ipynb.zip
this is the file for a random 2-link robot I created.

[wangyanhit]

If you can do well in the dynamic model identification of UR10 I can put a link of your result in this repo such that more people may use it for different robots in the future.

[dbdxnuliba]

thanks ,the velocity at start time is not close to zero ,this may casue pound to arm ,why don't you constrain the vstart and vend close to zero

[wangyanhit]

I actually made the starting velocity zero here. you can modify it to make the ending velocity zero too.

[wangyanhit]

My robot is not very stiff so that it can bear some small impact. But for a general industrial arm with a large reduction ratio, I agree it is a good idea to make the velocity zero at the beginning and the end.

[dbdxnuliba]

your velocity result show some joint starting velocity is not zero

[wangyanhit]

I will do that when I run the robot. It's not in the ipynb file.

[dbdxnuliba]

ok ,and I find the result q_curve is different from each other between two times running the script
this is the first time's q_curve

this is the second time's q_curve

[dbdxnuliba]

so the result is not same each time run the script ,isn't it?

[wangyanhit]

so the result is not same each time run the script ,isn't it?

you are right. it's a nonlinear optimization, which doesn't ensure global optimal solution and relies on the initial guess. I think I used random initial guess.

[dbdxnuliba]

ok ,thanks a lot