Fixed the wild movement issue - Need to set the joint angles close to current angles before using trajopt.