def main():
w = WorldModel()
w.readFile("../data/robots/kinova_gen3_7dof.urdf")
robot = w.robot(0)
#put a "pen" geometry on the end effector
gpen = Geometry3D()
res = gpen.loadFile("../data/objects/cylinder.off")
assert res
gpen.scale(0.01,0.01,0.15)
gpen.rotate(so3.rotation((0,1,0),math.pi/2))
robot.link(7).geometry().setCurrentTransform(*se3.identity())
gpen.transform(*robot.link(8).getParentTransform())
robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry()))
robot.setConfig(robot.getConfig())
trajs = get_paths()
#place on a reasonable height and offset
tableh = 0.1
for traj in trajs:
for m in traj.milestones:
m[0] = m[0]*0.4 + 0.35
m[1] = m[1]*0.4
m[2] = tableh
if len(m) == 6:
m[3] *= 0.4
m[4] *= 0.4
return run_cartesian(w,trajs)
def loadForceSamples(self,directory):
"""Loads from [directory]/wrench_slices.csv and associated obj files"""
f = open("%s/wrench_slices.csv"%(directory,),'r')
lines = f.readlines()
f.close()
pts = []
volumes = []
numNonConvex = 0
for i in xrange(1,len(lines)):
entries = lines[i].split(',')
assert len(entries) == 4
ind,x,y,z = int(entries[0]),float(entries[1]),float(entries[2]),float(entries[3])
geom = Geometry3D()
geom.loadFile("%s/wrench_slice_%d.obj"%(directory,ind))
V = Volume()
V.setGeom(geom)
if len(V.convex_decomposition) > 1:
print "Encountered a non-convex force volume on CH point",i
numNonConvex += 1
pts.append((x,y,z))
volumes.append(V)
print "# of non-convex force volumes:",numNonConvex
self.setFromForceSamples(volumes,pts)
def merge_triangle_meshes(*items):
merged_geom = Geometry3D()
verts = []
tris = []
nverts = 0
for i, item in enumerate(items):
if isinstance(item, Geometry3D):
xform, (iverts, itris) = numpy_convert.to_numpy(item)
elif hasattr(item, 'geometry'):
xform, (iverts, itris) = numpy_convert.to_numpy(item.geometry())
else:
raise ValueError(
"Don't know how to merge trimesh from item of type " +
item.__class__.__name__)
verts.append(
np.dot(np.hstack((iverts, np.ones((len(iverts), 1)))),
xform.T)[:, :3])
tris.append(itris + nverts)
nverts += len(iverts)
verts = np.vstack(verts)
tris = np.vstack(tris)
for t in tris:
assert all(v >= 0 and v < len(verts) for v in t)
mesh = numpy_convert.from_numpy((verts, tris), 'TriangleMesh')
merged_geom.setTriangleMesh(mesh)
return merged_geom
def create_geometry_cache(self, robot, link_index, geom_index):
"""Copy into triangle mesh format"""
self.link_cache = {}
link_geom_info = [] # stores the (hull, geom_idx, link_idx) tuple
for i, geom_idx in enumerate(geom_index):
link_idx = geom_idx
while link_idx not in link_index: # Here it's important to find the minimum link that controls this geometry
link_idx = robot.link(
link_idx).getParent() # find its parent... makes sense
assert link_idx != -1 # error may occur
link = self.robot.link(geom_idx)
geom = link.geometry().convert('ConvexHull', 0)
link_geom_info.append((geom, geom_idx, link_idx))
assert self.compute_point_collision or self.compute_sweep_collision, "You have to at least consider one link collision"
self.link_cache['link'] = link_geom_info
# do the sweep part
if self.compute_sweep_collision:
link_sweep_geoms = []
for geom, geom_idx, link_idx in link_geom_info:
sweep_geom = Geometry3D()
sweep_geom.asConvexHull().fromTransform(geom.asConvexHull())
link_sweep_geoms.append((sweep_geom, geom_idx, link_idx))
self.link_cache['sweep'] = link_sweep_geoms
# consider the rigidly mounted part
mount_cache = []
for i in range(len(self.mounted)):
link_idx, relT, geom = self.mounted[i]
geom = geom.convert('ConvexHull', 0)
mount_cache.append((link_idx, relT, geom))
self.link_cache['mount'] = mount_cache
# consider the sweep of mounted geometry
if self.compute_sweep_collision:
mount_sweep_geoms = []
for link_idx, _, geom in mount_cache:
sweep_geom = Geometry3D()
sweep_geom.asConvexHull().fromTransform(geom.asConvexHull())
mount_sweep_geoms.append((sweep_geom, None, link_idx))
self.link_cache['sweepmount'] = mount_sweep_geoms
# for obstacles, there is not much we have to do
n_obs = len(self.world)
obs_geoms = []
for i in range(n_obs):
terrain = self.world.terrain(i)
geom = terrain.geometry().convert('ConvexHull', 0)
obs_geoms.append(geom)
self.obs_cache = obs_geoms
def _addTerrain(self, path, T, color, name):
item_1_geom = Geometry3D()
item_1_geom.loadFile(path)
item_1_geom.transform(T[0], T[1])
item_1 = self.w.makeTerrain(name)
item_1.geometry().set(item_1_geom)
item_1.appearance().setColor(color[0], color[1], color[2], color[3])
return item_1
def make_shelf(world,width,depth,height,wall_thickness=0.005):
"""Makes a new axis-aligned "shelf" centered at the origin with
dimensions width x depth x height. Walls have thickness wall_thickness.
"""
left = Geometry3D()
right = Geometry3D()
back = Geometry3D()
bottom = Geometry3D()
top = Geometry3D()
left.loadFile(KLAMPT_Demo+"/data/objects/cube.off")
left.transform([wall_thickness,0,0,0,depth,0,0,0,height],[-width*0.5,-depth*0.5,0])
right.loadFile(KLAMPT_Demo+"/data/objects/cube.off")
right.transform([wall_thickness,0,0,0,depth,0,0,0,height],[width*0.5,-depth*0.5,0])
back.loadFile(KLAMPT_Demo+"/data/objects/cube.off")
back.scale(width,wall_thickness,height)
back.translate([-width*0.5,depth*0.5,0])
bottom = primitives.box(width,depth,wall_thickness,center=[0,0,0])
top = primitives.box(width,depth,wall_thickness,center=[0,0,height-wall_thickness*0.5])
shelfgeom = Geometry3D()
shelfgeom.setGroup()
for i,elem in enumerate([left,right,back,bottom,top]):
g = Geometry3D(elem)
shelfgeom.setElement(i,g)
shelf = world.makeTerrain("shelf")
shelf.geometry().set(shelfgeom)
shelf.appearance().setColor(0.2,0.6,0.3,1.0)
return shelf
def show_workspace(grid):
vis.add("world",w)
res = numpy_convert.from_numpy((lower_corner,upper_corner,grid),'VolumeGrid')
g_workspace = Geometry3D(res)
g_surface = g_workspace.convert('TriangleMesh',0.5)
if g_surface.numElements() != 0:
vis.add("reachable_boundary",g_surface,color=(1,1,0,0.5))
else:
print("Nothing reachable?")
Tee = robot.link(ee_link).getTransform()
gpen.setCurrentTransform(*Tee)
box = GeometricPrimitive()
box.setAABB(lower_corner,upper_corner)
gbox = Geometry3D(box)
#show this if you want to debug the size of the grid domain
#vis.add("box",gbox,color=(1,1,1,0.2))
vis.add("pen tip",se3.apply(Tee,ee_local_pos))
vis.loop()
def _addRigidObject(self, path, T_g, T_p, color, object_mass, Com, name):
item_1_geom = Geometry3D()
item_1_geom.loadFile(path)
item_1_geom.transform(T_g[0], T_g[1])
item_1 = self.w.makeRigidObject(name)
item_1.geometry().set(item_1_geom)
item_1.appearance().setColor(color[0], color[1], color[2], color[3])
bmass = item_1.getMass()
bmass.setMass(object_mass)
bmass.setCom(Com)
item_1.setMass(bmass)
item_1.setTransform(T_p[0], T_p[1])
return item_1
def addTable(self, x=0, y=0):
"""
Add a table to the world
Parameters:
--------------
x,y: floats, the x,y position of the center of the table
"""
table_top = Geometry3D()
table_leg_1 = Geometry3D()
table_leg_2 = Geometry3D()
table_leg_3 = Geometry3D()
table_leg_4 = Geometry3D()
table_top.loadFile(model_path + "cube.off")
table_leg_1.loadFile(model_path + "cube.off")
table_leg_2.loadFile(model_path + "cube.off")
table_leg_3.loadFile(model_path + "cube.off")
table_leg_4.loadFile(model_path + "cube.off")
table_top.transform([self.table_length,0,0,0,self.table_width,0,0,0,\
self.table_top_thickness],[0,0,self.table_height - self.table_top_thickness])
table_leg_1.transform([self.leg_width,0,0,0,self.leg_width,0,0,0,self.table_height\
- self.table_top_thickness],[0,0,0])
table_leg_2.transform([self.leg_width,0,0,0,self.leg_width,0,0,0,self.table_height - \
self.table_top_thickness],[self.table_length-self.leg_width,0,0])
table_leg_3.transform([
self.leg_width, 0, 0, 0, self.leg_width, 0, 0, 0,
self.table_height - self.table_top_thickness
], [
self.table_length - self.leg_width,
self.table_width - self.leg_width, 0
])
table_leg_4.transform([
self.leg_width, 0, 0, 0, self.leg_width, 0, 0, 0,
self.table_height - self.table_top_thickness
], [0, self.table_width - self.leg_width, 0])
table_geom = Geometry3D()
table_geom.setGroup()
for i, elem in enumerate(
[table_top, table_leg_1, table_leg_2, table_leg_3, table_leg_4]):
g = Geometry3D(elem)
table_geom.setElement(i, g)
#table_geom.transform([1,0,0,0,1,0,0,0,1],[x-self.table_length/2.0,y-self.table_width/2.0,0])
table_geom.transform(
so3.from_rpy([0, 0, math.pi / 2]),
[x - self.table_length / 2.0, y - self.table_width / 2.0, 0])
table = self.w.makeTerrain("table")
table.geometry().set(table_geom)
table.appearance().setColor(self.light_blue[0], self.light_blue[1],
self.light_blue[2], self.light_blue[3])
def get_geometry(self,robot,qfinger=None,type='Group'):
"""Returns a Geometry of the gripper frozen at its configuration.
If qfinger = None, the current configuration is used. Otherwise,
qfinger is a finger configuration.
type can be 'Group' (most general and fastest) or 'TriangleMesh'
(compatible with Jupyter notebook visualizer.)
"""
if qfinger is not None:
q0 = robot.getConfig()
robot.setConfig(self.set_finger_config(q0,qfinger))
res = Geometry3D()
gripper_links = self.gripper_links if self.gripper_links is not None else [self.base_link] + self.descendant_links(robot)
if type == 'Group':
res.setGroup()
Tbase = robot.link(self.base_link).getTransform()
for i,link in enumerate(gripper_links):
Trel = se3.mul(se3.inv(Tbase),robot.link(link).getTransform())
g = robot.link(link).geometry().clone()
if not g.empty():
g.setCurrentTransform(*se3.identity())
g.transform(*Trel)
else:
print("Uh... link",robot.link(link).getName(),"has empty geometry?")
res.setElement(i,g)
if qfinger is not None:
robot.setConfig(q0)
return res
else:
import numpy as np
from klampt.io import numpy_convert
#merge the gripper parts into a static geometry
verts = []
tris = []
nverts = 0
for i,link in enumerate(gripper_links):
xform,(iverts,itris) = numpy_convert.to_numpy(robot.link(link).geometry())
verts.append(np.dot(np.hstack((iverts,np.ones((len(iverts),1)))),xform.T)[:,:3])
tris.append(itris+nverts)
nverts += len(iverts)
verts = np.vstack(verts)
tris = np.vstack(tris)
for t in tris:
assert all(v >= 0 and v < len(verts) for v in t)
mesh = numpy_convert.from_numpy((verts,tris),'TriangleMesh')
res.setTriangleMesh(mesh)
if qfinger is not None:
robot.setConfig(q0)
return res
def make_box(world,
width,
depth,
height,
wall_thickness=0.005,
mass=float('inf')):
"""Makes a new axis-aligned open-top box with its bottom centered at the origin
with dimensions width x depth x height. Walls have thickness wall_thickness.
If mass=inf, then the box is a TerrainModel, otherwise it's a RigidObjectModel
with automatically determined inertia.
"""
left = primitives.box(
wall_thickness, depth, height,
[-width * 0.5 - wall_thickness * 0.5, 0, height * 0.5])
right = primitives.box(
wall_thickness, depth, height,
[width * 0.5 + wall_thickness * 0.5, 0, height * 0.5])
front = primitives.box(
width, wall_thickness, height,
[0, -depth * 0.5 - wall_thickness * 0.5, height * 0.5])
back = primitives.box(
width, wall_thickness, height,
[0, depth * 0.5 + wall_thickness * 0.5, height * 0.5])
bottom = primitives.box(width, depth, wall_thickness,
[0, 0, wall_thickness * 0.5])
boxgeom = Geometry3D()
boxgeom.setGroup()
for i, elem in enumerate([left, right, front, back, bottom]):
boxgeom.setElement(i, elem)
if mass != float('inf'):
print("Making open-top box a rigid object")
bmass = Mass()
bmass.setMass(mass)
bmass.setCom([0, 0, height * 0.3])
bmass.setInertia([width / 12, depth / 12, height / 12])
box = world.makeRigidObject("box")
box.geometry().set(boxgeom)
box.appearance().setColor(0.6, 0.3, 0.2, 1.0)
box.setMass(bmass)
return box
else:
print("Making open-top box a terrain")
box = world.makeTerrain("box")
box.geometry().set(boxgeom)
box.appearance().setColor(0.6, 0.3, 0.2, 1.0)
return box
def main():
w = WorldModel()
w.readFile("../data/robots/kinova_gen3_7dof.urdf")
robot = w.robot(0)
#put a "pen" geometry on the end effector
gpen = Geometry3D()
res = gpen.loadFile("../data/objects/cylinder.off")
assert res
gpen.scale(0.01,0.01,0.15)
gpen.rotate(so3.rotation((0,1,0),math.pi/2))
robot.link(7).geometry().setCurrentTransform(*se3.identity())
gpen.transform(*robot.link(8).getParentTransform())
robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry()))
robot.setConfig(robot.getConfig())
return run_simulation(w)
def debug_stable_faces(obj, faces):
from klampt import vis, Geometry3D, GeometricPrimitive
from klampt.math import se3
import random
vis.createWindow()
obj.setTransform(*se3.identity())
vis.add("object", obj)
for i, f in enumerate(faces):
gf = GeometricPrimitive()
gf.setPolygon(np.stack(f).flatten())
color = (1, 0.5 + 0.5 * random.random(), 0.5 + 0.5 * random.random(),
0.5)
vis.add("face{}".format(i),
Geometry3D(gf),
color=color,
hide_label=True)
vis.setWindowTitle("Stable faces")
vis.dialog()
def __init__(self, floor_length=20, floor_width=20):
self.w = WorldModel()
self.floor = Geometry3D()
self.floor.loadFile(model_path + "cube.off")
self.floor.transform(
[floor_length, 0, 0, 0, floor_width, 0, 0, 0, 0.01],
[-floor_length / 2.0, -floor_width / 2.0, -0.01])
floor_terrain = self.w.makeTerrain("floor")
floor_terrain.geometry().set(self.floor)
floor_terrain.appearance().setColor(0.4, 0.3, 0.2, 1.0)
###colors
self.light_blue = [3.0 / 255.0, 140.0 / 255.0, 252.0 / 255.0, 1.0]
self.wall_green = [50.0 / 255.0, 168.0 / 255.0, 143.0 / 255.0, 1.0]
###sizes
self.table_length = 1.0
self.table_width = 0.5
self.table_top_thickness = 0.03
self.table_height = 0.6
self.leg_width = 0.05
self.cube_width = 0.05
def hull_to_klampt_geom(hull):
"""Converts a 3D hull to a Klamp't Geometry3D object."""
if len(hull.points) != 0:
if len(hull.points[0]) != 3:
raise RuntimeError("Invalid hull, not 3D")
mesh = TriangleMesh()
ptov = dict()
ind = 0
for v in hull.vertices:
p = hull.points[v]
mesh.vertices.append(p[0])
mesh.vertices.append(p[1])
mesh.vertices.append(p[2])
ptov[v]=ind
ind+=1
for s in hull.simplices:
assert len(s) == 3,"TODO: triangulate non-triangular facets"
mesh.indices.append(ptov[s[0]])
mesh.indices.append(ptov[s[1]])
mesh.indices.append(ptov[s[2]])
return Geometry3D(mesh)
def camera_to_points(camera,
points_format='numpy',
all_points=False,
color_format='channels'):
"""Given a SimRobotSensor that is a CameraSensor, returns a point cloud
associated with the current measurements.
Points are triangulated with respect to the camera's intrinsic coordinates,
and are returned in the camera local frame (+z backward, +x toward the
right, +y toward up).
The arguments
Args:
points_format (str, optional): configures the format of the return
value. Can be:
* 'numpy' (default): either an Nx3, Nx4, or Nx6 numpy array,
depending on whether color is requested (and its format). Will
fall back to 'native' if numpy is not available.
* 'native': same as numpy, but in list-of-lists format rather than
numpy arrays.
* 'PointCloud': a Klampt PointCloud object
* 'Geometry3D': a Klampt Geometry3D point cloud object
all_points (bool, optional): configures whether bad points should be
stripped out. If False (default), this strips out all pixels that
don't have a good depth reading (i.e., the camera sensor's maximum
reading.) If True, these pixels are all set to (0,0,0).
color_format (str): If the sensor has an RGB component, then color
channels may be produced. This value configures the output format,
and can take on the values:
* 'channels': produces individual R,G,B channels in the range
[0,1]. (note this is different from the interpretation of
camera_to_images)
* 'rgb': produces a single 32-bit integer channel packing the 8-bit
color channels together in the format 0xrrggbb.
* None: no color is produced.
Returns:
object: the point cloud in the requested format.
"""
assert isinstance(camera,
SimRobotSensor), "Must provide a SimRobotSensor instance"
assert camera.type(
) == 'CameraSensor', "Must provide a camera sensor instance"
assert int(camera.getSetting(
'depth')) == 1, "Camera sensor must have a depth channel"
has_numpy = _try_numpy_import()
if points_format == 'numpy' and not has_numpy:
points_format = 'native'
images = camera_to_images(camera, 'numpy', color_format)
assert images != None
rgb, depth = None, None
if int(camera.getSetting('rgb')) == 0:
depth = images
color_format = None
else:
rgb, depth = images
w = int(camera.getSetting('xres'))
h = int(camera.getSetting('yres'))
xfov = float(camera.getSetting('xfov'))
yfov = float(camera.getSetting('yfov'))
zmin = float(camera.getSetting('zmin'))
zmax = float(camera.getSetting('zmax'))
xshift = -w * 0.5
yshift = -h * 0.5
xscale = math.tan(xfov * 0.5) / (w * 0.5)
#yscale = -1.0/(math.tan(yfov*0.5)*h/2)
yscale = xscale #square pixels are assumed
xs = [(j + xshift) * xscale for j in range(w)]
ys = [(i + yshift) * yscale for i in range(h)]
if has_numpy:
if all_points:
depth[depth >= zmax] = 0
if color_format == 'channels':
#scale to range [0,1]
rgb = rgb * (1.0 / 255.0)
xgrid = np.repeat(np.array(xs).reshape((1, w)), h, 0)
ygrid = np.repeat(np.array(ys).reshape((h, 1)), w, 1)
assert xgrid.shape == (h, w)
assert ygrid.shape == (h, w)
pts = np.dstack((np.multiply(xgrid, depth), np.multiply(ygrid,
depth), depth))
assert pts.shape == (h, w, 3)
if color_format is not None:
if len(rgb.shape) == 2:
rgb = rgb.reshape(rgb.shape[0], rgb.shape[1], 1)
pts = np.concatenate((pts, rgb), 2)
#now have a nice array containing all points, shaped h x w x (3+c)
#extract out the valid points from this array
if all_points:
pts = pts.reshape(w * h, pts.shape[2])
else:
pts = pts[depth < zmax]
if points_format == 'native':
return pts.tolist()
elif points_format == 'numpy':
return pts
elif points_format == 'PointCloud' or points_format == 'Geometry3D':
res = PointCloud()
if all_points:
res.setSetting('width', str(w))
res.setSetting('height', str(h))
res.setPoints(pts.shape[0], pts[:, 0:3].flatten().tolist())
if color_format == 'rgb':
res.addProperty('rgb')
res.setProperties(pts[:, 3].flatten().tolist())
elif color_format == 'channels':
res.addProperty('r')
res.addProperty('g')
res.addProperty('b')
res.setProperties(pts[:, 3:6].flatten().tolist())
elif color_format == 'bgr':
raise ValueError(
"bgr color format not supported with PointCloud output")
if points_format == 'PointCloud':
return res
else:
from klampt import Geometry3D
g = Geometry3D()
g.setPointCloud(res)
return g
else:
raise ValueError("Invalid points_format " + points_format)
return Nnoe
else:
raise NotImplementedError(
"Native format depth image processing not done yet")
def image_to_points(depth,
color,
xfov,
yfov=None,
depth_scale=None,
depth_range=None,
color_format='auto',
points_format='numpy',
all_points=False):
"""Given a depth and optionally color image, returns a point cloud
representing the depth or RGB-D scene.
Args:
depth (list of lists or numpy array): the w x h depth image (rectified).
color (list of lists or numpy array, optional): the w x h color image.
Assumed that color maps directly onto depth pixels. If None,
an uncolored point cloud will be produced.
xfov (float): horizontal field of view, in radians.
yfov (float, optional): vertical field of view, in radians. If not
given, square pixels are assumed.
depth_scale (float, optional): a scaling from depth image values to
absolute depth values.
depth_range (pair of floats, optional): if given, only points within
this depth range (non-inclusive) will be extracted. If
all_points=False, points that fail the range test will be stripped
from the output. E.g., (0.5,8.0) only extracts points with
z > 0.5 and z < 8 units.
color_format (str): governs how pixels in the RGB result are packed.
Can be:
* 'auto' (default): if it's a 3D array, it assumes elements are in
'channels' format, otherwise it assumes 'rgb'.
* 'channels': a 3D array with 3 channels corresponding to R, G,
B values in the range [0,255] if uint8 type, otherwise in the
range [0,1].
* 'rgb' a 2D array with a 32-bit integer channel, with
R,G,B channels packed in hex format 0xrrggbb.
points_format (str, optional): configures the format of the return
value. Can be:
* 'numpy' (default): either an Nx3, Nx4, or Nx6 numpy array,
depending on whether color is requested (and its format). Will
fall back to 'native' if numpy is not available.
* 'native': same as numpy, but in list-of-lists format rather than
numpy arrays.
* 'PointCloud': a Klampt PointCloud object
* 'Geometry3D': a Klampt Geometry3D point cloud object
all_points (bool, optional): configures whether bad points should be
stripped out. If False (default), this strips out all pixels that
don't have a good depth reading (i.e., the camera sensor's maximum
reading.) If True, these pixels are all set to (0,0,0).
Returns:
numpy ndarray or Geometry3D: the point cloud. Represented as being local to the standard
camera frame with +x to the right, +y down, +z forward.
"""
has_numpy = _try_numpy_import()
if not has_numpy:
raise NotImplementedError("TODO image_to_points without numpy")
depth = np.asarray(depth)
assert len(depth.shape) == 2
h, w = depth.shape
if color is not None:
color = np.asarray(color)
if h != color.shape[0] or w != color.shape[1]:
raise ValueError("color and depth need to have same dimensions")
if color_format == 'auto':
if len(color.shape) == 3:
color_format = 'channels'
else:
assert len(color.shape) == 2
color_format = 'rgb'
if depth_scale is not None:
depth *= depth_scale
if depth_range is not None:
valid = np.logical_and((depth > depth_range[0]),
(depth < depth_range[1]))
if all_points:
depth[~valid] = 0
valid = (depth > 0)
else:
valid = (depth > 0)
xshift = -w * 0.5
yshift = -h * 0.5
xscale = math.tan(xfov * 0.5) / (w * 0.5)
if yfov is not None:
yscale = -1.0 / (math.tan(yfov * 0.5) * h / 2)
else:
yscale = xscale #square pixels are assumed
xs = [(j + xshift) * xscale for j in range(w)]
ys = [(i + yshift) * yscale for i in range(h)]
if color_format == 'channels' and color.dtype == np.uint8:
#scale to range [0,1]
color = color * (1.0 / 255.0)
xgrid = np.repeat(np.array(xs).reshape((1, w)), h, 0)
ygrid = np.repeat(np.array(ys).reshape((h, 1)), w, 1)
assert xgrid.shape == (h, w)
assert ygrid.shape == (h, w)
pts = np.dstack((np.multiply(xgrid, depth), np.multiply(ygrid,
depth), depth))
assert pts.shape == (h, w, 3)
if color_format is not None:
if len(color.shape) == 2:
color = color.reshape(color.shape[0], color.shape[1], 1)
pts = np.concatenate((pts, color), 2)
#now have a nice array containing all points, shaped h x w x (3+c)
#extract out the valid points from this array
if all_points:
pts = pts.reshape(w * h, pts.shape[2])
else:
pts = pts[valid]
if points_format == 'native':
return pts.tolist()
elif points_format == 'numpy':
return pts
elif points_format == 'PointCloud' or points_format == 'Geometry3D':
res = PointCloud()
if all_points:
res.setSetting('width', str(w))
res.setSetting('height', str(h))
res.setPoints(pts.shape[0], pts[:, 0:3].flatten().tolist())
if color_format == 'rgb':
res.addProperty('rgb')
res.setProperties(pts[:, 3].flatten().tolist())
elif color_format == 'channels':
res.addProperty('r')
res.addProperty('g')
res.addProperty('b')
res.setProperties(pts[:, 3:6].flatten().tolist())
if points_format == 'PointCloud':
return res
else:
from klampt import Geometry3D
g = Geometry3D()
g.setPointCloud(res)
return g
else:
raise ValueError(
"Invalid points_format, must be either native, numpy, PointCloud, or Geometry3D"
)
#initiate the world
w = WorldModel()
#add a floor
# floor = Geometry3D()
# floor.loadFile("cube.off")
# floor_length = 0.2
# floor_width = 0.2
# floor.transform([floor_length,0,0,0,floor_width,0,0,0,0.01],[-floor_length/2.0,-floor_width/2.0,-0.01])
# floor_terrain = w.makeTerrain("floor")
# floor_terrain.geometry().set(floor)
# floor_terrain.appearance().setColor(0.4,0.3,0.2,1.0)
#add the probe
item_1_geom = Geometry3D()
item_1_geom.loadFile('Contact_Probe_linear2.STL')
#item_1_geom.transform(T_g[0],T_g[1])
item_1 = w.makeRigidObject('probe')
item_1.geometry().set(item_1_geom)
item_1.appearance().setColor(1, 0, 0, 0.2)
###################Load data for line probe#################################################
exp_path = '../data_final/exp_' + str(exp_N) + '/'
exp_path_2 = '../data_final/exp_' + str(exp_N) + '_debiased/'
#load line geometry data
lineStarts, lineEnds, lineNormals, lineTorqueAxes = cal_line_data(exp_path)
#load visual model
pcd = load_pcd(exp_path_2 + 'originalPcd.txt', pcdtype='return_lines')
points = np.array(pcd)
def make_cube(self,
world,
width,
depth,
height,
x,
y,
z,
wall_thickness,
color,
name,
object_file="cube.off",
object_type="terrain"):
"""
This function created a cube object based on the given dimension and position
:param world: world object
:param width: width of the cube
:param depth: depth of the cube
:param height: height of the cube
:param x: X coordinate of the of the cube center
:param y: Y coordinate of the of the cube center
:param z: Z coordinate of the of the cube center
:param wall_thickness: wall thickness of the cube
:param object_file: file of the cube object
:return:
"""
object_path = self.objects_dir + object_file
# create the Geometry for all the faces of the object
left = Geometry3D()
right = Geometry3D()
front = Geometry3D()
back = Geometry3D()
bottom = Geometry3D()
top = Geometry3D()
left.loadFile(object_path)
right.loadFile(object_path)
front.loadFile(object_path)
back.loadFile(object_path)
bottom.loadFile(object_path)
top.loadFile(object_path)
left.transform([wall_thickness, 0, 0, 0, depth, 0, 0, 0, height],
[-width * 0.5, -depth * 0.5, 0])
right.transform([wall_thickness, 0, 0, 0, depth, 0, 0, 0, height],
[width * 0.5, -depth * 0.5, 0])
front.transform([-width, 0, 0, 0, wall_thickness, 0, 0, 0, height],
[width * 0.5, -depth * 0.5, 0])
back.transform([width, 0, 0, 0, wall_thickness, 0, 0, 0, height],
[-width * 0.5, depth * 0.5, 0])
bottom.transform([width, 0, 0, 0, depth, 0, 0, 0, wall_thickness],
[-width * 0.5, -depth * 0.5, 0])
top.transform([width, 0, 0, 0, depth, 0, 0, 0, wall_thickness],
[-width * 0.5, -depth * 0.5, height - wall_thickness])
cubegeom = Geometry3D()
cubegeom.setGroup()
for i, elem in enumerate([left, right, front, back, bottom, top]):
g = Geometry3D(elem)
cubegeom.setElement(i, g)
if object_type == "rigid" or object_type == "goal":
cube = world.makeRigidObject(name)
else:
cube = world.makeTerrain(name)
cube.geometry().set(cubegeom)
cube.appearance().setColor(color[0], color[1], color[2], color[3])
cube.geometry().translate((x, y, z))
return cube
def find_collision_pair(self, thetas, dcheck):
"""
Given current solution, find all possible collision pairs.
"""
link_trans, mount_trans = self.all_linkgeom_transforms(thetas)
point_collisions = []
sweep_collisions = []
# Just iterate through all stuff and see what happens... We can safely ignore first and last
n_theta = thetas.shape[0]
setting = DistanceQuerySettings()
setting.upperBound = dcheck # value greater than this is not considered
def add_point(link_idx, geom_idx, rst, i, oj):
# This function add point collision information...
p1, p2 = np.array(rst.cp1), np.array(rst.cp2)
if rst.d > 0:
normal = (p1 - p2)
else:
normal = p2 - p1
normal /= np.linalg.norm(normal)
# find p1 in local coordinate
self.robot.setConfig(thetas[i])
p1_local = self.robot.link(link_idx).getLocalPosition(list(p1))
jacobian = self.robot.positionJacobian(link_idx, lcl_pos=p1_local)
point_collisions.append(
JointObjectInfo(geom_idx, oj, thetas[i].copy(), i, jacobian,
normal, rst.d))
def add_sweep(link_idx, geom_idx, sgl_lv, ltran1, ltran2, rst, i, oj):
# This function add sweep collision information
p2, pswept = np.array(rst.cp2), np.array(rst.cp1)
if rst.d > 0:
normal = pswept - p2
else:
normal = p2 - pswept
normal /= np.linalg.norm(normal)
# I have to find the two points on two seperate geometry
sgl_lv.setCurrentTransform(*ltran1)
p0 = np.array(sgl_lv.support(-normal))
sgl_lv.setCurrentTransform(*ltran2)
p1 = np.array(sgl_lv.support(-normal))
dist1 = np.linalg.norm(p1 - pswept)
dist0 = np.linalg.norm(p0 - pswept)
alpha = dist1 / (dist0 + dist1)
# print(f'Sweep link {li} obj {oj} step {i} dist {dist:.3f} dist0 {dist0:.3f} dist1 {dist1:.3f}')
# compute the two jacobians
self.robot.setConfig(thetas[i])
the_link = self.robot.link(link_idx)
p0_local = the_link.getLocalPosition(p0)
# jac0 = np.array(self.robot.link(li_).getPositionJacobian(p0_local))
jac0 = self.robot.positionJacobian(link_idx, None, p0_local)
self.robot.setConfig(thetas[i + 1])
p1_local = the_link.getLocalPosition(p1)
# jac1 = np.array(self.robot.link(li_).getPositionJacobian(p1_local))
jac1 = self.robot.positionJacobian(link_idx, None, p1_local)
sweep_collisions.append(
SweepJointObjectInfo(geom_idx, oj, thetas[i].copy(),
thetas[i + 1].copy(), i, alpha, jac0,
jac1, normal, rst.d))
sweep_collisions[-1].point1 = p0
sweep_collisions[-1].point2 = p1
sweep_collisions[-1].pswept = pswept
sweep_collisions[-1].pobs = p2
# outer loop is for obstacles
for oj_, oj in enumerate(self.world.index):
ov = self.geom_cache['obs'][
oj_] # transformation information is already contained in Geometry3D
for li_, (lv, lv_copy, geom_idx,
link_idx) in enumerate(self.geom_cache['link']):
# first step is to compute point violation...
if self.compute_point_collision:
for i in range(1, n_theta - 1):
ltran = link_trans[i][li_]
lv.setCurrentTransform(*ltran)
rst = lv.distance_ext(ov, setting)
# append if dist is smaller than dcheck
if rst.d < dcheck:
add_point(link_idx, geom_idx, rst, i, oj)
# the next step is to compute sweep information
if self.compute_sweep_collision:
for i in range(n_theta - 1):
ltran1, ltran2 = link_trans[i][li_], link_trans[i +
1][li_]
lv.setCurrentTransform(
*ltran1) # set transformation of link start
lv_copy.setCurrentTransform(
*ltran2) # set transform of link end
cvxhull = Geometry3D()
cvxhull.setConvexHullGroup(lv, lv_copy)
rst = cvxhull.distance_ext(ov, setting)
if rst.d < dcheck:
add_sweep(link_idx, geom_idx, lv, ltran1, ltran2,
rst, i, oj)
# now I consider attached geometries
for li_, (link_idx, relT, geom,
geom_copy) in enumerate(self.geom_cache['mount']):
# start with point ocllision
if self.compute_point_collision:
for i in range(1, n_theta - 1):
ltran = mount_trans[i][li_]
geom.setCurrentTransform(*ltran)
rst = geom.distance_ext(ov, setting)
if rst.d < dcheck:
add_point(link_idx, -1 - li_, rst, i, oj)
# then sweep one
if self.compute_sweep_collision:
for i in range(n_theta - 1):
ltran1, ltran2 = mount_trans[i][li_], mount_trans[
i + 1][li_]
geom.setCurrentTransform(*ltran1)
geom_copy.setCurrentTransform(*ltran2)
cvxhull = Geometry3D()
cvxhull.setConvexHullGroup(geom, geom_copy)
rst = cvxhull.distance_ext(ov, setting)
if rst.d < dcheck:
add_sweep(link_idx, -1 - li_, geom, ltran1, ltran2,
rst, i, oj)
return point_collisions, sweep_collisions
def from_numpy(obj, type='auto', template=None):
"""Converts a numpy array or multiple numpy arrays to a Klamp't object.
Supports:
* lists and tuples
* RigidTransform: accepts a 4x4 homogeneous coordinate transform
* Matrix3, Rotation: accepts a 3x3 matrix.
* Configs
* Trajectory: accepts a pair (times,milestones)
* TriangleMesh: accepts a pair (verts,indices)
* PointCloud: accepts a n x (3+k) array, where k is the # of properties
* VolumeGrid: accepts a triple (bmin,bmax,array)
* Geometry3D: accepts a pair (T,geomdata)
"""
global supportedTypes
if type == 'auto' and template is not None:
otype = types.objectToTypes(template)
if isinstance(otype, (list, tuple)):
for t in otype:
if t in supportedTypes:
type = t
break
if type == 'auto':
raise ValueError('obj is not a supported type: ' +
', '.join(otype))
else:
type = otype
if type == 'auto':
if isinstance(obj, (tuple, list)):
if all(isinstance(v, np.ndarray) for v in obj):
if len(obj) == 2:
if len(obj[0].shape) == 1 and len(obj[1].shape) == 2:
type = 'Trajectory'
elif len(obj[0].shape) == 2 and len(
obj[1].shape
) == 2 and obj[0].shape[1] == 3 and obj[1].shape[1] == 3:
type = 'TriangleMesh'
if len(obj) == 3:
if obj[0].shape == (3, ) and obj[1].shape == (3, ):
type = 'VolumeGrid'
if type == 'auto':
raise ValueError(
"Can't auto-detect type of list of shapes" +
', '.join(str(v.shape) for v in obj))
else:
if isinstance(obj[0], np.ndarray) and obj[0].shape == (4, 4):
type = 'Geometry3D'
else:
raise ValueError(
"Can't auto-detect type of irregular list")
else:
assert isinstance(
obj, np.ndarray
), "Can only convert lists, tuples, and arrays from numpy"
if obj.shape == (3, 3):
type = 'Matrix3'
elif obj.shape == (4, 4):
type = 'RigidTransform'
elif len(obj.shape) == 1:
type = 'Config'
else:
raise ValueError("Can't auto-detect type of matrix of shape " +
str(obj.shape))
if type not in supportedTypes:
raise ValueError(type + ' is not a supported type')
if type == 'RigidTransform':
return se3.from_homogeneous(obj)
elif type == 'Rotation' or type == 'Matrix3':
return so3.from_matrix(obj)
elif type == 'Trajectory':
assert len(obj) == 2, "Trajectory format is (times,milestones)"
times = obj[0].tolist()
milestones = obj[1].tolist()
if template is not None:
return template.constructor()(times, milestones)
from klampt.model.trajectory import Trajectory
return Trajectory(times, milestones)
elif type == 'TriangleMesh':
from klampt import TriangleMesh
res = TriangleMesh()
vflat = obj[0].flatten()
res.vertices.resize(len(vflat))
for i, v in enumerate(vflat):
res.vertices[i] = float(v)
iflat = obj[1].flatten()
res.indices.resize(len(iflat))
for i, v in enumerate(iflat):
res.indices[i] = int(v)
return res
elif type == 'PointCloud':
from klampt import PointCloud
assert len(obj.shape) == 2, "PointCloud array must be a 2D array"
assert obj.shape[1] >= 3, "PointCloud array must have at least 3 values"
points = obj[:, :3]
properties = obj[:, 3:]
res = PointCloud()
res.setPoints(points.shape[0], points.flatten())
if template is not None:
if len(template.propertyNames) != properties.shape[1]:
raise ValueError(
"Template object doesn't have the same properties as the numpy object"
)
for i in range(len(template.propertyNames)):
res.propertyNames[i] = template.propertyNames[i]
else:
for i in range(properties.shape[1]):
res.propertyNames.append('property %d' % (i + 1))
if len(res.propertyNames) > 0:
res.properties.resize(len(res.propertyNames) * points.shape[0])
if obj.shape[1] >= 3:
res.setProperties(properties.flatten())
return res
elif type == 'VolumeGrid':
from klampt import VolumeGrid
assert len(obj) == 3, "VolumeGrid format is (bmin,bmax,values)"
assert len(obj[2].shape) == 3, "VolumeGrid values must be a 3D array"
bmin = obj[0]
bmax = obj[1]
values = obj[2]
res = VolumeGrid()
res.bbox.append(bmin[0])
res.bbox.append(bmin[1])
res.bbox.append(bmin[2])
res.bbox.append(bmax[0])
res.bbox.append(bmax[1])
res.bbox.append(bmax[2])
res.dims.append(values.shape[0])
res.dims.append(values.shape[1])
res.dims.append(values.shape[2])
vflat = values.flatten()
res.values.resize(len(vflat))
for i, v in enumerate(vflat):
res.values[i] = v
return res
elif type == 'Group':
from klampt import Geometry3D
res = Geometry3D()
assert isinstance(
obj,
(list, tuple)), "Group format is a list or tuple of Geometry3D's"
for i in range(len(obj)):
res.setElement(i, from_numpy(obj[i], 'Geometry3D'))
return res
elif type == 'Geometry3D':
from klampt import Geometry3D
if not isinstance(obj, (list, tuple)) or len(obj) != 2:
raise ValueError("Geometry3D must be a (transform,geometry) tuple")
T = from_numpy(obj[0], 'RigidTransform')
geomdata = obj[1]
subtype = None
if template is not None:
subtype = template.type()
if subtype == 'PointCloud':
g = Geometry3D(
from_numpy(geomdata, subtype, template.getPointCloud()))
else:
g = Geometry3D(from_numpy(geomdata, subtype))
g.setCurrentTransform(*T)
return g
subtype = 'Group'
if all(isinstance(v, np.ndarray) for v in geomdata):
if len(geomdata) == 2:
if len(geomdata[0].shape) == 1 and len(geomdata[1].shape) == 2:
subtype = 'Trajectory'
elif len(geomdata[0].shape) == 2 and len(
geomdata[1].shape) == 2 and geomdata[0].shape[
1] == 3 and geomdata[1].shape[1] == 3:
subtype = 'TriangleMesh'
if len(geomdata) == 3:
if geomdata[0].shape == (3, ) and geomdata[1].shape == (3, ):
subtype = 'VolumeGrid'
g = Geometry3D(from_numpy(obj, subtype))
g.setCurrentTransform(*T)
return g
else:
return obj.flatten()
def box(width,
depth,
height,
center=None,
R=None,
t=None,
world=None,
name=None,
mass=float('inf'),
type='TriangleMesh'):
"""Makes a box with dimensions width x depth x height. The box is centered
at (0,0,0) by default.
Args:
width,depth,height (float): x,y,z dimensions of the box
center (list of 3 floats, optional): if None (typical),
the *geometry* of the box is centered at 0. Otherwise,
the *geometry* of the box is shifted relative to the
box's local coordinate system.
R,t (se3 transform, optional): if given, the box's world coordinates
will be rotated and shifted by this transform.
world (WorldModel, optional): If given, then the box will be a
RigidObjectModel or TerrainModel will be created in this world
name (str, optional): If world is given, this is the name of the object.
Default is 'box'.
mass (float, optional): If world is given and this is inf, then a
TerrainModel will be created. Otherwise, a RigidObjectModel
will be created with automatically determined inertia.
type (str, optional): the geometry type. Defaults to 'TriangleMesh',
but also 'GeometricPrimitive' and 'VolumeGrid' are accepted.
Returns:
Geometry3D, RigidObjectModel, or TerrainModel: A representation
of the box. If a world is given, then either a RigidObjectModel
or TerrainModel is added to the world and returned.
"""
if center is None:
center = [0, 0, 0]
prim = GeometricPrimitive()
prim.setAABB([
center[0] - width * 0.5, center[1] - depth * 0.5,
center[2] - height * 0.5
], [
center[0] + width * 0.5, center[1] + depth * 0.5,
center[2] + height * 0.5
])
geom = Geometry3D(prim)
if type != 'GeometricPrimitive':
geom = geom.convert(type)
if world is None:
if R is not None and t is not None:
geom.setCurrentTransform(R, t)
return geom
#want a RigidObjectModel or TerrainModel
if name is None:
name = 'box'
if mass != float('inf'):
bmass = Mass()
bmass.setMass(mass)
bmass.setCom(center)
bmass.setInertia([
mass * (depth**2 + height**2) / 12,
mass * (width**2 + height**2) / 12,
mass * (width**2 + height**2) / 12
])
robj = world.makeRigidObject(name)
robj.geometry().set(geom)
robj.setMass(bmass)
if R is not None and t is not None:
robj.setTransform(R, t)
return robj
else:
tobj = world.makeTerrain(name)
if R is not None and t is not None:
geom.transform(R, t)
tobj.geometry().set(geom)
return tobj
def sphere(radius,
center=None,
R=None,
t=None,
world=None,
name=None,
mass=float('inf'),
type='TriangleMesh'):
"""Makes a sphere with the given radius
Args:
radius (float): radius of the sphere
center (list of 3 floats, optional): if None (typical), the *geometry*
of the sphere is centered at 0. Otherwise, the *geometry* of
the sphere is shifted relative to the sphere's local coordinate system.
R,t (se3 transform, optional): if given, the sphere's world coordinates
will be rotated and shifted by this transform.
world (WorldModel, optional): If given, then the sphere will be a
RigidObjectModel or TerrainModel will be created in this world
name (str, optional): If world is given, this is the name of the object.
Default is 'sphere'.
mass (float, optional): If world is given and this is inf, then a
TerrainModel will be created. Otherwise, a RigidObjectModel
will be created with automatically determined inertia.
type (str, optional): the geometry type. Defaults to 'TriangleMesh',
but also 'GeometricPrimitive' and 'VolumeGrid' are accepted.
Returns:
Geometry3D, RigidObjectModel, or TerrainModel: A representation
of the sphere. If a world is given, then either a RigidObjectModel
or TerrainModel is added to the world and returned.
"""
if center is None:
center = [0, 0, 0]
prim = GeometricPrimitive()
prim.setSphere(center, radius)
geom = Geometry3D(prim)
if type != 'GeometricPrimitive':
geom = geom.convert(type)
if world is None:
if R is not None and t is not None:
geom.setCurrentTransform(R, t)
return geom
#want a RigidObjectModel or TerrainModel
if name is None:
name = 'sphere'
if mass != float('inf'):
bmass = Mass()
bmass.setMass(mass)
bmass.setCom(center)
bmass.setInertia([0.4 * mass * radius**2] * 3)
robj = world.makeRigidObject(name)
robj.geometry().set(geom)
robj.setMass(bmass)
if R is not None and t is not None:
robj.setTransform(R, t)
return robj
else:
tobj = world.makeTerrain(name)
if R is not None and t is not None:
geom.transform(R, t)
tobj.geometry().set(geom)
return tobj
def loadGeom(self,fn):
"""Loads a nonconvex volume from a Klamp't readable mesh (must be watertight)"""
g = Geometry3D()
g.loadFile(fn)
self.setGeom(g)
robot.setConfig(orig_config)
return reachable
w = WorldModel()
w.readFile("../data/robots/kinova_gen3_7dof.urdf")
w.readFile("../data/terrains/plane.env")
robot = w.robot(0)
obstacles = [w.terrain(0)]
ee_link = 'EndEffector_Link'
ee_local_pos = (0.15,0,0)
ee_local_axis = (1,0,0)
#put a "pen" geometry on the end effector
gpen = Geometry3D()
res = gpen.loadFile("../data/objects/cylinder.off")
assert res
gpen.scale(0.01,0.01,0.15)
gpen.rotate(so3.rotation((0,1,0),math.pi/2))
robot.link(7).geometry().setCurrentTransform(*se3.identity())
gpen.transform(*robot.link(8).getParentTransform())
robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry()))
robot.setConfig(robot.getConfig())
def show_workspace(grid):
vis.add("world",w)
res = numpy_convert.from_numpy((lower_corner,upper_corner,grid),'VolumeGrid')
g_workspace = Geometry3D(res)
g_surface = g_workspace.convert('TriangleMesh',0.5)
if g_surface.numElements() != 0:
def add_to_vis(self,robot=None,animate=True,base_xform=None):
"""Adds the gripper to the klampt.vis scene."""
from klampt import vis
from klampt import WorldModel,Geometry3D,GeometricPrimitive
from klampt.model.trajectory import Trajectory
prefix = "gripper_"+self.name
if robot is None and self.klampt_model is not None:
w = WorldModel()
if w.readFile(self.klampt_model):
robot = w.robot(0)
vis.add(prefix+"_gripper",w)
robotPath = (prefix+"_gripper",robot.getName())
elif robot is not None:
vis.add(prefix+"_gripper",robot)
robotPath = prefix+"_gripper"
if robot is not None:
assert self.base_link >= 0 and self.base_link < robot.numLinks()
robot.link(self.base_link).appearance().setColor(1,0.75,0.5)
if base_xform is None:
base_xform = robot.link(self.base_link).getTransform()
else:
if robot.link(self.base_link).getParent() >= 0:
print("Warning, setting base link transform for an attached gripper base")
#robot.link(self.base_link).setParent(-1)
robot.link(self.base_link).setParentTransform(*base_xform)
robot.setConfig(robot.getConfig())
for l in self.finger_links:
assert l >= 0 and l < robot.numLinks()
robot.link(l).appearance().setColor(1,1,0.5)
if robot is not None and animate:
q0 = robot.getConfig()
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(1)
else:
robot.driver(i).setValue(1)
traj = Trajectory([0],[robot.getConfig()])
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(0)
traj.times.append(traj.endTime()+0.5)
traj.milestones.append(robot.getConfig())
else:
robot.driver(i).setValue(0)
traj.times.append(traj.endTime()+1)
traj.milestones.append(robot.getConfig())
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
assert len(traj.times) == len(traj.milestones)
traj.checkValid()
vis.animate(robotPath,traj)
robot.setConfig(q0)
if self.center is not None:
vis.add(prefix+"_center",se3.apply(base_xform,self.center))
center_point = (0,0,0) if self.center is None else self.center
outer_point = (0,0,0)
if self.primary_axis is not None:
length = 0.1 if self.finger_length is None else self.finger_length
outer_point = vectorops.madd(self.center,self.primary_axis,length)
line = Trajectory([0,1],[self.center,outer_point])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_primary",line,color=(1,0,0,1))
if self.secondary_axis is not None:
width = 0.1 if self.maximum_span is None else self.maximum_span
line = Trajectory([0,1],[vectorops.madd(outer_point,self.secondary_axis,-0.5*width),vectorops.madd(outer_point,self.secondary_axis,0.5*width)])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_secondary",line,color=(0,1,0,1))
elif self.maximum_span is not None:
#assume vacuum gripper?
p = GeometricPrimitive()
p.setSphere(outer_point,self.maximum_span)
g = Geometry3D()
g.set(p)
vis.add(prefix+"_opening",g,color=(0,1,0,0.25))
prim.setSegment((0.05, 0.2, 0.0), (0.05, 0.2, 0.1))
point.geometry().setGeometricPrimitive(prim)
point.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.),
random.uniform(0., 1.), 1.)
#example AABB
aabb = world.makeRigidObject("AABB")
prim = GeometricPrimitive()
prim.setAABB((0., 0.3, 0.), (0.1, 0.4, 0.1))
aabb.geometry().setGeometricPrimitive(prim)
aabb.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.),
random.uniform(0., 1.), 0.5)
#example TriangleMesh
ladder = world.makeTerrain("TriangleMesh")
mesh = Geometry3D()
mesh.loadFile("../../data/terrains/drc_ladder.off")
mesh.transform([.033, 0, 0, 0, .033, 0, 0, 0, .033], [0.05, 0.5, 0])
ladder.geometry().set(mesh)
ladder.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.),
random.uniform(0., 1.), 1.)
#example Group
ladders = world.makeTerrain("Group")
ladders_geom = Geometry3D()
ladders_geom.setGroup()
for d in range(4):
mesh = Geometry3D()
mesh.loadFile("../../data/terrains/drc_ladder.off")
mesh.transform(so3.from_axis_angle(([0, 0, 1], math.pi / 2 * d)),
[0, 0, 0])