From 84c712b7d53b5a69bb85aad7da80afedc146f1bd Mon Sep 17 00:00:00 2001 From: tiboy Date: Sun, 5 Apr 2020 23:06:09 +0800 Subject: [PATCH 1/7] force-control --- examples/force_control/Force_Control.py | 237 ++++++++++++++++++++++++ 1 file changed, 237 insertions(+) create mode 100644 examples/force_control/Force_Control.py diff --git a/examples/force_control/Force_Control.py b/examples/force_control/Force_Control.py new file mode 100644 index 0000000..19161b5 --- /dev/null +++ b/examples/force_control/Force_Control.py @@ -0,0 +1,237 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Force control with the Kuka robot where the goal is to follow a moving sphere and contact with the table. +""" + +# Reference : +# [1] A Tutorial Survey and Comparison of Impedance Control on Robotic Manipulation + + +# this is for my pc because some setting issues +import sys +sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') + +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 * + +import matplotlib.pyplot as plt + + +# The sphere is used to visualize the reference trajectory, So I creat the sphere trajectory as the reference +def manipulation(world, robot, sphere, FT_sensor): + + # First step is to arrive the initial position + for t in count(): + # move sphere + sphere.position = np.array([0.33, 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) + # after 300 steps, continue to next phase + if t > 300: + break + # step in simulation + world.step(sleep_dt=dt) + + # this process is to approach to the face of table + for t in count(): + Fz_desired = 10 + # move sphere + sphere.position = np.array([0.33, 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: + # condition to the next phase + if FT_sensor.sense()[2] > Fz_desired: + break + # step in simulation + world.step(sleep_dt=dt) + # initial some necessary parameters + Fz_error_old = 0 # used in PI + sp_z = [] + num = [] # used to plot the figure + force_z = [] # store the current force + force_z_desired = [] # store the desired force + + if flag == 1: + detx = np.array([0.0, 0.0, 0.0]) + + # force control phase + for t in count(): + Fz_desired = 100 # set the desire force 100N + # move sphere + + if t == 0: + z = robot.get_link_world_positions(link_id)[2] + sphere.position = np.array([0.38 - r * np.sin(w * t + np.pi / 2), r * np.cos(w * t + np.pi / 2), z]) + # zz = z - 0.002 # Try to make the end-effector touch the surface of the table + + # 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 + force_z.append(Fz_current) + force_z_desired.append(Fz_desired) + Fz_error = Fz_current - Fz_desired # record the current error + + # flag == 0 PI(force feedback to adjust x) + # flag ==1 (admittance control) + if flag == 0: + Fz_error_integral = Fz_error + Fz_error_old + zzz = sphere.position[2] + 0.0000001 * Fz_error + 0.000002 * Fz_error_integral + Fz_error_old = Fz_error # record the current error as the old error + + elif flag == 1: + # the equation is demonstrated in reference [1] eq(33) + M = 1 + D = 2000 + K = 800000 + 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 + detx[2] = detx[1] + detx[1] = detx[0] + detx[0] = detx_ + zzz = sphere.position[2] + detx[0] + # keep a circle trajectory + sphere.position = np.array([0.38 - r * np.sin(w * t + np.pi / 2), r * np.cos(w * t + np.pi / 2), zzz]) + dv = kp * (sphere.position - x) - kd * dx # compute the other direction tracking error term + + 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) + plt.plot(num, force_z, 'b') + plt.plot(num, force_z_desired, '--r') + 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.05 + + # 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) + + manipulation(world, robot, sphere, FT_sensor) \ No newline at end of file From d9c30dff5e1549f2a6dac0d697339757020ae2b7 Mon Sep 17 00:00:00 2001 From: tiboy Date: Sun, 19 Apr 2020 20:37:15 +0800 Subject: [PATCH 2/7] add proper parameter --- ...=> Force_Control_not_proper_parameters.py} | 0 .../force_control/Force_control_example.py | 256 ++++++++++++++++++ 2 files changed, 256 insertions(+) rename examples/force_control/{Force_Control.py => Force_Control_not_proper_parameters.py} (100%) create mode 100644 examples/force_control/Force_control_example.py diff --git a/examples/force_control/Force_Control.py b/examples/force_control/Force_Control_not_proper_parameters.py similarity index 100% rename from examples/force_control/Force_Control.py rename to examples/force_control/Force_Control_not_proper_parameters.py diff --git a/examples/force_control/Force_control_example.py b/examples/force_control/Force_control_example.py new file mode 100644 index 0000000..823ab10 --- /dev/null +++ b/examples/force_control/Force_control_example.py @@ -0,0 +1,256 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Inverse kinematics with the Kuka robot where the goal is to follow a moving sphere. +""" +import sys +sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') +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 simulate_test_ur.plotting_ee_FT 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) + + for t in count(): + Fz_desired = 10 + # 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) + Fz_error_old = 0 + sp_z = [] + num = [] + if flag == 1: + detx = np.array([0.0, 0.0, 0.0]) + for t in count(): + Fz_desired = 100 + # 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]) + # zz = z - 0.002 # Try to make the end-effector touch the surface of the table + + # 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 + # dv[2] = dv[2] + 0.00016 * Fz_error + 0.0000008 * (Fz_error - Fz_error_old) / dt # 结果较好的dt=2400 + # dv[2] = 0.0013 * Fz_error + 0.0000020 * (Fz_error - Fz_error_old) / dt # 结果较好的dt=2400 + # dv[2] = 0.00093 * Fz_error + 0.000060 * (Fz_error - Fz_error_old) / dt + # dv[2] = 0.0052 * Fz_error + # sphere.position[2] = sphere.position[2] + 0.00095 * Fz_error + 0.000060 * (Fz_error - Fz_error_old) / dt + if flag == 0: + Fz_error_integral = Fz_error + Fz_error_old + zzz = sphere.position[2] + 0.000001 * Fz_error + 0.000002 * Fz_error_integral + Fz_error_old = Fz_error # record the current error as the old error + elif flag == 1: + # xyz 3 direction impedance control + # M = np.array([[50, 0, 0], [0, 50, 0], [0, 0, 50]]) + # D = np.array([[10, 0, 0], [0, 10, 0], [0, 0, 10]]) + # K = np.array([[20, 0, 0], [0, 20, 0], [0, 0, 20]]) + # numerator = np.array([[Fz_error[0], 0, 0], [0, Fz_error[1], 0], [0, 0, Fz_error[2]]]) * np.square(dt) \ + # + D * dt * dx[:, 1] + M * (2 * dx[:, 1] - dx[:, 2]) + # denominator = M + D*dt + K*np.square(dt) + # dx_ = numerator * np.linalg.inv(denominator) + # dx[:, 2] = dx[:, 1] + # dx[:, 1] = dx[:, 0] + # dx[:, 0] = np.array([dx_[0, 0], dx_[1, 1], dx_[2, 2]]) + + M = 1 + D = 9500 + K = 500000 + 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] + + sphere.position = np.array([0.46 - r * np.sin(w * t + np.pi / 2), 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) + plt.show() + + + +if __name__=='__main__': + # Create simulator + sim = Bullet() + + # create world + world = BasicWorld(sim) + + # flag : 0 # PI control + flag = 1 + # 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() + From 7753316c1d9077fb167bbe144315e958701f1180 Mon Sep 17 00:00:00 2001 From: tiboy Date: Sat, 30 May 2020 21:27:11 +0800 Subject: [PATCH 3/7] delete the useless file --- .../Force_Control_not_proper_parameters.py | 237 ------------------ 1 file changed, 237 deletions(-) delete mode 100644 examples/force_control/Force_Control_not_proper_parameters.py diff --git a/examples/force_control/Force_Control_not_proper_parameters.py b/examples/force_control/Force_Control_not_proper_parameters.py deleted file mode 100644 index 19161b5..0000000 --- a/examples/force_control/Force_Control_not_proper_parameters.py +++ /dev/null @@ -1,237 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -"""Force control with the Kuka robot where the goal is to follow a moving sphere and contact with the table. -""" - -# Reference : -# [1] A Tutorial Survey and Comparison of Impedance Control on Robotic Manipulation - - -# this is for my pc because some setting issues -import sys -sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') - -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 * - -import matplotlib.pyplot as plt - - -# The sphere is used to visualize the reference trajectory, So I creat the sphere trajectory as the reference -def manipulation(world, robot, sphere, FT_sensor): - - # First step is to arrive the initial position - for t in count(): - # move sphere - sphere.position = np.array([0.33, 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) - # after 300 steps, continue to next phase - if t > 300: - break - # step in simulation - world.step(sleep_dt=dt) - - # this process is to approach to the face of table - for t in count(): - Fz_desired = 10 - # move sphere - sphere.position = np.array([0.33, 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: - # condition to the next phase - if FT_sensor.sense()[2] > Fz_desired: - break - # step in simulation - world.step(sleep_dt=dt) - # initial some necessary parameters - Fz_error_old = 0 # used in PI - sp_z = [] - num = [] # used to plot the figure - force_z = [] # store the current force - force_z_desired = [] # store the desired force - - if flag == 1: - detx = np.array([0.0, 0.0, 0.0]) - - # force control phase - for t in count(): - Fz_desired = 100 # set the desire force 100N - # move sphere - - if t == 0: - z = robot.get_link_world_positions(link_id)[2] - sphere.position = np.array([0.38 - r * np.sin(w * t + np.pi / 2), r * np.cos(w * t + np.pi / 2), z]) - # zz = z - 0.002 # Try to make the end-effector touch the surface of the table - - # 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 - force_z.append(Fz_current) - force_z_desired.append(Fz_desired) - Fz_error = Fz_current - Fz_desired # record the current error - - # flag == 0 PI(force feedback to adjust x) - # flag ==1 (admittance control) - if flag == 0: - Fz_error_integral = Fz_error + Fz_error_old - zzz = sphere.position[2] + 0.0000001 * Fz_error + 0.000002 * Fz_error_integral - Fz_error_old = Fz_error # record the current error as the old error - - elif flag == 1: - # the equation is demonstrated in reference [1] eq(33) - M = 1 - D = 2000 - K = 800000 - 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 - detx[2] = detx[1] - detx[1] = detx[0] - detx[0] = detx_ - zzz = sphere.position[2] + detx[0] - # keep a circle trajectory - sphere.position = np.array([0.38 - r * np.sin(w * t + np.pi / 2), r * np.cos(w * t + np.pi / 2), zzz]) - dv = kp * (sphere.position - x) - kd * dx # compute the other direction tracking error term - - 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) - plt.plot(num, force_z, 'b') - plt.plot(num, force_z_desired, '--r') - 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.05 - - # 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) - - manipulation(world, robot, sphere, FT_sensor) \ No newline at end of file From 09709f3120d7c7316fe08f47ec3eeefabe4d2af5 Mon Sep 17 00:00:00 2001 From: tiboy Date: Thu, 4 Jun 2020 22:29:59 +0800 Subject: [PATCH 4/7] add some necessary comments --- .../force_control/Force_control_example.py | 58 ++++++------------- 1 file changed, 19 insertions(+), 39 deletions(-) diff --git a/examples/force_control/Force_control_example.py b/examples/force_control/Force_control_example.py index 823ab10..e686c79 100644 --- a/examples/force_control/Force_control_example.py +++ b/examples/force_control/Force_control_example.py @@ -2,8 +2,7 @@ # -*- coding: utf-8 -*- """Inverse kinematics with the Kuka robot where the goal is to follow a moving sphere. """ -import sys -sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') + import numpy as np from itertools import count @@ -102,18 +101,15 @@ def manipulator_thread(world, robot, sphere, FT_sensor): break # step in simulation world.step(sleep_dt=dt) - Fz_error_old = 0 sp_z = [] num = [] - if flag == 1: - detx = np.array([0.0, 0.0, 0.0]) + detx = np.array([0.0, 0.0, 0.0]) for t in count(): Fz_desired = 100 # 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]) - # zz = z - 0.002 # Try to make the end-effector touch the surface of the table # get current end-effector position and velocity in the task/operational space x = robot.get_link_world_positions(link_id) @@ -136,41 +132,25 @@ def manipulator_thread(world, robot, sphere, FT_sensor): Fz_current = FT_sensor.sense()[2] # record the current Fz Fz_error = Fz_current - Fz_desired # record the current error - # dv[2] = dv[2] + 0.00016 * Fz_error + 0.0000008 * (Fz_error - Fz_error_old) / dt # 结果较好的dt=2400 - # dv[2] = 0.0013 * Fz_error + 0.0000020 * (Fz_error - Fz_error_old) / dt # 结果较好的dt=2400 - # dv[2] = 0.00093 * Fz_error + 0.000060 * (Fz_error - Fz_error_old) / dt - # dv[2] = 0.0052 * Fz_error - # sphere.position[2] = sphere.position[2] + 0.00095 * Fz_error + 0.000060 * (Fz_error - Fz_error_old) / dt - if flag == 0: - Fz_error_integral = Fz_error + Fz_error_old - zzz = sphere.position[2] + 0.000001 * Fz_error + 0.000002 * Fz_error_integral - Fz_error_old = Fz_error # record the current error as the old error - elif flag == 1: - # xyz 3 direction impedance control - # M = np.array([[50, 0, 0], [0, 50, 0], [0, 0, 50]]) - # D = np.array([[10, 0, 0], [0, 10, 0], [0, 0, 10]]) - # K = np.array([[20, 0, 0], [0, 20, 0], [0, 0, 20]]) - # numerator = np.array([[Fz_error[0], 0, 0], [0, Fz_error[1], 0], [0, 0, Fz_error[2]]]) * np.square(dt) \ - # + D * dt * dx[:, 1] + M * (2 * dx[:, 1] - dx[:, 2]) - # denominator = M + D*dt + K*np.square(dt) - # dx_ = numerator * np.linalg.inv(denominator) - # dx[:, 2] = dx[:, 1] - # dx[:, 1] = dx[:, 0] - # dx[:, 0] = np.array([dx_[0, 0], dx_[1, 1], dx_[2, 2]]) - M = 1 - D = 9500 - K = 500000 - 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_ + # 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. + # 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] + 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]) dv = kp * (sphere.position - x) - kd * dx # compute the other direction tracking error term @@ -204,7 +184,7 @@ if __name__=='__main__': world = BasicWorld(sim) # flag : 0 # PI control - flag = 1 + flag = 0 # create robot robot = KukaIIWA(sim) robot.print_info() From 6e696fe4477686540769c3ce97db4b3c641a4991 Mon Sep 17 00:00:00 2001 From: tiboy Date: Fri, 5 Jun 2020 21:58:44 +0800 Subject: [PATCH 5/7] improve the comment and add the end-effector F/T info plotting file --- .../force_control/Force_control_example.py | 42 ++-- examples/force_control/plotting_ee_FT.py | 216 ++++++++++++++++++ 2 files changed, 245 insertions(+), 13 deletions(-) create mode 100644 examples/force_control/plotting_ee_FT.py 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 From 755d898baa163c178cc3fc84bdda70130973a8e8 Mon Sep 17 00:00:00 2001 From: tiboy Date: Fri, 5 Jun 2020 22:29:13 +0800 Subject: [PATCH 6/7] improve the comment and add the end-effector F/T info plotting file --- examples/force_control/plotting_ee_FT.py | 35 +++++++++++------------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/examples/force_control/plotting_ee_FT.py b/examples/force_control/plotting_ee_FT.py index 1fb5cf4..4a0d6f7 100644 --- a/examples/force_control/plotting_ee_FT.py +++ b/examples/force_control/plotting_ee_FT.py @@ -21,21 +21,19 @@ class EeFtRealTimePlot(RealTimePlot): 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 - 参数： - robot: 所创造的机械臂实体 - force: 如果为真则绘制力信息 - torque: 如果为真则绘制力矩信息 - num_point: 保持多少个点在实时的绘制坐标系下 - xlims: x轴的限制 - ylims: y轴的限制 - suptitle: 图的标题 - ticks: 采样实时点的时间步间隔 - blit: 如果为真只更新数据内容不会改变标注等内容 - interval: 在不同frame之间的延迟单位mm + :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)) @@ -46,10 +44,10 @@ class EeFtRealTimePlot(RealTimePlot): 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) @@ -67,10 +65,10 @@ class EeFtRealTimePlot(RealTimePlot): else: ncols = 2 - # 设置点 + # set the point self._num_points = num_point if num_point > 10 else 10 - # 检查x和y的极限 + # check the limited of the x y if xlims is None: xlims = (0, self._num_points) if ylims is None: @@ -82,7 +80,7 @@ class EeFtRealTimePlot(RealTimePlot): def _init(self, axes): """ - 初始化图像在每个轴下创造线 + initialize the figure :param axes: :return: """ @@ -124,7 +122,6 @@ class EeFtRealTimePlot(RealTimePlot): 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 From 1a4eff2db58f0b7c18fb9fa126915529578a86f7 Mon Sep 17 00:00:00 2001 From: tiboy Date: Fri, 5 Jun 2020 23:17:52 +0800 Subject: [PATCH 7/7] move the end-effector F/T info plotting file to pyrobolearn utils --- examples/force_control/Force_control_example.py | 2 +- .../utils/plotting/end_effector_realtime_FT_plot.py | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename examples/force_control/plotting_ee_FT.py => pyrobolearn/utils/plotting/end_effector_realtime_FT_plot.py (100%) diff --git a/examples/force_control/Force_control_example.py b/examples/force_control/Force_control_example.py index 04bc766..cd6ec0e 100644 --- a/examples/force_control/Force_control_example.py +++ b/examples/force_control/Force_control_example.py @@ -16,7 +16,7 @@ from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KukaIIWA, Body, sensors from pyrobolearn.utils.transformation import * -from plotting_ee_FT import EeFtRealTimePlot +from pyrobolearn.utils.plotting.end_effector_realtime_FT_plot import EeFtRealTimePlot from threading import Thread diff --git a/examples/force_control/plotting_ee_FT.py b/pyrobolearn/utils/plotting/end_effector_realtime_FT_plot.py similarity index 100% rename from examples/force_control/plotting_ee_FT.py rename to pyrobolearn/utils/plotting/end_effector_realtime_FT_plot.py