move_to_pose.py result not consistent with book

**Describe the bug**
move_to_pose.py simulation result not consistent with book.
The trajectory is obviously not consistent with the book, fig. 4.14

**Expected behavior**
The trajectory should at least look similar to book's simulation result.

**Screenshots**
1. Books' simulation result

2. code simulation result(with same param kpho=3, kalpha=8, kbeta=3)
Note that I have already consider the case that `kbeta_book = -kbeta_code`