And I dont understand how the joint is drived exactly.
get_dof_effort only returns effort = K_usd * (set_dof_position_target - init_position_usd) + D_usd * (set_dof_velocity_target - init_velocity_usd), where init_position_usd and init_velocity_usd is defined as joint parameters in usd file. This is not the real torque the joint should output.
Driving torque should be K_usd * (set_dof_position_target - position_now) + D_usd * (set_dof_velocity_target - velocity_now)
This would be tricky for applications involving contacts.
Will this be updated in future?