New features for visualizer, and issue

The new visualizer is intended for debugging urdf's, and visualizing robot states. Currently, its use is fairly limited. This is some features that would be good to implement, and also a bug that needs fixing.

[bug] Visualize links. Currently I believe there is a bug in the following method. When I try to visualize the robot links, the center of the link is correctly positioned, but the cylinders that make up the axes are all in the wrong place. Update: I will just use lines for the axes rather than cylinders. See 617e216.

optas/optas/visualize.py

Lines 217 to 252 in ee6e02f

    
           def create_cylinder_actor(start, end, radius): 
        
               s = cs.np.array(start) 
        
               e = cs.np.array(end) 
        
               a = e - s 
        
               l = cs.np.linalg.norm(a) 
        
               p = 0.5*a 
        
               y = a / l 
        
               temp = cs.np.random.random(3) 
        
               temp /= cs.np.linalg.norm(temp) 
        
               x = cs.np.cross(y, temp) 
        
               z = cs.np.cross(x, y) 
        
               T = cs.np.eye(4) 
        
               T[:3, 0] = x 
        
               T[:3, 1] = y 
        
               T[:3, 2] = z 
        
               T[:3, 3] = p 
        
               cylinder = vtkCylinderSource() 
        
               cylinder.SetRadius(radius) 
        
               cylinder.SetHeight(l) 
        
               cylinder.SetResolution(20) 
        
               mapper = vtk.vtkPolyDataMapper() 
        
               mapper.SetInputConnection(cylinder.GetOutputPort()) 
        
               actor = vtk.vtkActor() 
        
               actor.SetMapper(mapper) 
        
               transform = vtk.vtkTransform() 
        
               transform.SetMatrix(T.flatten().tolist()) 
        
               actor.SetUserTransform(transform) 
        
               return actor

[feature] Support materials from the urdf. Currently, links are visualized in white. The colors ~~and potentially textures~~ should be supported.
[feature] Visualize additional objects (e.g. boxes/spheres).
[feature] Visualize robot motion, i.e. a planned trajectory. Option: video-link motion, and also a static figure where older states decrease the opacity of the robot.
[feature] Support loading several robots at once.
[feature] Support visualization of task models/plans (e.g. a 3D trajectory represented by a sphere flying through space)
[feature] Support visualization of other meshes (e.g. tables)
[feature] The above may need a revamp of the RobotVisualizer class, and also the visualization.py example script will need to be updated

Closing, once #88 is merged this is resolved.

	def create_cylinder_actor(start, end, radius):

	s = cs.np.array(start)
	e = cs.np.array(end)
	a = e - s
	l = cs.np.linalg.norm(a)
	p = 0.5*a
	y = a / l

	temp = cs.np.random.random(3)
	temp /= cs.np.linalg.norm(temp)

	x = cs.np.cross(y, temp)
	z = cs.np.cross(x, y)
	T = cs.np.eye(4)
	T[:3, 0] = x
	T[:3, 1] = y
	T[:3, 2] = z
	T[:3, 3] = p

	cylinder = vtkCylinderSource()
	cylinder.SetRadius(radius)
	cylinder.SetHeight(l)
	cylinder.SetResolution(20)

	mapper = vtk.vtkPolyDataMapper()
	mapper.SetInputConnection(cylinder.GetOutputPort())

	actor = vtk.vtkActor()
	actor.SetMapper(mapper)

	transform = vtk.vtkTransform()
	transform.SetMatrix(T.flatten().tolist())
	actor.SetUserTransform(transform)

	return actor