Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
389 lines (332 sloc) 14.6 KB
'''
Copyright (C) 2016 Travis DeWolf
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
'''
import numpy as np
import vrep
import ur5
# create instance of ur5 class which provides all
# the transform and Jacobian information for this arm
robot_config = ur5.robot_config()
# close any open connections
vrep.simxFinish(-1)
# Connect to the V-REP continuous server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)
try:
if clientID != -1: # if we connected successfully # noqa C901
print('Connected to remote API server')
# --------------------- Setup the simulation
vrep.simxSynchronous(clientID, True)
# joint target velocities discussed below
joint_target_velocities = np.ones(robot_config.num_joints) * 10000.0
# get the handles for each joint and set up streaming
joint_handles = [vrep.simxGetObjectHandle(
clientID,
name,
vrep.simx_opmode_blocking)[1] for name in robot_config.joint_names]
# get handle for target and set up streaming
_, target_handle = vrep.simxGetObjectHandle(
clientID,
'target',
vrep.simx_opmode_blocking)
# get handle for hand
_, hand_handle = vrep.simxGetObjectHandle(
clientID,
'hand',
vrep.simx_opmode_blocking)
# get handle for obstacle and set up streaming
_, obstacle_handle = vrep.simxGetObjectHandle(
clientID,
'obstacle',
vrep.simx_opmode_blocking)
# how close can we get to the obstacle?
threshold = .2 # distance in metres
obstacle_radius = .05
# define a set of targets
center = np.array([0, 0, 0.6])
dist = .2
num_targets = 10
target_positions = np.array([
[dist*np.cos(theta)*np.sin(theta),
dist*np.cos(theta),
dist*np.sin(theta)] +
center for theta in np.linspace(0, np.pi*2, num_targets)])
# define variables to share with nengo
q = np.zeros(len(joint_handles))
dq = np.zeros(len(joint_handles))
# --------------------- Start the simulation
dt = .001
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# start our simulation in lockstep with our code
vrep.simxStartSimulation(
clientID,
vrep.simx_opmode_blocking)
track_hand = []
track_target = []
track_obstacle = []
count = 0
target_index = 0
change_target_time = dt*1000
vmax = 0.5
kp = 200.0
kv = np.sqrt(kp)
# NOTE: main loop starts here -----------------------------------------
while count < len(target_positions) * change_target_time:
# every so often move the target
if (count % change_target_time) < dt:
vrep.simxSetObjectPosition(
clientID,
target_handle,
-1, # set absolute, not relative position
target_positions[target_index],
vrep.simx_opmode_blocking)
target_index += 1
# get the (x,y,z) position of the target
_, target_xyz = vrep.simxGetObjectPosition(
clientID,
target_handle,
-1, # retrieve absolute, not relative, position
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
track_target.append(np.copy(target_xyz)) # store for plotting
target_xyz = np.asarray(target_xyz)
for ii, joint_handle in enumerate(joint_handles):
old_q = np.copy(q)
# get the joint angles
_, q[ii] = vrep.simxGetJointPosition(
clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
# get the joint velocity
_, dq[ii] = vrep.simxGetObjectFloatParameter(
clientID,
joint_handle,
2012, # parameter ID for angular velocity of the joint
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
# calculate position of the end-effector
# derived in the ur5 calc_TnJ class
xyz = robot_config.Tx('EE', q)
# calculate the Jacobian for the end effector
JEE = robot_config.J('EE', q)
# calculate the inertia matrix in joint space
Mq = robot_config.Mq(q)
# calculate the effect of gravity in joint space
Mq_g = robot_config.Mq_g(q)
# convert the mass compensation into end effector space
Mx_inv = np.dot(JEE, np.dot(np.linalg.inv(Mq), JEE.T))
svd_u, svd_s, svd_v = np.linalg.svd(Mx_inv)
# cut off any singular values that could cause control problems
singularity_thresh = .00025
for i in range(len(svd_s)):
svd_s[i] = 0 if svd_s[i] < singularity_thresh else \
1./float(svd_s[i])
# numpy returns U,S,V.T, so have to transpose both here
Mx = np.dot(svd_v.T, np.dot(np.diag(svd_s), svd_u.T))
# calculate desired force in (x,y,z) space
dx = np.dot(JEE, dq)
# implement velocity limiting
lamb = kp / kv
x_tilde = xyz - target_xyz
sat = vmax / (lamb * np.abs(x_tilde))
scale = np.ones(3)
if np.any(sat < 1):
index = np.argmin(sat)
unclipped = kp * x_tilde[index]
clipped = kv * vmax * np.sign(x_tilde[index])
scale = np.ones(3) * clipped / unclipped
scale[index] = 1
u_xyz = -kv * (dx - np.clip(sat / scale, 0, 1) *
-lamb * scale * x_tilde)
u_xyz = np.dot(Mx, u_xyz)
# transform into joint space, add vel and gravity compensation
u = np.dot(JEE.T, u_xyz) - Mq_g
# calculate the null space filter
Jdyn_inv = np.dot(Mx, np.dot(JEE, np.linalg.inv(Mq)))
null_filter = (np.eye(robot_config.num_joints) -
np.dot(JEE.T, Jdyn_inv))
# calculate our secondary control signal
q_des = np.zeros(robot_config.num_joints)
dq_des = np.zeros(robot_config.num_joints)
# calculated desired joint angle acceleration using rest angles
for ii in range(1, robot_config.num_joints):
if robot_config.rest_angles[ii] is not None:
q_des[ii] = (
((robot_config.rest_angles[ii] - q[ii]) + np.pi) %
(np.pi*2) - np.pi)
dq_des[ii] = dq[ii]
# only compensate for velocity for joints with a control signal
nkp = kp * .1
nkv = np.sqrt(nkp)
u_null = np.dot(Mq, (nkp * q_des - nkv * dq_des))
u += np.dot(null_filter, u_null)
# get the (x,y,z) position of the center of the obstacle
_, v = vrep.simxGetObjectPosition(
clientID,
obstacle_handle,
-1, # retrieve absolute, not relative, position
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
track_obstacle.append(np.copy(v)) # store for plotting
v = np.asarray(v)
# find the closest point of each link to the obstacle
for ii in range(robot_config.num_joints):
# get the start and end-points of the arm segment
p1 = robot_config.Tx('joint%i' % ii, q=q)
if ii == robot_config.num_joints - 1:
p2 = robot_config.Tx('EE', q=q)
else:
p2 = robot_config.Tx('joint%i' % (ii + 1), q=q)
# calculate minimum distance from arm segment to obstacle
# the vector of our line
vec_line = p2 - p1
# the vector from the obstacle to the first line point
vec_ob_line = v - p1
# calculate the projection normalized by length of arm segment
projection = np.dot(vec_ob_line, vec_line) / np.sum((vec_line)**2)
if projection < 0:
# then closest point is the start of the segment
closest = p1
elif projection > 1:
# then closest point is the end of the segment
closest = p2
else:
closest = p1 + projection * vec_line
# calculate distance from obstacle vertex to the closest point
dist = np.sqrt(np.sum((v - closest)**2))
# account for size of obstacle
rho = dist - obstacle_radius
if rho < threshold:
eta = .02 # feel like i saw 4 somewhere in the paper
drhodx = (v - closest) / rho
Fpsp = (eta * (1.0/rho - 1.0/threshold) *
1.0/rho**2 * drhodx)
# get offset of closest point from link's reference frame
T_inv = robot_config.T_inv('link%i' % ii, q=q)
m = np.dot(T_inv, np.hstack([closest, [1]]))[:-1]
# calculate the Jacobian for this point
Jpsp = robot_config.J('link%i' % ii, x=m, q=q)[:3]
# calculate the inertia matrix for the
# point subjected to the potential space
Mxpsp_inv = np.dot(Jpsp,
np.dot(np.linalg.pinv(Mq), Jpsp.T))
svd_u, svd_s, svd_v = np.linalg.svd(Mxpsp_inv)
# cut off singular values that could cause problems
singularity_thresh = .00025
for ii in range(len(svd_s)):
svd_s[ii] = 0 if svd_s[ii] < singularity_thresh else \
1./float(svd_s[ii])
# numpy returns U,S,V.T, so have to transpose both here
Mxpsp = np.dot(svd_v.T, np.dot(np.diag(svd_s), svd_u.T))
u_psp = -np.dot(Jpsp.T, np.dot(Mxpsp, Fpsp))
if rho < .01:
u = u_psp
else:
u += u_psp
# multiply by -1 because torque is opposite of expected
u *= -1
print('u: ', u)
for ii, joint_handle in enumerate(joint_handles):
# the way we're going to do force control is by setting
# the target velocities of each joint super high and then
# controlling the max torque allowed (yeah, i know)
# get the current joint torque
_, torque = \
vrep.simxGetJointForce(
clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
joint_target_velocities[ii] = \
joint_target_velocities[ii] * -1
vrep.simxSetJointTargetVelocity(
clientID,
joint_handle,
joint_target_velocities[ii], # target velocity
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
# and now modulate the force
vrep.simxSetJointForce(
clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
# Update position of hand sphere
vrep.simxSetObjectPosition(
clientID,
hand_handle,
-1, # set absolute, not relative position
xyz,
vrep.simx_opmode_blocking)
track_hand.append(np.copy(xyz)) # and store for plotting
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
count += dt
else:
raise Exception('Failed connecting to remote API server')
finally:
# stop the simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Before closing the connection to V-REP,
# make sure that the last command sent out had time to arrive.
vrep.simxGetPingTime(clientID)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
print('connection closed...')
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
track_hand = np.array(track_hand)
track_target = np.array(track_target)
track_obstacle = np.array(track_obstacle)
fig = plt.figure()
ax = fig.gca(projection='3d')
# plot start point of hand
ax.plot([track_hand[0, 0]],
[track_hand[0, 1]],
[track_hand[0, 2]],
'bx', mew=10)
# plot trajectory of hand
ax.plot(track_hand[:, 0],
track_hand[:, 1],
track_hand[:, 2])
# plot trajectory of target
ax.plot(track_target[:, 0],
track_target[:, 1],
track_target[:, 2],
'rx', mew=10)
# plot trajectory of obstacle
ax.plot(track_obstacle[:, 0],
track_obstacle[:, 1],
track_obstacle[:, 2],
'yx', mew=10)
ax.set_xlim([-1, 1])
ax.set_ylim([-.5, .5])
ax.set_zlim([0, 1])
ax.legend()
plt.show()