Progress on ArmControl. Try to see if can use trajopt in loop