diff --git a/mujoco_py/mjpid.pyx b/mujoco_py/mjpid.pyx index aee279dcf6dda236518d30d510b29eb39f1490c0..27e5723a01c30b99a92b1427367b793ea9d693ce 100644 --- a/mujoco_py/mjpid.pyx +++ b/mujoco_py/mjpid.pyx @@ -118,7 +118,7 @@ cdef mjtNum c_pid_bias(const mjModel*m, const mjData*d, int id): cdef double effort_limit_low = m.actuator_forcerange[id * 2] cdef double effort_limit_high = m.actuator_forcerange[id * 2 + 1] - + if effort_limit_low != 0.0 or effort_limit_high != 0.0: f = fmax(effort_limit_low, fmin(effort_limit_high, f)) return f @@ -169,13 +169,18 @@ cdef mjtNum c_pi_cascade_bias(const mjModel*m, const mjData*d, int id): """ cdef double dt_in_sec = m.opt.timestep cdef int NGAIN = int(const.NGAIN) + cdef double smooth_pos_setpoint; cdef double Kp_cas = m.actuator_gainprm[id * NGAIN + IDX_CAS_PROPORTIONAL_GAIN] - # Apply Exponential Moving Average smoothing to the position setpoint - ctrl_ema = d.userdata[id * NUM_USER_DATA_PER_ACT + IDX_CAS_STORED_EMA_SMOOTH] - smoothing_factor = m.actuator_gainprm[id * NGAIN + IDX_CAS_EMA_SMOOTH] - smooth_pos_setpoint = (smoothing_factor * ctrl_ema) + (1 - smoothing_factor) * d.ctrl[id] + # Apply Exponential Moving Average smoothing to the position setpoint, applying warmstart + # on the first iteration. + if d.time > 0.0: + ctrl_ema = d.userdata[id * NUM_USER_DATA_PER_ACT + IDX_CAS_STORED_EMA_SMOOTH] + smoothing_factor = m.actuator_gainprm[id * NGAIN + IDX_CAS_EMA_SMOOTH] + smooth_pos_setpoint = (smoothing_factor * ctrl_ema) + (1 - smoothing_factor) * d.ctrl[id] + else: + smooth_pos_setpoint = d.ctrl[id] d.userdata[id * NUM_USER_DATA_PER_ACT + IDX_CAS_STORED_EMA_SMOOTH] = smooth_pos_setpoint if Kp_cas != 0: diff --git a/mujoco_py/tests/test_pid.py b/mujoco_py/tests/test_pid.py index 21b6d4f0dfba6778c3787c2c66f7cdfe5af7ae7a..48636cd1b2f295b7c4eee499ba447d6012c2617d 100644 --- a/mujoco_py/tests/test_pid.py +++ b/mujoco_py/tests/test_pid.py @@ -143,7 +143,8 @@ def test_cascaded_pipi(): print('final pos', sim.data.qpos[0]) assert abs(sim.data.qpos[0] - desired_pos) < 1e-3 assert max_torque <= 3 # Torque limit set on the actuator - + + def test_cascaded_pdpi(): """ This tests using a PD-PI loop with the cascaded controller. This is achieved by setting the @@ -158,7 +159,7 @@ def test_cascaded_pdpi(): model = load_model_from_xml(xml) sim = MjSim(model) cymj.set_pid_control(sim.model, sim.data) - + """ sim2 uses the same Kp gain on the position loop as sim but does not have any derivative gain. It is expected that given this Kp gain and no derivative gain, the system will be unstable @@ -185,10 +186,10 @@ def test_cascaded_pdpi(): sim2.step() if abs(sim.data.actuator_force[0]) > max_torque: max_torque = abs(sim.data.actuator_force[0]) - + print('final pos', sim.data.qpos[0]) - assert abs(sim2.data.qpos[0] - desired_pos) > 1e-3 # Verify instability without D term - assert abs(sim.data.qpos[0] - desired_pos) < 1e-3 # Verify stability with D term + assert abs(sim2.data.qpos[0] - desired_pos) > 1e-3 # Verify instability without D term + assert abs(sim.data.qpos[0] - desired_pos) < 1e-3 # Verify stability with D term assert max_torque <= 3 # Torque limit set on the actuator @@ -199,3 +200,30 @@ def test_mjsize_exception(): sim = MjSim(model) with pytest.raises(Exception): cymj.set_pid_control(sim.model, sim.data) + + +CASCADED_PDPI_ACTUATOR_NO_EMA = """ + <general ctrlrange='-1 1' gaintype="user" biastype="user" forcerange="-3 3" gainprm="10 0 0 .1 1 10 .1 1.5 0 3" joint="hinge" name="a-hinge" user="1"/> +""" + + +def test_warm_start_ema(): + """ + Test that the smoothed commanded position is initialized to the commanded position at + start. + """ + # Load model with EMA smoothing (0.97) + model = load_model_from_xml(MODEL_XML.format(actuator=CASCADED_PDPI_ACTUATOR, nuser_actuator=1)) + sim = MjSim(model) + cymj.set_pid_control(sim.model, sim.data) + # Load model without EMA smoothing (0.0) + model2 = load_model_from_xml(MODEL_XML.format(actuator=CASCADED_PDPI_ACTUATOR_NO_EMA, nuser_actuator=1)) + sim2 = MjSim(model2) + cymj.set_pid_control(sim2.model, sim2.data) + init_pos = 0.1 * (random.random() - 0.5) + sim.data.ctrl[0] = sim2.data.ctrl[0] = .2 + sim.step() + sim2.step() + EMA_SMOOTH_IDX = 4 + # Compare that the saved smoothed position is the same for both EMA=.97 and EMA=0 + assert abs(sim.data.userdata[EMA_SMOOTH_IDX] - sim2.data.userdata[EMA_SMOOTH_IDX]) < 1e-10 diff --git a/mujoco_py/version.py b/mujoco_py/version.py index 0371ae31a31fae9d663147e0ddf303ebb8dc0b2c..14bbbf9dce296e82ffd30e01da128f84874c8fd6 100644 --- a/mujoco_py/version.py +++ b/mujoco_py/version.py @@ -1,6 +1,6 @@ __all__ = ['__version__', 'get_version'] -version_info = (2, 0, 2, 12) +version_info = (2, 0, 2, 13) # format: # ('mujoco_major', 'mujoco_minor', 'mujoco_py_major', 'mujoco_py_minor')