def draw_ray(env, ray, dist=0.03, linewidth=2, color=None):
"""
Draw a ray as an arrow + line into the OpenRAVE environment.
Parameters
----------
env: orpy.Environment
The OpenRAVE environment
ray: orpy.ray
The input ray with the position and direction of the arrow
dist: float
Length of the line
linewidth: float
Linewidth of the arrow and line (pixels)
color: array_like
Color of the arrow in the format `(red, green, blue, alpha)`
Returns
-------
h: orpy.GraphHandle
Handles holding the plot.
"""
if dist < 0:
newpos = ray.pos() + dist * ray.dir()
newray = orpy.Ray(newpos, ray.dir())
else:
newray = ray
iktype = orpy.IkParameterizationType.TranslationDirection5D
ikparam = orpy.IkParameterization(ray, iktype)
h = orpy.misc.DrawIkparam2(env,
ikparam,
dist=dist,
linewidth=linewidth,
coloradd=color)
return h
def compute_robot_configurations(env, robot, targets, params, qstart=None):
# Compute the IK solutions
starttime = time.time()
configurations = None
configuration_ID = 1
pos_check = np.array([0.56, -0.23, 0.305])
if qstart is not None:
configurations.append([qstart])
for i, ray in enumerate(targets):
newpos = ray.pos() - params.standoff * ray.dir()
if np.sqrt(np.sum((newpos - pos_check)**2)) < 0.0001:
print(configuration_ID)
newray = orpy.Ray(newpos, ray.dir())
solutions = ru.kinematics.find_ik_solutions(robot,
newray,
params.iktype,
collision_free=True,
freeinc=params.step_size)
if np.size(solutions) > 0:
ID = np.array([[configuration_ID]] * len(solutions))
configuration_ID += 1
point = np.array([ray.pos()] * len(solutions))
solutions = np.hstack(
(solutions, point, ID, np.array([[i]] * len(solutions))))
if configurations is None:
configurations = solutions
else:
configurations = np.vstack((configurations, solutions))
cpu_time = time.time() - starttime
return configurations, cpu_time
def random_lookat_ray(goal, radius, variance, fov):
"""
This function returns a random point and the direction from the point to the given goal.
The random point is inside a chopped upper sphere.
@type goal : np.array
@param goal : homogeneous transformation of the goal to look at
@type radius : float
@param radius : minimum radius of the sphere in meters
@type variance : float
@param variance : variance of the radius in meters
@rtype: orpy.Ray
@return: The random Ray that looks towards the goal
"""
theta1 = 2. * np.pi * np.random.uniform(-fov, fov)
theta2 = np.arccos(1 - np.random.uniform(0, fov)**2)
r = radius + variance * np.random.uniform(0, 1.)
x = r * np.cos(theta1) * np.sin(theta2)
y = r * np.sin(theta1) * np.sin(theta2)
z = r * np.cos(theta2)
R = goal[:3, :3]
point = goal[:3, 3] + np.dot(R, np.array([x, y, z]))
# Find the direction
direction = -np.dot(R, np.array([x, y, z]))
direction = tr.unit_vector(direction)
return orpy.Ray(point, direction)
def to_ray(T):
"""
Converts a homogeneous transformation into a C{orpy.Ray}.
@type T: np.array
@param T: The C{np.array} to be converted
@rtype: orpy.Ray
@return: The resulting ray
"""
return orpy.Ray(T[:3,3], T[:3,2])
def test_ray_conversions(self):
# Create the ray
position = np.array([0.5, -0.25, 0.1])
direction = br.vector.unit([-0.2506, 0.6846, 0.6846])
ray = orpy.Ray(position, direction)
# Check that the conversion works in both direction
transform = ru.conversions.from_ray(ray)
ray_from_transform = ru.conversions.to_ray(transform)
np.testing.assert_allclose(ray.pos(), ray_from_transform.pos())
np.testing.assert_allclose(ray.dir(), ray_from_transform.dir())
def test_draw_ray(self):
np.random.seed(123)
direction = np.random.randn(3)
direction /= np.linalg.norm(direction)
position = np.random.randn(3) * 0.5
ray = orpy.Ray(position, direction)
handles = ru.visual.draw_ray(self.env, ray)
if self.display_available:
self.assertEqual(len(handles), 3)
types = [type(h) for h in handles]
self.assertEqual(len(set(types)), 1)
self.assertEqual(set(types), {orpy.GraphHandle})
# Use negative distance
handles = ru.visual.draw_ray(self.env, ray, dist=-0.03)
if self.display_available:
self.assertEqual(len(handles), 3)
def compute_robot_configurations(robot, targets, params, qstart=None):
manip = robot.GetActiveManipulator()
# Compute the IK solutions
starttime = time.time()
configurations = []
if qstart is not None:
configurations.append([qstart])
for i, ray in enumerate(targets):
newpos = ray.pos() - params.standoff * ray.dir()
newray = orpy.Ray(newpos, ray.dir())
solutions = ru.kinematics.find_ik_solutions(robot,
newray,
params.iktype,
collision_free=True,
freeinc=params.step_size)
if len(solutions) == 0:
raise Exception('Failed to find IK solution for target %d' % i)
configurations.append(solutions)
cpu_time = time.time() - starttime
return configurations, cpu_time
#! /usr/bin/env python
import criutils as cu
import raveutils as ru
import openravepy as orpy
env = orpy.Environment()
robot_xml = 'robots/sda10f.robot.xml'
robot = env.ReadRobotXMLFile(robot_xml)
env.AddRobot(robot)
env.SetViewer('qtcoin')
prefixes = ['arm_left_', 'arm_right_']
handles = []
for prefix in prefixes:
manipname = prefix + 'tool0'
manip = robot.SetActiveManipulator(manipname)
for idx in manip.GetArmJoints():
joint = robot.GetJointFromDOFIndex(idx)
ray = orpy.Ray(joint.GetAnchor(), joint.GetAxis())
T = ru.conversions.from_ray(ray)
handles.append(ru.visual.draw_axes(env, T, dist=0.2))
def get_ray(self):
return orpy.Ray(self.position, self.direction)
if not manip.ik_database.load():
print 'Generating IK database for %s.' % manip.GetName()
manip.ik_database.autogenerate()
target = np.array([0.42, 0, 0.13])
#target = np.array([ 0.39, -0.06220808, 0.2342928])
#target = np.array([ 0.39, 0.0, 0.2342928])
direction = np.array([-1, 0, 0])
zstep = 0.001
prevtarget = None
# move up to find highest point with an ik.
while True:
solution = manip.FindIKSolution(
orpy.IkParameterization(
orpy.Ray(target, direction),
orpy.IkParameterization.Type.TranslationDirection5D),
orpy.IkFilterOptions.CheckEnvCollisions)
if solution is None:
print 'highest: ', prevtarget
target = prevtarget.copy()
break
else:
prevtarget = target.copy()
target[2] = target[2] + zstep
path = []
while True:
solution = manip.FindIKSolution(
orpy.IkParameterization(
orpy.Ray(target, direction),
def tsp_cspace_solver(robot, targets, params):
if not params.initialized():
raise ValueError('SolverParameters has not been initialized')
solver_starttime = time.time()
# Working entities
if params.qhome is None:
qhome = robot.GetActiveDOFValues()
else:
qhome = np.array(params.qhome)
with robot:
robot.SetActiveDOFValues(qhome)
home_yoshi = ru.kinematics.compute_yoshikawa_index(
robot, params.jac_link_name, params.translation_only,
params.penalize_jnt_limits)
# Select the IK solution with the best manipulability for each ray
cspace_nodes = [qhome]
indices = [home_yoshi]
for i, ray in enumerate(targets):
newpos = ray.pos() - params.standoff * ray.dir()
newray = orpy.Ray(newpos, ray.dir())
solutions = ru.kinematics.find_ik_solutions(robot,
newray,
params.iktype,
collision_free=True,
freeinc=params.step_size)
if len(solutions) == 0:
raise Exception('Failed to find IK solution for target %d' % i)
max_yoshikawa = -float('inf')
for j, q in enumerate(solutions):
with robot:
robot.SetActiveDOFValues(q)
yoshikawa = ru.kinematics.compute_yoshikawa_index(
robot, params.jac_link_name, params.translation_only,
params.penalize_jnt_limits)
if yoshikawa > max_yoshikawa:
max_yoshikawa = yoshikawa
max_idx = j
cspace_nodes.append(solutions[max_idx])
indices.append(max_yoshikawa)
# Solve the TSP on the configuration space
cgraph = rtsp.construct.from_coordinate_list(cspace_nodes,
params.cspace_metric,
params.cspace_metric_args)
ctour = params.tsp_solver(cgraph)
ctour = rtsp.tsp.rotate_tour(ctour, start=0) # Start from robot home
ctour.append(0) # Go back to the robot home
# Compute the c-space trajectories
trajectories, traj_cpu_times = compute_cspace_trajectories(
robot, cgraph, ctour, params)
solver_cpu_time = time.time() - solver_starttime
# Store the yoshikawa index in the C-Space graph
for n in cgraph.nodes_iter():
cgraph.node[n]['yoshikawa'] = indices[n]
info = dict()
# Return the graph
info['cgraph'] = cgraph
# Return the tour
info['ctour'] = ctour
# Return the costs
info['ccost'] = rtsp.tsp.compute_tour_cost(cgraph, ctour, is_cycle=False)
# Return the cpu times
info['traj_cpu_times'] = traj_cpu_times
info['solver_cpu_time'] = solver_cpu_time
# Task execution time
info['task_execution_time'] = compute_execution_time(trajectories)
return trajectories, info
def clusterRTSP(dataset='target_points_pipes.csv', visualise=0, weights=None, visualise_speed=1.0, qhome=None, kmin=3, kmax=40, vel_limits=0.5, acc=0.5):
"""
:param dataset: filename of input target points stored in /data/ package subdirectory
:param visualise: 1 to display simulation otherwise 0
:param weights: weights used for configuration selection
:param visualise_speed: speed of visualisation (float)
:param qhome: specify robot home position (array)
:param kmin: min number of clusters if xmeans is selected (int)
:param kmax: max number of clusters if xmeans is selected (int)
:param vel_limits: desired maximum allowed velocity (as % of max.)
:param acc: desired maximum acceleration (as % of max.)
"""
starttime = timer()
# ________________________________________________________
### DEFINE ENVIRONMENT
# ________________________________________________________
# initialise environment
if qhome is None:
qhome = [0, 0, 0, 0, 0, 0]
env, robot, manipulator, iktype = environments.load_KUKA_kr6(display=0, qhome=qhome, vel_limits=vel_limits, acc=acc)
# ________________________________________________________
### INITIALISE
# ________________________________________________________
targets = []
rospack = rospkg.RosPack()
points = []
path_to_files = rospack.get_path('cluster_rtsp')
filename = str(path_to_files + '/data/'+dataset)
with open(filename, 'rb') as csvfile:
csv_points=csv.reader(csvfile, delimiter=',')
for row in csv_points:
points.append([float(i) for i in row])
for i in xrange(0, len(points)):
targets.append(orpy.Ray(np.array(points[i][0:3]), -1*np.array(points[i][3:])))
n_points = len(targets)
print('Number of points: %d' % n_points)
initialise_end = timer()
# set solver parameters
params = classes.SolverParameters()
params.standoff = 0.001
params.step_size = np.pi / 3
params.qhome = robot.GetActiveDOFValues()
params.max_iters = 5000
params.max_ppiters = 100
params.try_swap = False
configurations, ik_cpu_time = kinematics.compute_robot_configurations(env, robot, targets, params)
# ________________________________________________________
### FORMAT CONFIG LIST
# ________________________________________________________
# append unique ID to each configuration
# all_configs format: [j1 j2 j3 j4 j5 j6 cluster_n x y z task_point_ID config_ID]
row = range(0, len(configurations))
all_configs = np.column_stack((configurations[:, 0:6], row, configurations[:, 6:10], row))
home_config = np.hstack((qhome, np.array([0, 0, 0, 0, all_configs[-1, 10] + 1, all_configs[-1, 11] + 1])))
all_configs = np.vstack((all_configs, home_config))
# ________________________________________________________
### CONFIGURATION SELECTIONS
# ________________________________________________________
print('Starting configuration selection...')
if weights is None:
weights = [0.2676, 0.3232, 0.2576, 0.0303, 0.0917, 0.0296]
selected_configurations, select_time = cselect.clusterConfigSelection(all_configs, qhome, weights)
# ________________________________________________________
### APPLY CLUSTERING METHOD TO CONFIG LIST
# ________________________________________________________
cluster_start = timer()
print ('Clustering configurations...')
xmeans = XMeans.fit(selected_configurations[:, 0:6], kmax=kmax, kmin=kmin, weights=np.array(weights)*6)
N = xmeans.k
labels = xmeans.labels_
print('Number of clusters assigned: %d.' % N)
# append cluster number to end of configuration points
for i in xrange(0, len(selected_configurations)):
selected_configurations[i, 6] = int(labels[i])
# sort rows in ascending order based on 7th element
ind = np.argsort(selected_configurations[:, 6])
selected_configurations = selected_configurations[ind]
cluster_end = timer()
# ________________________________________________________
### GLOBAL TSP COMPUTATIONS
# ________________________________________________________
# Generate new variable for local clusters of points
clusters = [None] * N
for i in xrange(0, N):
cluster = np.where(selected_configurations[:, 6] == i)[0]
clusters[i] = selected_configurations[cluster, :]
globsequence_start = timer()
print('Computing global sequence...')
gtour, pairs, closest_points, entry_points = tsp.globalTSP(clusters, qhome)
global_path = gtour[1:-1]
entry_points = entry_points[1:-1]
globsequence_end = timer()
# ________________________________________________________
### LOCAL TSP COMPUTATIONS
# ________________________________________________________
path = [None] * N
print('Solving TSP for each cluster...')
localtsp_start = timer()
# plan intra-cluster paths
for i in xrange(0, N):
if np.shape(clusters[global_path[i]])[0] > 1:
# Run Two-Opt
tgraph = tsp.construct_tgraph(clusters[global_path[i]][:, 0:6], distfn=tsp.euclidean_fn)
path[i] = tsp.two_opt(tgraph, start=entry_points[i][0], end=entry_points[i][1])
else:
path[i] = [0]
localtsp_end = timer()
# ________________________________________________________
### PLAN PATHS BETWEEN ALL POINTS IN COMPUTED PATH
# ________________________________________________________
# need to correct color indexing - somehow identify cluster index
plan_start = timer()
robot.SetActiveDOFValues(qhome)
if N == 1:
c = np.array([1, 0, 0])
elif N < 10:
c = np.array([[0.00457608, 0.58586408, 0.09916249],
[0.26603989, 0.36651324, 0.64662435],
[0.88546289, 0.63658585, 0.75394724],
[0.29854082, 0.26499636, 0.20025494],
[0.86513743, 0.98080264, 0.18520593],
[0.39864878, 0.33938585, 0.27366609],
[0.90286517, 0.51585244, 0.09724035],
[0.55158651, 0.56320824, 0.44465467],
[0.57776588, 0.38423542, 0.59291004],
[0.21227011, 0.9159966, 0.59002942]])
else:
c = np.random.rand(N, 3)
h = []
count = 0
traj = [None] * (len(selected_configurations) + 1)
skipped = 0
points = [None] * len(selected_configurations)
for i in xrange(0, N):
idx = global_path[i]
cluster = i + 1
print ('Planning paths for configurations in cluster %i ...' % cluster)
config = 0
clock_start = timer()
while config <= len(path[i]) - 1:
q = clusters[idx][path[i][config], 0:6]
traj[count] = ru.planning.plan_to_joint_configuration(robot, q, params.planner, params.max_iters, params.max_ppiters)
if traj[count] is None:
print ("Could not find a feasible path, skipping current point in cluster %i ... " % cluster)
skipped += 1
config += 1
continue
points[count] = np.hstack((clusters[idx][path[i][config], 7:10], clusters[idx][path[i][config], 6]))
robot.SetActiveDOFValues(q)
config += 1
count += 1
if config == len(path[i]):
end_time = timer() - clock_start
print('Planning time for cluster %d: %f' % (cluster, end_time))
traj[-1] = ru.planning.plan_to_joint_configuration(robot, qhome, params.planner, params.max_iters, params.max_ppiters)
robot.SetActiveDOFValues(qhome)
# get time at end of planning execution
end = timer()
info = classes.InfoObj()
info.initialise = initialise_end - starttime
info.getconfig = ik_cpu_time
info.configselection = select_time
info.clustering = cluster_end - cluster_start
info.globaltsp = globsequence_end - globsequence_start
info.localtsp = localtsp_end - localtsp_start
info.total_tsp = info.localtsp + info.globaltsp
info.pathplanning = end - plan_start
info.totalplanning = end - starttime
info.execution = kinematics.compute_execution_time(traj)
info.N_clusters = N
info.n_points = len(selected_configurations) - skipped
print('Initialisation time (s): %f' % info.initialise)
print ('Get configurations time (s): %f' % info.getconfig)
print ('Select configurations time (s): %f' % info.configselection)
print ('Clustering time (s): %f' % info.clustering)
print ('Compute global sequence time (s): %f' % info.globaltsp)
print ('Local TSP time (s): %f' % info.localtsp)
print ('Total TSP time (s): %f' % info.total_tsp)
print ('Path planning time (s): %f' % info.pathplanning)
print ('Total planning time (s): %f' % info.totalplanning)
print ('Execution time (s): %f' % info.execution)
print ('Number of visited points: %d' % info.n_points)
# ________________________________________________________
### ACTUATE ROBOT
# ________________________________________________________
if visualise == 1:
# Open display
env.SetViewer('qtcoin')
Tcamera = [[-0.75036157, 0.12281536, -0.6495182, 2.42751741],
[0.66099327, 0.12938928, -0.73915243, 2.34414649],
[-0.00673858, -0.98395874, -0.17826888, 1.44325936],
[0., 0., 0., 1.]]
time.sleep(1)
env.GetViewer().SetCamera(Tcamera)
print('Starting simulation in 2 seconds...')
time.sleep(2)
start_exec = timer()
cluster = 0
count = 0
for i in xrange(0, len(traj)):
if traj[i] is None:
continue
if i <= len(points) - 1:
cluster_next = points[i][3] + 1
if cluster_next != cluster:
cluster = cluster_next
count += 1
print ('Moving to configurations in cluster {:d} ...'.format(count))
play_trajectory(env, robot, traj[i], visualise_speed)
if i <= len(points) - 1:
h.append(env.plot3(points=(points[i][0:3]), pointsize=4, colors=c[int(points[i][3])]))
end_exec = timer() - start_exec
print ('Simulation time: %f' % end_exec)
raw_input('Press Enter to terminate...')
return info