Added first_ds_com_velocity option.
test_06_02: fixed upper body, short preview horizon.