Hello!
I am attempting to implement admittance control with a control cycle of 0.005 seconds. To achieve this, I have set the simulation time step to 0.005 seconds and adopted a step mode for control execution. Within my control loop, I use an admittance controller to generate admittance acceleration ddxc. By integrating ddxc once, I obtain velocity dxc, and by integrating it again, I get position xc. I then convert xc into a pose matrix and send it to the target dummy of the robotic arm using sim.setObjectMatrix().
However, I've noticed that the tip dummy of the robotic arm never reaches the sent position (i.e., the calculated xc) within one simulation step. What could be causing this issue?
I can confirm that the admittance accelerations calculated are not large, and the deviations between xc and the current position are minimal. Below is some printed data from the system (motion is along the negative z-axis). xc represents the target pose vector obtained through the admittance controller, dxc is the velocity vector, x is the current pose vector, xc_last is the target pose vector from the previous control cycle, x_last is the pose vector from the previous control cycle, and dx is the current velocity obtained via sim.getVelocity(). Observing the z-axis information reveals that there's always a gap between x and xc_last at each control cycle, and there's also a discrepancy between dx and dxc:
--------------------------------------
xc: [ 7.50231503e-01 -1.41290445e-05 1.11154154e-01 -3.14134800e+00
3.12394872e-05 -9.15403495e-05]
dxc: [ 3.22697396e-05 -1.52505475e-06 -1.08291987e-02 -2.84268422e-05
-3.20273724e-04 -5.65828339e-06]
x: [ 7.50231342e-01 -1.41214192e-05 1.11208300e-01 -3.14134787e+00
3.11953634e-05 -8.90251101e-05]
xc_last: [ 7.50231319e-01 -1.41202292e-05 1.11158892e-01 -3.14134803e+00
3.12394897e-05 -9.14785261e-05]
x_last: [ 7.50231158e-01 -1.41126350e-05 1.11213038e-01 -3.14134787e+00
3.11848258e-05 -8.89629514e-05]
dx: [ 3.68000814e-05 -1.75684931e-06 -9.47655406e-04 4.77459869e-10
2.06352637e-09 -8.42936944e-06]
--------------------------------------
xc: [ 7.50231687e-01 -1.41378591e-05 1.11149415e-01 -3.14134801e+00
3.12603106e-05 -9.16025234e-05]
dxc: [ 3.22686409e-05 -1.52856156e-06 -1.08291975e-02 -2.79853245e-05
-3.20268847e-04 -7.03872015e-06]
x: [ 7.50231526e-01 -1.41302163e-05 1.11203561e-01 -3.14134787e+00
3.12053456e-05 -8.90873207e-05]
xc_last: [ 7.50231503e-01 -1.41290445e-05 1.11154154e-01 -3.14134800e+00
3.12394872e-05 -9.15403495e-05]
x_last: [ 7.50231342e-01 -1.41214192e-05 1.11208300e-01 -3.14134787e+00
3.11953634e-05 -8.90251101e-05]
dx: [ 3.67988440e-05 -1.75942623e-06 -9.47653954e-04 4.77793464e-10
2.06353341e-09 -8.42936944e-06]
--------------------------------------
xc: [ 7.50231871e-01 -1.41465773e-05 1.11144677e-01 -3.14134800e+00
3.12711559e-05 -9.16650552e-05]
dxc: [ 3.22675869e-05 -1.51897440e-06 -1.08291993e-02 -3.10662527e-05
-3.20314671e-04 -7.27212203e-06]
x: [ 7.50231710e-01 -1.41389824e-05 1.11198823e-01 -3.14134785e+00
3.12143586e-05 -8.91494920e-05]
xc_last: [ 7.50231687e-01 -1.41378591e-05 1.11149415e-01 -3.14134801e+00
3.12603106e-05 -9.16025234e-05]
x_last: [ 7.50231526e-01 -1.41302163e-05 1.11203561e-01 -3.14134787e+00
3.12053456e-05 -8.90873207e-05]
dx: [ 3.67977709e-05 -1.75321323e-06 -9.47656974e-04 4.78127337e-10
2.06352493e-09 -8.42936944e-06]