Hi, I would like to control robots in PyBullet using trajectory optimization. I need to get dynamics constraint violation (e.x. x_t + dt * x_t_dot = x_tp1, where x_t_dot = f(x_t, u_t)) between consecutive timesteps? Is there another way than calling resetJointState(x_t), applying u_t to motors and stepping the simulation?

Secondly, if I optimize for state trajectory only, I would like to get control input from inverse dynamics model. Is it ok to take x_t, x_tp1 and calculate required acceleration that would produce x_tp1 from x_t and put it in calculateInverseDynamics? Is it required to do torque control therefore?

Thanks,

Marek