add ik q_solved refinement function, to make it as close as possible to last_q.

update the rm75_kine_rm.py to included a series of arm-angle for inverse kinematics calculation.

kine_ctrl/main.py

@@ -66,8 +66,6 @@ def main():
     print(f'pose = {pose}')
     print('-'*100)
 
-    time.sleep(5)
-
 
 
 
@@ -110,12 +108,9 @@ def main():
                 if d_p_ik < 0.01:
                     result[0][1] += 1
 
-                # while 1:
-                #     time.sleep(1)
-
-                robot_mjk.send_command(q)
-                robot_mjk.wait_until_reached()
-                robot_mjk.print_state()
+                # robot_mjk.send_command(q)
+                # robot_mjk.wait_until_reached()
+                # robot_mjk.print_state()
             else:
                 fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
                 d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
@@ -136,6 +131,7 @@ def main():
 
         print(f'results with qp and rm for ik are {result}')
         print(f'solve_sum is {solve_sum}')
+    robot_mjk.stop()
 
 
 def cal_pose_deviation(pose1, pose2):