update the mjc function names

kine_ctrl/main.py

@@ -109,7 +109,7 @@ def demo_position_control():
 
         solve_sum = 0
 
-        for i in range(2000):
+        for i in range(10):
             print(f'\n-------------- in i = {i} ----------------')
             joint_rand = np.random.uniform(ub/180*pi, lb/180*pi)
             print(f'the predefined joints are {joint_rand}')
@@ -146,6 +146,10 @@ def demo_position_control():
                 print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
                 if d_p_ik < 0.01:
                     result[0][1] += 1
+
+                robot_mjk.send_command(joint_solution)
+                robot_mjk.wait_until_reached()
+                robot_mjk.print_state()
             else:
                 fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)
                 fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0)