# Source code for klampt.model.workspace

```"""Functions for computing robot workspaces, with lots of options for setting
feasibility conditions.

"""

from klampt import vis
from klampt.math import vectorops,so3,se3
from klampt.model import ik
from typing import Union,Sequence,Dict,Callable
from klampt.model.typing import Vector3,Config
import math
import numpy as np

[docs]def compute_occupancy_grid(points : Sequence[Vector3],
resolution=0.05,
dimensions=None,
bounds=None,
value='occupancy') -> VolumeGrid:
"""
Helper to compute an occupancy grid given a set of points.

Args:
points (list of Vector3 or nx3 array): the points
resolution (float, 3-vector, or None): the resolution of the resulting
grid.
dimensions (int or 3-vector optional): if resolution=None and dimensions
is given, the number of dimensions. If a single int, the cell size
is determined by dividing the longest side by dimensions.
bounds (pair of 3-vectors, optional): specifies the minimum and maximum
range of the grid. If not given, calculated by the
value (str): either 'occupancy', 'count', or 'probability'.

"""
points = np.asarray(points)
auto_bounds = False
expand = False
if bounds is not None:
lower_corner,upper_corner = bounds
if len(lower_corner) != 3 or len(upper_corner) != 3:
raise ValueError("Invalid bounds")
else:
if len(points) == 0:
raise ValueError("Cannot compute occupancy grid of empty set of points")
lower_corner,upper_corner = np.min(points,axis=0),np.max(points,axis=0)
assert len(lower_corner) == 3
assert len(upper_corner) == 3
auto_bounds = True
expand = True

if dimensions is not None:
if hasattr(dimensions,'__iter__'):
cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),dimensions)
invcellsize = vectorops.div(dimensions,vectorops.sub(upper_corner,lower_corner))
auto_bounds = False
else:
w = max(*vectorops.sub(upper_corner,lower_corner))
cellsize = [w / dimensions]*3
invcellsize = [dimensions/w]*3
dimensions = [int(math.floor(d/c)+1) for d,c in zip(vectorops.sub(upper_corner,lower_corner),cellsize)]
else:
if resolution is None:
raise ValueError("One of resolution or dimensions must be given")
cellsize = resolution
if not hasattr(resolution,'__iter__'):
cellsize = [resolution]*3
invcellsize = [1.0/c for c in cellsize]
dimensions = [int(math.floor(d/c)+1) for d,c in zip(vectorops.sub(upper_corner,lower_corner),cellsize)]

if auto_bounds:
bmax = vectorops.add(lower_corner,[d*c for (d,c) in zip(dimensions,cellsize)])
shift = vectorops.mul(vectorops.sub(upper_corner,bmax),0.5)
if expand:
lower_corner = vectorops.sub(lower_corner,cellsize)
dimensions = [d+2 for d in dimensions]

#compact way to compute all valid indices of points
if len(points)==0:
valid_indices = []
else:
indices = np.multiply(points - np.asarray(lower_corner),np.asarray(invcellsize))
indices = np.floor(indices).astype(int)
valid_points = np.all((0 <= indices) & (indices < np.array(dimensions)),axis=1)
valid_indices = indices[valid_points,:]
#output
reachable = np.zeros(dimensions)
if value=='occupancy':
for ind in valid_indices:
ind=tuple(ind)
reachable[ind] = 1.0
elif value in ['count','probability']:
for ind in valid_indices:
ind=tuple(ind)
reachable[ind] += 1
if value == 'probability' and len(points)>0:
reachable *= 1.0/len(points)
vg = VolumeGrid()
vg.setBounds(lower_corner,upper_corner)
vg.setValues(reachable)
return vg

[docs]def compute_field_grid(points : Sequence[Vector3],
values : Sequence[float],
resolution=0.05,
dimensions=None,
bounds=None,
aggregator='max',
initial_value='auto') -> VolumeGrid:
"""
Helper to compute a gridded value field over a set of scattered points.

Args:
points (list of Vector3 or nx3 array): the points
values (list of float): the values at each point
resolution (float, 3-vector, or None): the resolution of the resulting
grid.
dimensions (int or 3-vector optional): if resolution=None and dimensions
is given, the number of dimensions. If a single int, the cell size
is determined by dividing the longest side by dimensions.
bounds (pair of 3-vectors, optional): specifies the minimum and maximum
range of the grid. If not given, calculated by the
aggregator (str or pair): either 'max', 'min', 'sum', 'average', or a
pair of functions (f,g) giving an arbitrary aggregator
f(x,value) -> x', g(x) -> float. x is an arbitrary object, which
for each cell is initialized to the value specified by
initial_value (default None).
initial_value (float or 'auto'): the initial value of the cell before
aggregation.  If aggregator is a pair of functions, 'auto' sets x
to None by default.

"""
points = np.asarray(points)
auto_bounds = False
expand = False
if bounds is not None:
lower_corner,upper_corner = bounds
if len(lower_corner) != 3 or len(upper_corner) != 3:
raise ValueError("Invalid bounds")
else:
if len(points) == 0:
raise ValueError("Cannot compute occupancy grid of empty set of points")
lower_corner,upper_corner = np.min(points,axis=0),np.max(points,axis=0)
assert len(lower_corner) == 3
assert len(upper_corner) == 3
auto_bounds = True
expand = True

if dimensions is not None:
if hasattr(dimensions,'__iter__'):
cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),dimensions)
invcellsize = vectorops.div(dimensions,vectorops.sub(upper_corner,lower_corner))
auto_bounds = False
else:
w = max(*vectorops.sub(upper_corner,lower_corner))
cellsize = [w / dimensions]*3
invcellsize = [dimensions/w]*3
dimensions = [int(math.floor(d/c)+1) for d,c in zip(vectorops.sub(upper_corner,lower_corner),cellsize)]
else:
if resolution is None:
raise ValueError("One of resolution or dimensions must be given")
cellsize = resolution
if not hasattr(resolution,'__iter__'):
cellsize = [resolution]*3
invcellsize = [1.0/c for c in cellsize]
dimensions = [int(math.floor(d/c)+1) for d,c in zip(vectorops.sub(upper_corner,lower_corner),cellsize)]

if auto_bounds:
bmax = vectorops.add(lower_corner,[d*c for (d,c) in zip(dimensions,cellsize)])
shift = vectorops.mul(vectorops.sub(upper_corner,bmax),0.5)
if expand:
lower_corner = vectorops.sub(lower_corner,cellsize)
dimensions = [d+2 for d in dimensions]

#compact way to compute all valid indices of points
if len(points)==0:
valid_indices = []
else:
indices = np.multiply(points - np.asarray(lower_corner),np.asarray(invcellsize))
indices = np.floor(indices).astype(int)
valid_points = np.all((0 <= indices) & (indices < np.array(dimensions)),axis=1)
valid_indices = indices[valid_points,:]

result = None
if aggregator == 'max':
if initial_value == 'auto':
initial_value = -float('inf')
value_grid = np.full(dimensions,fill_value=initial_value)
f=max
g=None
elif aggregator == 'min':
if initial_value == 'auto':
initial_value = float('inf')
value_grid = np.full(dimensions,fill_value=float('inf'))
f=min
g=None
elif aggregator == 'sum':
if initial_value == 'auto':
initial_value = 0.0
value_grid = np.full(dimensions,fill_value=initial_value)
elif aggregator == 'average':
if initial_value == 'auto':
prior,strength = 0.0,0
else:
if not isinstance(initial_value,(tuple,list)) or len(initial_value) != 2:
raise ValueError("Initial value for average must be of the form (prior,strength)")
prior,strength = initial_value
vsum = np.full(dimensions,fill_value=prior*strength)
count = np.full(dimensions,fill_value=strength)
for ind,v in zip(valid_indices,values):
vsum[ind] += v
count[ind] += 1
if strength > 0:
result = np.divide(vsum,count)
else:
result = np.zeros(dimensions)
nz = (count > 0)
result[nz] = np.divide(vsum[nz],count[nz])
elif isinstance(aggregator,tuple):
f,g = aggregator
value_grid = np.full(dimensions,fill_value=initial_value)
else:
raise ValueError("Invalid value for aggregator, must be min, max, sum, average, or a pair of callables")
if result is None:
for ind,v in zip(valid_indices,values):
ind=tuple(ind)
value_grid[ind] = f(value_grid[ind],v)
if g is not None:
result = np.empty(dimensions)
for i in range(dimensions):
for j in range(dimensions):
for k in range(dimensions):
result[i,j,k] = g(value_grid[i,j,k])
else:
result = value_grid
vg = VolumeGrid()
vg.setBounds(lower_corner,upper_corner)
vg.setValues(result)
return vg

constraint_or_point : Union[Vector3,IKObjective],
Nsamples=100000,
resolution=0.05,
dimensions=None,
qmin : Config = None,
qmax : Config = None,
obstacles : Sequence[Union[RigidObjectModel,TerrainModel]] = None,
gravity : Vector3 = (0,0,-9.8),
self_collision : bool = True,
feasibility_test : Callable = None,
value='occupancy',
all_tests=True) -> Union[VolumeGrid,Dict[str,VolumeGrid]]:
"""Compute the reachable workspace of a point on a robot's end effector.
Has several options to ensure various feasibility conditions.  Ensures that
the robot does not collide with itself, optionally with obstacles.

Arguments:
should be computed
constraint_or_point (3-vector or IKObjective): the point on the link.
For fixed orientations, use an IKObjective.
Nsamples (int): the number of configuration samples to use
resolution (float, 3-vector, or None): the resolution of the resulting
grid.
dimensions (int or 3-vector optional): if resolution=None and
dimensions is given, the number of dimensions. If a single int, the
cell size is determined by dividing the longest side by dimensions.
fixed_links (list of int or str, optional): list of fixed link indices
qmin (Config, optional): overrides the robots lower joint limits
qmax (Config, optional): overrides the robots upper joint limits
obstacles (list of RigidObjectModel or TerrainModel): Ensures that the
robot is collision free with environment obstacles
isotropic load or a specific vector
load_type (str, optional): either 'auto' (default), 'isotropic',
'force', 'torque', 'wrench', 'force_local', 'torque_local', or
'wrench_local'.
gravity (3-vector, optional): if load testing is used, the gravity
vector used for gravity compensation.
self_collision (bool): whether self collision should be tested.
feasibility_test (callable, list, or dict): a function
feasible(q)->bool that tests whether the configuration is feasible.
If a list, a list of callables. If a dict, a map of str->callable.
value (str): either 'occupancy', 'count', or 'probability'.
all_tests (bool): whether to return a dict of results.

Returns:
The grid of reached points. If ``all_tests=True``, gives a set of
grids of reached points indexed by strings.  Key
'workspace' gives the reachable workspace of points meeting every
feasibility condition.  Keys 'self_collision_free',
'obstacle_X_collision_free' (X in range 0,...,len(obstacles)-1),
'load', 'feasibility_test' give the set of points passing each
feasibility condition.
"""
robot = link.robot()   # type: RobotModel
qmin_orig,qmax_orig = robot.getJointLimits()

if qmin is None:
qmin = qmin_orig
else:
if len(qmin)!=len(qmin_orig):
raise ValueError("Invalid length of qmin")
if qmax is None:
qmax = qmax_orig
else:
if len(qmax)!=len(qmax_orig):
raise ValueError("Invalid length of qmin")
import copy
q = robot.getConfig()
qmin = copy.copy(qmin)
qmax = copy.copy(qmax)
qmin[i] = q[i]
qmax[i] = q[i]
robot.setJointLimits(qmin,qmax)
else:
robot.setJointLimits(qmin,qmax)

if isinstance(constraint_or_point,IKObjective):
point_local,_ = constraint_or_point.getPosition()
constraint = constraint_or_point
else:
point_local = constraint_or_point
constraint = None

#setup feasibility test
def overall_feasibility_test():
return all(test() for k,test in feasibleTests.items())

#run tests
if all_tests:
points = {}
points['workspace'] = []
for name in feasibleTests:
points[name] = []
if isinstance(constraint_or_point,IKObjective):
#first do a run to figure out the possible cells
resolution,dimensions)
bmin,bmax = [vg_temp.bbox,vg_temp.bbox,vg_temp.bbox],[vg_temp.bbox,vg_temp.bbox,vg_temp.bbox]
cellsize = vectorops.div(vectorops.sub(bmax,bmin),vg_temp.dims)
vals = vg_temp.getValues()

#now solve the IK constraint
i,j,k = vals.nonzero()
num_restarts = max(5,Nsamples//len(i))
print("Using",num_restarts,"restarts to solve IK constraint")
for n in range(len(i)):
ind = (i[n],j[n],k[n])
#try reaching point described by ind. TODO: allow up to cellsize/2 slop
constraint.setFixedPosConstraint(point_local,target)
res = ik.solve_global(constraint,iters=20,numRestarts=num_restarts)
if res:
feasible = True
for (name,test) in feasibleTests.items():
if test():
points[name].append(target)
else:
feasible = False
if feasible:
points['workspace'].append(target)
print("With IK constraint, reached",len(points[name]),"/",len(i),"cells")
res = {}
for name in points:
res[name] = compute_occupancy_grid(points[name],resolution=None,dimensions=vg_temp.dims,bounds=(bmin,bmax),value=value)
else:
resolution,dimensions)
bmin,bmax = [vg_temp.bbox,vg_temp.bbox,vg_temp.bbox],[vg_temp.bbox,vg_temp.bbox,vg_temp.bbox]
for target,q in zip(temppoints,configs):
robot.setConfig(q)
feasible = True
for (name,test) in feasibleTests.items():
if test():
points[name].append(target)
else:
feasible = False
if feasible:
points['workspace'].append(target)
res = {}
for name in points:
res[name] = compute_occupancy_grid(points[name],resolution=None,dimensions=vg_temp.dims,bounds=(bmin,bmax),value=value)
if name=='workspace':
else:
else:
points = []
if isinstance(constraint_or_point,IKObjective):
#first do a run to figure out the possible cells
resolution,dimensions)
bmin,bmax = [vg_temp.bbox,vg_temp.bbox,vg_temp.bbox],[vg_temp.bbox,vg_temp.bbox,vg_temp.bbox]
cellsize = vectorops.div(vectorops.sub(bmax,bmin),vg_temp.dims)
vals = vg_temp.getValues()

#now solve the IK constraint
i,j,k = vals.nonzero()
for n in range(len(i)):
ind = (i[n],j[n],k[n])
#try reaching point described by ind. TODO: allow up to cellsize/2 slop
constraint.setFixedPosConstraint(point_local,target)
res = ik.solve_global(constraint,iters=20,numRestarts=5,feasibilityCheck=overall_feasibility_test)
if res:
points.append(target)

res = VolumeGrid(points,resolution=None,dimensions=vg_temp.dims,bounds=(bmin,bmax),value=value)
else:
resolution,dimensions,overall_feasibility_test)
bmin,bmax = [v for v in vg_temp.bbox[0:3]],[v for v in vg_temp.bbox[3:6]]
res = compute_occupancy_grid(points1+points2,dimensions=vg_temp.dims,bounds=(bmin,bmax),value=value)

#restore original limits
robot.setJointLimits(qmin_orig,qmax_orig)

return res

constraint_or_point : Union[Vector3,IKObjective],
value : Union[str,Callable],
Nsamples=100000,
resolution=0.05,
dimensions=None,
qmin : Config = None,
qmax : Config = None,
obstacles : Sequence[Union[RigidObjectModel,TerrainModel]] = None,
gravity : Vector3 = (0,0,-9.8),
self_collision : bool = True,
feasibility_test : Callable = None) -> VolumeGrid:
"""Compute a scalar field over the reachable workspace of a point on a
robot's end effector. Has several options to ensure various feasibility
conditions. Ensures that the robot does not collide with itself, optionally
with obstacles.

Arguments:
should be computed
constraint_or_point (3-vector or IKObjective): the point on the link.
For fixed orientations, use an IKObjective.
value (str or callable): 'occupancy', 'manipulability',
'max manipulability', 'min manipulability', 'load', or an arbitrary
function f(q) -> float.
Nsamples (int): the number of configuration samples to use
resolution (float, 3-vector, or None): the resolution of the resulting
grid.
dimensions (int or 3-vector optional): if resolution=None and
dimensions is given, the number of dimensions. If a single int, the
cell size is determined by dividing the longest side by dimensions.
fixed_links (list of int or str, optional): list of fixed link indices
qmin (Config, optional): overrides the robots lower joint limits
qmax (Config, optional): overrides the robots upper joint limits
obstacles (list of RigidObjectModel or TerrainModel): Ensures that the
robot is collision free with environment obstacles
isotropic load or a specific vector
load_type (str, optional): either 'auto' (default), 'isotropic',
'force', 'torque', 'wrench', 'force_local', 'torque_local', or
'wrench_local'.
gravity (3-vector, optional): if load testing is used, the gravity
vector used for gravity compensation.
self_collision (bool): whether self collision should be tested.
feasibility_test (callable, list, or dict): a function
feasible(q)->bool that tests whether the configuration is feasible.
If a list, a list of callables. If a dict, a map of str->callable.

Returns:
The grid of reached points.
If ``all_tests=True``, gives a set of grids of reached points.  Key
'workspace' gives the reachable workspace of points meeting every
feasibility condition.  Keys 'self_collision_free',
'obstacle_X_collision_free' (X in range 0,...,len(obstacles)-1),
'load', 'feasibility_test' give the set of points passing each
feasibility condition.
"""
qmin_orig,qmax_orig = robot.getJointLimits()

if qmin is None:
qmin = qmin_orig
else:
if len(qmin)!=len(qmin_orig):
raise ValueError("Invalid length of qmin")
if qmax is None:
qmax = qmax_orig
else:
if len(qmax)!=len(qmax_orig):
raise ValueError("Invalid length of qmin")
import copy
q = robot.getConfig()
qmin = copy.copy(qmin)
qmax = copy.copy(qmax)
qmin[i] = q[i]
qmax[i] = q[i]
robot.setJointLimits(qmin,qmax)
else:
robot.setJointLimits(qmin,qmax)

if isinstance(constraint_or_point,IKObjective):
point_local,_ = constraint_or_point.getPosition()
constraint = constraint_or_point
else:
point_local = constraint_or_point
constraint = None

#setup feasibility test
def overall_feasibility_test():
return all(test() for k,test in feasibleTests.items())

if isinstance(value,callable):
value_fn = lambda : value(robot.getConfig())
elif value == 'occupancy':
value_fn = lambda : 1
elif value.endswith('manipulability'):
raise NotImplementedError("TODO: do manipulability calculation")
else:
raise ValueError("Invalid value to output")

points = []
values = []
if isinstance(constraint_or_point,IKObjective):
#first do a run to figure out the possible cells
resolution,dimensions)
bmin,bmax = [vg_temp.bbox,vg_temp.bbox,vg_temp.bbox],[vg_temp.bbox,vg_temp.bbox,vg_temp.bbox]
cellsize = vectorops.div(vectorops.sub(bmax,bmin),vg_temp.dims)
vals = vg_temp.getValues()

#now solve the IK constraint
i,j,k = vals.nonzero()
for n in range(len(i)):
ind = (i[n],j[n],k[n])
#try reaching point described by ind. TODO: allow up to cellsize/2 slop
constraint.setFixedPosConstraint(point_local,target)
res = ik.solve_global(constraint,iters=20,numRestarts=5,feasibilityCheck=overall_feasibility_test)
if res:
points.append(target)
values.append(value_fn())

res = VolumeGrid(points,dimensions=vg_temp.dims,bounds=(bmin,bmax),value=value)
else:
resolution,dimensions,overall_feasibility_test)
for (p,q) in zip(points1+points2,configs1+configs2):
robot.setConfig(q)
if overall_feasibility_test():
points.append(p)
values.append(value_fn())
res = compute_field_grid(points,values,resolution,dimensions,value=value)

#restore original limits
robot.setJointLimits(qmin_orig,qmax_orig)

return res

self_collision,feasibility_test):
feasibleTests = {}
if self_collision:
feasibleTests['self_collision_free'] = lambda: not robot.selfCollides()
if obstacles is not None:
def collision_free_obstacle(obstacle):
return False
return True
for i,o in enumerate(obstacles):
feasibleTests['obstacle_{}_collision_free'.format(i)] = lambda : collision_free_obstacle(o)

tmax = np.asarray(robot.getTorqueLimits())
tmax[i] = float('inf')

#|J^T f| <= tmax
#for isotropic, test if |G(q)-J^T f| <= tmax for all |f|<=load
#test whether tmax_i <= max |gi - Ai f|  s.t. |f| <= load
G = robot.getGravityForces(gravity)
if i in fixed: continue
d = Jp[:,i]
g = G[i]
if max_torque_i > tmax[i]:
return False
return True
raise ValueError("Need load to be a 3-vector")
G = np.asarray(robot.getGravityForces(gravity))
raise ValueError("Need load to be a 3-vector")
G = np.asarray(robot.getGravityForces(gravity))
raise ValueError("Need load to be a 6-vector")
G = np.asarray(robot.getGravityForces(gravity))
else:
if feasibility_test is not None:
if isinstance(feasibility_test,(list,tuple)):
for (name,test) in enumerate(feasibility_test):
if not callable(test):
raise ValueError("Feasibility test {} must be callable".format(name))
feasibleTests['feasibility_test_{}'.format(name)] = lambda: test(robot.getConfig())
elif isinstance(feasibility_test,dict):
for (name,test) in feasibility_test.items():
if not callable(test):
raise ValueError("Feasibility test {} must be callable".format(name))
feasibleTests[name] = lambda: test(robot.getConfig())
else:
if not callable(feasibility_test):
raise ValueError("Feasibility test must be callable")
feasibleTests['feasibility_test'] = lambda: feasibility_test(robot.getConfig())
return feasibleTests

Nsamples,resolution,dimensions,feasibility_test=None):
if feasibility_test is None:
feasibility_test = lambda : True
points = []
configs = []
for i in range(Nsamples):
robot.randomizeConfig()
if feasibility_test():
configs.append(robot.getConfig())
vg_temp = compute_occupancy_grid(points,resolution,dimensions)
return vg_temp,points,configs

feasibility_test=None):
bmin,bmax = [vg_temp.bbox,vg_temp.bbox,vg_temp.bbox],[vg_temp.bbox,vg_temp.bbox,vg_temp.bbox]
cellsize = vectorops.div(vectorops.sub(bmax,bmin),vg_temp.dims)
vals = vg_temp.getValues()

fringe = set()
if x-1 >= 0 and vals[x-1,y,z]==0:
if x+1 < vals.shape and vals[x+1,y,z]==0:
if y-1 >= 0 and vals[x,y-1,z]==0:
if y+1 < vals.shape and vals[x,y+1,z]==0:
if z-1 >= 0 and vals[x,y,z-1]==0:
if z+1 < vals.shape and vals[x,y,z+1]==0:

#initialize neighbors  of visited cells
i,j,k = vals.nonzero()
for n in range(len(i)):

points = []
configs = []
numTests = 0
numSuccesses = 0
while len(fringe) > 0:
numTests += 1
cell = next(iter(fringe))