from klampt.robotsim import Geometry3D,WorldModel
from klampt.math import se3,so3,vectorops
from klampt.vis import colorize
import sys
import time
if len(sys.argv) > 1:
a = Geometry3D()
if not a.loadFile(sys.argv[1]):
print("Error loading",sys.argv[1])
exit()
else:
a = sphere(0.4,center=(0,0,0),type='TriangleMesh')
a = Geometry3D(a)
w = WorldModel()
w.makeRigidObject("a")
w.rigidObject(0).geometry().set(a)
#a_pc = a.convert("PointCloud",0.05)
bb = a.getBBTight()
a_pc = a.convert("PointCloud")
a_pc.setCurrentTransform(so3.identity(),[1.25*(bb[1][0]-bb[0][0]),0,0])
value = None
cmap = None
feature = None
lighting = None
vis.add("A",a)
vis.add("B",a_pc)
