diff --git a/examples/force_control/Force_control_example.py b/examples/force_control/Force_control_example.py
new file mode 100644
index 0000000..cd6ec0e
--- /dev/null
+++ b/examples/force_control/Force_control_example.py
@@ -0,0 +1,252 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+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
+from itertools import count
+
+from pyrobolearn.simulators import Bullet
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import KukaIIWA, Body, sensors
+from pyrobolearn.utils.transformation import *
+
+from pyrobolearn.utils.plotting.end_effector_realtime_FT_plot import EeFtRealTimePlot
+
+from threading import Thread
+
+import matplotlib.pyplot as plt
+
+# Real-time plot the End-effector force and torque
+def plotting_thread(plot):
+    if not isinstance(plot, EeFtRealTimePlot):
+        raise TypeError("Expecting to plot type is CartesianRealTimePlot, not ""{}".format(plot))
+    while True:
+        plot.update()
+
+# 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
+    """
+    for t in count():
+        # move sphere
+        sphere.position = np.array([0.36, 0, 0.8])
+
+        # get current end-effector position and velocity in the task/operational space
+        x = robot.get_link_world_positions(link_id)
+        dx = robot.get_link_world_linear_velocities(link_id)
+        o = robot.get_link_world_orientations(link_id)
+        do = robot.get_link_world_angular_velocities(link_id)
+
+        # Get joint positions
+        q = robot.get_joint_positions()
+
+        # Get linear jacobian
+        if robot.has_floating_base():
+            J = robot.get_jacobian(link_id, q=q)[:, qIdx + 6]
+        else:
+            J = robot.get_jacobian(link_id, q=q)[:, qIdx]
+
+        # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
+        Jp = robot.get_damped_least_squares_inverse(J, damping)
+
+        dv = kp * (sphere.position - x) - kd * dx
+        dw = kp * quaternion_error(sphere.orientation, o) - kd * do
+        # evaluate damped-least-squares IK
+        dq = Jp.dot(np.hstack((dv, dw)))
+
+        # set joint positions
+        q = q[qIdx] + dq * dt
+        robot.set_joint_positions(q, joint_ids=joint_ids)
+        if t > 300:
+            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  # the threhold of the contact force with table
+        # move sphere
+        sphere.position = np.array([0.36, 0, 0.8-0.0005*t])
+
+        # get current end-effector position and velocity in the task/operational space
+        x = robot.get_link_world_positions(link_id)
+        dx = robot.get_link_world_linear_velocities(link_id)
+        o = robot.get_link_world_orientations(link_id)
+        do = robot.get_link_world_angular_velocities(link_id)
+
+        # Get joint positions
+        q = robot.get_joint_positions()
+
+        # Get linear jacobian
+        if robot.has_floating_base():
+            J = robot.get_jacobian(link_id, q=q)[:, qIdx + 6]
+        else:
+            J = robot.get_jacobian(link_id, q=q)[:, qIdx]
+
+        # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
+        Jp = robot.get_damped_least_squares_inverse(J, damping)
+
+        dv = kp * (sphere.position - x) - kd * dx
+        dw = kp * quaternion_error(sphere.orientation, o) - kd * do
+        # evaluate damped-least-squares IK
+        dq = Jp.dot(np.hstack((dv, dw)))
+
+        # set joint positions
+        # robot.set_joint_velocities(dq, joint_ids=joint_ids)
+        q = q[qIdx] + dq * dt
+        robot.set_joint_positions(q, joint_ids=joint_ids)
+
+        if FT_sensor.sense() is not None:
+            if FT_sensor.sense()[2] > Fz_desired:
+                break
+        # step in simulation
+        world.step(sleep_dt=dt)
+    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  # desired force
+        # move sphere
+        if t == 0:
+            z = robot.get_link_world_positions(link_id)[2]
+            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)
+        dx = robot.get_link_world_linear_velocities(link_id)
+        o = robot.get_link_world_orientations(link_id)
+        do = robot.get_link_world_angular_velocities(link_id)
+
+        # Get joint positions
+        q = robot.get_joint_positions()
+
+        # Get linear jacobian
+        if robot.has_floating_base():
+            J = robot.get_jacobian(link_id, q=q)[:, qIdx + 6]
+        else:
+            J = robot.get_jacobian(link_id, q=q)[:, qIdx]
+
+        # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
+        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 (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)
+        detx_ = numerator / denominator
+        print (detx_)
+        detx[2] = detx[1]
+        detx[1] = detx[0]
+        detx[0] = detx_
+
+        zzz = sphere.position[2] + detx[0]
+
+        # 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
+
+
+        sp_z.append(sphere.position[2])
+        num.append(t)
+
+        dw = kp * quaternion_error(sphere.orientation, o) - kd * do
+        # evaluate damped-least-squares IK
+        dq = Jp.dot(np.hstack((dv, dw)))
+
+        # set joint positions
+        q = q[qIdx] + dq * dt
+        robot.set_joint_positions(q, joint_ids=joint_ids)
+
+        # print(Fz_error, dv[2])
+        if t == 800:
+            break
+        # step in simulation
+        world.step(sleep_dt=dt)
+    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()
+
+
+
+if __name__=='__main__':
+    # Create simulator
+    sim = Bullet()
+
+    # create world
+    world = BasicWorld(sim)
+
+    # flag : 0 # PI control
+    flag = 0
+    # create robot
+    robot = KukaIIWA(sim)
+    robot.print_info()
+    world.load_robot(robot)
+    world.load_table(position=np.array([1, 0., 0.]), orientation=np.array([0.0, 0.0, 0.0, 1.0]))
+    # define useful variables for IK
+    dt = 1. / 240
+    link_id = robot.get_end_effector_ids(end_effector=0)
+    joint_ids = robot.joints  # actuated joint
+    damping = 0.01  # for damped-least-squares IK
+    wrt_link_id = -1  # robot.get_link_ids('iiwa_link_1')
+    qIdx = robot.get_q_indices(joint_ids)
+
+    # define gains
+    kp = 500  # 5 if velocity control, 50 if position control
+    kd = 5  # 2*np.sqrt(kp)
+
+    # create sphere to follow
+    sphere = world.load_visual_sphere(position=np.array([0.5, 0., 0.5]), radius=0.05, color=(1, 0, 0, 0.5))
+    sphere = Body(sim, body_id=sphere)
+
+    # set initial joint p
+    # ositions (based on the position of the sphere at [0.5, 0, 1])
+    robot.reset_joint_states(q=[8.84305270e-05, 7.11378917e-02, -1.68059886e-04, -9.71690439e-01, 1.68308810e-05,
+                                3.71467111e-01, 5.62890805e-05])
+
+    # define amplitude and angular velocity when moving the sphere
+    w = 0.01
+    r = 0.1
+
+    # I set the reference orientation to a constant
+    sphere.orientation = np.array([1, 0, 0, 0])
+
+    FT_sensor = sensors.JointForceTorqueSensor(sim, body_id=robot.id, joint_ids=6)
+    # The plotting handle
+    plot = EeFtRealTimePlot(robot, sensor=FT_sensor, forcex=True, forcey=True, forcez=True,
+                            torquex=True, torquey=True, torquez=True, num_point=1000, ticks=24)
+    # FT_ = np.zeros(6)
+
+    plot_t = Thread(target=plotting_thread, args=[plot], name='plotting task')
+    manipulator_t = Thread(target=manipulator_thread, args=(world, robot, sphere, FT_sensor), name='manipulator task')
+
+    thread_pools = [plot_t, manipulator_t]
+    for thread in thread_pools:
+        thread.start()
+
+    for thread in thread_pools:
+        thread.join()
+
diff --git a/pyrobolearn/utils/plotting/end_effector_realtime_FT_plot.py b/pyrobolearn/utils/plotting/end_effector_realtime_FT_plot.py
new file mode 100644
index 0000000..4a0d6f7
--- /dev/null
+++ b/pyrobolearn/utils/plotting/end_effector_realtime_FT_plot.py
@@ -0,0 +1,213 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+__author__ = "Boyang Ti"
+__copyright__ = "Copyright 2020, PyRoboLearn"
+__credits__ = ["Boyang Ti"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Boyang Ti"
+__email__ = "tiboyang@outlook.com"
+__status__ = "Development"
+
+from pyrobolearn.utils.plotting.plot import RealTimePlot
+import pyrobolearn as prl
+import numpy as np
+from pyrobolearn.utils.transformation import *
+from pyrobolearn.robots import Body
+
+class EeFtRealTimePlot(RealTimePlot):
+
+    def __init__(self, robot, sensor, forcex=False, forcey=False, forcez=False, torquex=False,
+                 torquey=False, torquez=False, num_point=100, xlims=None, ylims=None,
+                 suptitle='End effector Force and Torque', ticks=1, blit=True, interval=0.0001):
+        """
+        Initialize the configuration of the robot arm drawn in real time
+
+        :parameter：
+            robot: the
+            forcex, forcey,forcez: if is True plot the force information
+            torquex, torquey, torquez: if is True plot the torque information
+            num_point: the number of the points need to be obtained in the figure
+            xlims: the limited of x axis
+            ylims: the limited of y axis
+            suptitle: the title of the figure
+            ticks: Time step interval for sampling real-time points
+            blit: If it is true, only updating the data content will not change the label and other content
+        """
+        if not isinstance(robot, prl.robots.Robot):
+            raise TypeError("Expecting the given 'robot' to be an instance of `Robot`, but got instead: "
+                            "{}".format(robot))
+        if not isinstance(sensor, prl.robots.sensors.JointForceTorqueSensor):
+            raise TypeError("Expecting the given 'sensor' to be an instance of `sensor`, but got instead: "
+                            "{}".format(sensor))
+        self._robot = robot
+        self._sensor = sensor
+
+        self.axis_ids = ['Force', 'Torque']
+        # Set image layout
+        nrows, ncols = 1, 1
+
+        # Set the parameters we need to draw
+        self._plot_Fx = bool(forcex)
+        self._plot_Fy = bool(forcey)
+        self._plot_Fz = bool(forcez)
+        self._plot_Tx = bool(torquex)
+        self._plot_Ty = bool(torquey)
+        self._plot_Tz = bool(torquez)
+
+        states = np.array([self._plot_Fx, self._plot_Fy, self._plot_Fz, self._plot_Tx, self._plot_Ty, self._plot_Tz])
+        self._num_states = len(states[states])
+
+        if len(self.axis_ids) == 0:
+            raise ValueError("Expecting to plot at least something (force or torque)")
+        if len(self.axis_ids) == 1:
+            ncols = 1
+        else:
+            ncols = 2
+
+        # set the point
+        self._num_points = num_point if num_point > 10 else 10
+
+        # check the limited of the x y
+        if xlims is None:
+            xlims = (0, self._num_points)
+        if ylims is None:
+            ylims = (-2000, 2000)
+
+        super(EeFtRealTimePlot, self).__init__(nrows=nrows, ncols=ncols, xlims=xlims, ylims=ylims,
+                                                    titles=['Force', 'Torque'],
+                                                    suptitle=suptitle, ticks=ticks, blit=blit, interval=interval)
+
+    def _init(self, axes):
+        """
+        initialize the figure
+        :param axes:
+        :return:
+        """
+        self._lines = []
+        for i, axis_ids in enumerate(['Force', 'Torque']):
+            axes[0].legend(loc='upper left')
+            axes[1].legend(loc='upper left')
+            if self._plot_Fx:
+                line, = axes[0].plot([], [], lw=self._linewidths[i], color='r', label='Fx')
+                self._lines.append(line)
+            if self._plot_Fy:
+                line, = axes[0].plot([], [], lw=self._linewidths[i], color='y', label='Fy')
+                self._lines.append(line)
+            if self._plot_Fz:
+                line, = axes[0].plot([], [], lw=self._linewidths[i], color='g', label='Fz')
+                self._lines.append(line)
+            if self._plot_Tx:
+                line, = axes[1].plot([], [], lw=self._linewidths[i], color='m', label='Tx')
+                self._lines.append(line)
+            if self._plot_Ty:
+                line, = axes[1].plot([], [], lw=self._linewidths[i], color='k', label='Ty')
+                self._lines.append(line)
+            if self._plot_Tz:
+                line, = axes[1].plot([], [], lw=self._linewidths[i], color='b', label='Tz')
+                self._lines.append(line)
+        self._x = []
+        length = len(self.axis_ids) * self._num_states
+        self._ys = [[] for _ in range(length)]
+
+    def _init_anim(self):
+        """
+        Init function (plot the background of each frame) that is passed to FuncAnimation. This has to be
+        implemented in the child class.
+        :return:
+        """
+        for line in self._lines:
+            line.set_data([], [])
+        return self._lines
+
+    def _set_line(self, line_idx, data, state_name):
+        """
+        :param axis_idx: joint index
+        :param line_idx: line index
+        :param data: data sent through the pipe
+        :param state_name: name of the state; select from Fx, Fy, Fz, Tx, Ty, Tz
+        :return:
+        """
+        self._ys[line_idx].append(data[state_name])
+        self._ys[line_idx] = self._ys[line_idx][-self._num_points:]
+        self._lines[line_idx].set_data(self._x, self._ys[line_idx])
+        line_idx += 1
+        return line_idx
+
+    def _animate_data(self, i, data):
+        """
+        Animate function that is passed to FuncAnimation. This has to be implemented in the child class.
+        :param i: frame counter
+        :param data: data that has been received from the pipe
+        :return: list of object to update
+        """
+        if len(self._x) < self._num_points:
+            self._x = range(len(self._x) + 1)
+
+        k = 0
+        for j in range(len(self.axis_ids)):
+            if self._plot_Fx:
+                k = self._set_line(line_idx=k, data=data, state_name='Fx')
+            if self._plot_Fy:
+                k = self._set_line(line_idx=k, data=data, state_name='Fy')
+            if self._plot_Fz:
+                k = self._set_line(line_idx=k, data=data, state_name='Fz')
+            if self._plot_Tx:
+                k = self._set_line(line_idx=k, data=data, state_name='Tx')
+            if self._plot_Ty:
+                k = self._set_line(line_idx=k, data=data, state_name='Ty')
+            if self._plot_Tz:
+                k = self._set_line(line_idx=k, data=data, state_name='Tz')
+        return self._lines
+
+    def _update(self):
+        """
+        This return the next data to be plotted; this has to be implemented in the child class.
+        :return:data to be sent through the pipe and that have to be plotted. This will be given to `_animate_data`.
+        """
+        data = {}
+        if self._sensor.sense() is None:
+            data['Fx'] = 0
+            data['Fy'] = 0
+            data['Fz'] = 0
+            data['Tx'] = 0
+            data['Ty'] = 0
+            data['Tz'] = 0
+            return data
+        if self._plot_Fx:
+            data['Fx'] = self._sensor.sense()[0]
+        if self._plot_Fy:
+            data['Fy'] = self._sensor.sense()[1]
+        if self._plot_Fz:
+            data['Fz'] = self._sensor.sense()[2]
+        if self._plot_Tx:
+            data['Tx'] = self._sensor.sense()[3]
+        if self._plot_Ty:
+            data['Ty'] = self._sensor.sense()[4]
+        if self._plot_Tz:
+            data['Tz'] = self._sensor.sense()[5]
+        return data
+
+if __name__ == '__main__':
+    # Try to move the robot in the simulator
+    # WARNING: DON'T FORGET TO CLOSE FIRST THE FIGURE THEN THE SIMULATOR OTHERWISE YOU WILL HAVE THE PLOTTING PROCESS
+    # STILL RUNNING
+    from itertools import count
+
+
+
+    sim = prl.simulators.Bullet()
+    world = prl.worlds.BasicWorld(sim)
+    robot = world.load_robot('kuka_iiwa')
+
+    box = world.load_visual_box(position=[0.7, 0., 0.2], orientation=get_quaternion_from_rpy([0, 1.57, 0]),
+                                dimensions=(0.2, 0.2, 0.2))
+    box = Body(sim, body_id=box)
+
+    sensor = prl.robots.sensors.JointForceTorqueSensor(sim, body_id=robot.id, joint_ids=5)
+    plot = EeFtRealTimePlot(robot, sensor=sensor, forcex=True, forcey=True, forcez=True,
+                            torquex=True, torquey=True, torquez=True, ticks=24)
+
+    for t in count():
+        plot.update()
+        world.step(sim.dt)
\ No newline at end of file