improve the comment and add the end-effector F/T info plotting file

examples/force_control/Force_control_example.py

@@ -1,6 +1,11 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
-"""Inverse kinematics with the Kuka robot where the goal is to follow a moving sphere.
+"""
+The task is to track the force along z axis (vertical to the table) by employing admittance control, meanwhile tracking
+a circle trajectory on the xy plane. And the end-effector's target position is visualized by a sphere.
+Reference:
+[1] SONG, Peng; YU, Yueqing; ZHANG, Xuping. A tutorial survey and comparison of impedance control on robotic manipulation
+. Robotica, 2019, 37.5: 801-836.
 """
 
 import numpy as np
@@ -11,7 +16,7 @@ from pyrobolearn.worlds import BasicWorld
 from pyrobolearn.robots import KukaIIWA, Body, sensors
 from pyrobolearn.utils.transformation import *
 
-from simulate_test_ur.plotting_ee_FT import EeFtRealTimePlot
+from plotting_ee_FT import EeFtRealTimePlot
 
 from threading import Thread
 
@@ -27,7 +32,9 @@ def plotting_thread(plot):
 # Manipulate the whole process
 # The sphere is used to visualize the reference trajectory, So I creat the sphere trajectory as the reference
 def manipulator_thread(world, robot, sphere, FT_sensor):
-    # First step is to arrive the initial position
+    """
+    First step: is to arrive the initial position
+    """
     for t in count():
         # move sphere
         sphere.position = np.array([0.36, 0, 0.8])
@@ -62,9 +69,12 @@ def manipulator_thread(world, robot, sphere, FT_sensor):
             break
         # step in simulation
         world.step(sleep_dt=dt)
-
+    """
+        Second step: From the initial pose, Move vertically downward 
+        until end-effector touches the desktop with a force of 10N 
+    """
     for t in count():
-        Fz_desired = 10
+        Fz_desired = 10  # the threhold of the contact force with table
         # move sphere
         sphere.position = np.array([0.36, 0, 0.8-0.0005*t])
 
@@ -104,12 +114,17 @@ def manipulator_thread(world, robot, sphere, FT_sensor):
     sp_z = []
     num = []
     detx = np.array([0.0, 0.0, 0.0])
+    """
+    Third step to keep the target force along z axis(vertical to the table), 
+    and complete circular motion trajectory on plane xy
+    """
+    circle_center = np.array([0.46, 0]) # the center of the trajectory
     for t in count():
-        Fz_desired = 100
+        Fz_desired = 100  # desired force
         # move sphere
         if t == 0:
             z = robot.get_link_world_positions(link_id)[2]
-            sphere.position = np.array([0.46 - r * np.sin(w * t + np.pi / 2), r * np.cos(w * t + np.pi / 2), z])
+            sphere.position = np.array([circle_center[0] - r * np.sin(w * t + np.pi / 2), circle_center[1] + r * np.cos(w * t + np.pi / 2), z])
 
         # get current end-effector position and velocity in the task/operational space
         x = robot.get_link_world_positions(link_id)
@@ -130,15 +145,13 @@ def manipulator_thread(world, robot, sphere, FT_sensor):
         Jp = robot.get_damped_least_squares_inverse(J, damping)
         # Apply the admittance control
         Fz_current = FT_sensor.sense()[2]  # record the current Fz
-
         Fz_error = Fz_current - Fz_desired  # record the current error
 
         # set the M\D\K parameters by heuristic method, these parameters may have a good result
         M = 1
         D = 9500
         K = 500000
-        # Refer the formula in this article
-        # [1] SONG, Peng; YU, Yueqing; ZHANG, Xuping. A tutorial survey and comparison of impedance control on robotic manipulation. Robotica, 2019, 37.5: 801-836.
+        # Refer the formula (33) in this article [1]
         # the formula is theta_x(k) = Fc(k)*Ts^2+Bd*Ts*theta_x(k-1)+Md*(2*theta_x(k-1)-theta_x(k-2))/(Md+Bd*Ts+Kd*Ts^2)
         numerator = Fz_error * np.square(dt) + D * dt * detx[1] + M * (2 * detx[1] - detx[2])
         denominator = M + D*dt + K*np.square(dt)
@@ -150,8 +163,8 @@ def manipulator_thread(world, robot, sphere, FT_sensor):
 
         zzz = sphere.position[2] + detx[0]
 
-        # (0.46,0) is the the centre of the circle trajectory on the table
-        sphere.position = np.array([0.46 - r * np.sin(w * t + np.pi / 2), r * np.cos(w * t + np.pi / 2), zzz])
+        # circle_center the the centre of the circle trajectory on the table
+        sphere.position = np.array([circle_center[0] - r * np.sin(w * t + np.pi / 2), circle_center[1] + r * np.cos(w * t + np.pi / 2), zzz])
         dv = kp * (sphere.position - x) - kd * dx  # compute the other direction tracking error term
 
 
@@ -171,7 +184,10 @@ def manipulator_thread(world, robot, sphere, FT_sensor):
             break
         # step in simulation
         world.step(sleep_dt=dt)
-    plt.plot(num, sp_z)
+    plt.plot(num, sp_z)  # plot the position on the z axis
+    plt.xlabel("timesteps")
+    plt.ylabel("vertical position")
+    plt.title("The z axis position during the task")
     plt.show()