init_traj_distr setting problems when dX < 2 * dU

[dujinyu]

In this repo, it offers two methods to set the init_traj_distr, which are init_pd, init_lqr.

when dX < 2 * dU, errors occur. how to fix this bug?

# init_pd
 K = -config['pos_gains'] * np.tile(
            np.hstack([
                np.eye(dU) * Kp, np.eye(dU) * Kv,
                np.zeros((dU, dX - dU*2))
            ]), [T, 1, 1]
        )

# init_lqr
Fd = np.vstack([
        np.hstack([
            np.eye(dU), dt * np.eye(dU), np.zeros((dU, dX - dU*2)),
            dt ** 2 * np.diag(gains)
        ]),    # joint angles, joint_velocities, end-effector angles and velocities, action
        np.hstack([
            np.zeros((dU, dU)), np.eye(dU), np.zeros((dU, dX - dU*2)),
            dt * np.diag(gains)
        ]),
        np.zeros((dX - dU*2, dX+dU))
    ])
    fc = np.hstack([acc * dt ** 2, acc * dt, np.zeros((dX - dU*2))])