Handle old joint angles not being set in joint_changes
Added mechanism to detect which joints have changed and only generate commands for changed joints