diff --git a/examples/force_control/Force_control_example.py b/examples/force_control/Force_control_example.py index e686c79..04bc766 100644 --- a/examples/force_control/Force_control_example.py +++ b/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() diff --git a/examples/force_control/plotting_ee_FT.py b/examples/force_control/plotting_ee_FT.py new file mode 100644 index 0000000..1fb5cf4 --- /dev/null +++ b/examples/force_control/plotting_ee_FT.py @@ -0,0 +1,216 @@ +#!/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): + """ + 初始化实时绘制的机械臂的相关配置 + + 参数： + robot: 所创造的机械臂实体 + force: 如果为真则绘制力信息 + torque: 如果为真则绘制力矩信息 + num_point: 保持多少个点在实时的绘制坐标系下 + xlims: x轴的限制 + ylims: y轴的限制 + suptitle: 图的标题 + ticks: 采样实时点的时间步间隔 + blit: 如果为真只更新数据内容不会改变标注等内容 + interval: 在不同frame之间的延迟单位mm + """ + # 设置机器人实例 + 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'] + # 设置图像布局 + nrows, ncols = 1, 1 + + # 设置我们所需要绘制的参数 + 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 + + # 设置点 + self._num_points = num_point if num_point > 10 else 10 + + # 检查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): + """ + 初始化图像在每个轴下创造线 + :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