Inverse Dynamics Controller #551
Conversation
…vs ID custom controllers based on the actuator_user field that is defined per actuator. `user="1"` would trigger activating an ID controller where the gainprm are interpreted as Kp, Td, ema_smoothing_term, respectively.
mujoco_py/mjpid.pyx
Outdated
| :param id: actuator ID | ||
| :return: Custom actuator force | ||
| """ | ||
| controller_type = m.actuator_user[id * m.nuser_actuator + IDX_CONTROLLER_TYPE] |
There was a problem hiding this comment.
you might want to add typecast to int, as mujoco default to float.
|
|
||
| # Set the target forward dyanmics | ||
| for i in range(m.nv): | ||
| d.qacc[i] = qacc_des[i] |
There was a problem hiding this comment.
this seems strange, as each joint (specified by id) will be called on this function, it will use mj_inverse for each joint independently, I guess mj_inverse is a global operation, it is in-efficient perhaps also introduce non-deterministic behavior ; also this controller has two input and single output for each joint, is it stable? we shall add more tests.
There was a problem hiding this comment.
Ideally we do just as you say and only call mj_inverse once. However, this would require us to know how many actuators are using InvDyn and call mj_inverse on the last joint. This might not be robust in a mixture of PID and InvDyn actuators.
With regards to the two inputs, are you talking about the PD terms on acceleration?
There was a problem hiding this comment.
yes, input is both pos error and velocity (0-v)
There was a problem hiding this comment.
how many inv_dyn joints do you plan to have?? it might worth making one joint special (e.g. added annotation into actuator_user), which responsible for doing this logic once for all joints once each step.
|
|
||
| # Set desired acceleration of all DoFs (model.nv) to zero except the target actuator [id] | ||
| qacc_des = np.zeros(m.nv) | ||
| qacc_des[id] = kp * qpos_error + kd * qvel_error |
There was a problem hiding this comment.
this assumes every joint has 1DoF, we shall add assert at least. e.g. m.nu == m.nv
|
|
||
| # Set the target forward dyanmics | ||
| for i in range(m.nv): | ||
| d.qacc[i] = qacc_des[i] |
There was a problem hiding this comment.
how many inv_dyn joints do you plan to have?? it might worth making one joint special (e.g. added annotation into actuator_user), which responsible for doing this logic once for all joints once each step.
|
Closing in favor of #554 |
Introduce an inverse dynamics (ID) controller. We select between PID vs ID custom controllers based on the actuator_user field that is defined per actuator.
user="1"would trigger activating an ID controller where the gainprm are interpreted as Kp, Td, ema_smoothing_term, respectively.