This is quite visible with PSM-S but likely happens as well with PSM Classic. This is due to the coupling matrix change. After the matrix is changed, the PID resets setpoint_js based on measured_js that comes from IO to get a reasonable estimate. PID should instead query the current matrix from IO, compute corresponding actuator setpoint_js (pre_coupling_setpoint_as = coupling.Inverse() * setpoint_js) and after coupling is changed, update the joint setpoint_js using setpoint_js = coupling * pre_coupling_setpoint_as. To do this, we need to add either a read command to get the coupling from IO or some qualified read commands to convert to/from actuators from IO.
adeguet1
commented
2 years ago
This is quite visible with PSM-S but likely happens as well with PSM Classic. This is due to the coupling matrix change. After the matrix is changed, the PID resets setpoint_js based on measured_js that comes from IO to get a reasonable estimate. PID should instead query the current matrix from IO, compute corresponding actuator setpoint_js (
pre_coupling_setpoint_as = coupling.Inverse() * setpoint_js) and after coupling is changed, update the jointsetpoint_jsusingsetpoint_js = coupling * pre_coupling_setpoint_as. To do this, we need to add either a read command to get the coupling from IO or some qualified read commands to convert to/from actuators from IO.