fix some bugs when visualize optimized motions (still problem when init)