mirror of
https://github.com/wassname/pyrobolearn.git
synced 2026-09-10 12:21:16 +08:00
Merge pull request #34 from TFLQW/master
force-control example with real-time plotting through threads.
This commit is contained in:
Leonel Rozo
@@ -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()
|
||||
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user