VTK与Python实现机械臂三维模型可视化详解

更新时间：2020-05-14 07:06:01 作者：startmvc

三维可视化系统的建立依赖于三维图形平台，如OpenGL、VTK、OGRE、OSG等，传统的方法多采用Op

三维可视化系统的建立依赖于三维图形平台，如 OpenGL、VTK、OGRE、OSG等，传统的方法多采用OpenGL进行底层编程，即对其特有的函数进行定量操作，需要开发人员熟悉相关函数，从而造成了开发难度大、周期长等问题。VTK、 ORGE、OSG等平台使用封装更好的函数简化了开发过程。下面将使用Python与VTK进行机器人上位机监控界面的快速原型开发。

　　完整的上位机程序需要有三维显示模块、机器人信息监测模块（位置/角度/速度/电量/温度/错误信息...）、通信模块（串口/USB/WIFI/蓝牙...）、控制模块等功能模块。三维显示模块主要用于实时显示机器人的姿态（或位置）信息。比如机器人上肢手臂抬起，程序界面中的虚拟机器人也会同时进行同样的动作。三维显示模块也可以用于对机器人进行控制，实现良好的人机交互。比如在三维图像界面中可以点击拾取机器人某一关节，拖拽部件(肢体)控制真实的机器人完成同样的运动。Aldebaran Robotics的图形化编程软件Choregraphe可以完成上述的一些功能对NAO机器人进行控制。

对于简单的模型可以自己编写函数进行创建，但这种方法做出来的模型过于简单不够逼真。因此可以先在SolidWorks、Blender、3DMax、Maya、Rhino等三维设计软件中建立好模型，然后导出为通用的三维文件格式，再使用VTK将其读入并进行渲染。

　　在SolidWorks等三维设计软件中设计好机器人的大臂（upperarm）和小臂（forearm），然后创建装配体如下图所示。在将装配体导出为STL文件前需要注意几点：

　　1. 当从外界读入STL类型的模型时，其会按照它内部的坐标位置进行显示，因此它的位置和大小是确定的。为了以后的定位以及移动、旋转等操作的方便，需要先在SolidWorks中创建一个坐标系。如下图所示，坐标系建立在大臂关节中心点。

　　2. 如果将装配体整体输出为一个STL文件，则导入VTK后无法控制零部件进行相对运动。因此，需要将装配体各可动部件分别导出。

在SolidWorks的另存为STL对话框中，点开输出选项卡，如下图所示。注意之前提到的几点：如果勾选“在单一文件中保存装配体的所有零部件”则会将整个装配体导出为一个STL文件，否则就是分别命名的两个STL文件；输出坐标系下拉列表中选择之前创建的坐标系1，并勾选“不要转换STL输出数据到正的坐标空间”。

下面的Python代码简单实现了一个2自由度机械臂的三维仿真，可以拖动滑块或按键盘上的方向键控制肩关节或肘关节运动。当然程序还存在一些问题有待完善...


#!/usr/bin/env python
 import vtk
import math
from vtk.util.colors import *
filenames = ["upperarm.stl","forearm.stl"]
dt = 1.0 # degree step in rotation
angle = [0, 0] # shoulder and elbow joint angle
renWin = vtk.vtkRenderWindow()
assembly = vtk.vtkAssembly()
slider_shoulder = vtk.vtkSliderRepresentation2D()
slider_elbow = vtk.vtkSliderRepresentation2D()
actor = list() # the list of links
# Customize vtkInteractorStyleTrackballCamera 
class MyInteractor(vtk.vtkInteractorStyleTrackballCamera):
 def __init__(self,parent=None):
 self.AddObserver("CharEvent",self.OnCharEvent)
 self.AddObserver("KeyPressEvent",self.OnKeyPressEvent)
 # Override the default key operations which currently handle trackball or joystick styles is provided
 # OnChar is triggered when an ASCII key is pressed. Some basic key presses are handled here 
 def OnCharEvent(self,obj,event):
 pass
 def OnKeyPressEvent(self,obj,event):
 global angle
 # Get the compound key strokes for the event
 key = self.GetInteractor().GetKeySym()
 # Output the key that was pressed
 #print "Pressed: " , key
 # Handle an arrow key
 if(key == "Left"):
 actor[1].RotateY(-dt) 
 if(key == "Right"):
 actor[1].RotateY(dt) 
 if(key == "Up"):
 assembly.RotateY(-dt)
 angle[0] += dt
 if angle[0] >= 360.0:
 angle[0] -= 360.0
 slider_shoulder.SetValue(angle[0]) 
 if(key == "Down"):
 assembly.RotateY(dt)
 angle[0] -= dt
 if angle[0] < 0.0:
 angle[0] += 360.0 
 slider_shoulder.SetValue(angle[0])
 # Ask each renderer owned by this RenderWindow to render its image and synchronize this process
 renWin.Render()
 return
def LoadSTL(filename):
 reader = vtk.vtkSTLReader()
 reader.SetFileName(filename)
 mapper = vtk.vtkPolyDataMapper() # maps polygonal data to graphics primitives
 mapper.SetInputConnection(reader.GetOutputPort())
 actor = vtk.vtkLODActor() 
 actor.SetMapper(mapper)
 return actor # represents an entity in a rendered scene
def CreateCoordinates():
 # create coordinate axes in the render window
 axes = vtk.vtkAxesActor() 
 axes.SetTotalLength(100, 100, 100) # Set the total length of the axes in 3 dimensions 
 # Set the type of the shaft to a cylinder:0, line:1, or user defined geometry. 
 axes.SetShaftType(0) 
 axes.SetCylinderRadius(0.02) 
 axes.GetXAxisCaptionActor2D().SetWidth(0.03) 
 axes.GetYAxisCaptionActor2D().SetWidth(0.03) 
 axes.GetZAxisCaptionActor2D().SetWidth(0.03) 
 #axes.SetAxisLabels(0) # Enable:1/disable:0 drawing the axis labels
 #transform = vtk.vtkTransform() 
 #transform.Translate(0.0, 0.0, 0.0)
 #axes.SetUserTransform(transform)
 #axes.GetXAxisCaptionActor2D().GetCaptionTextProperty().SetColor(1,0,0)
 #axes.GetXAxisCaptionActor2D().GetCaptionTextProperty().BoldOff() # disable text bolding
 return axes
def ShoulderSliderCallback(obj,event):
 sliderRepres = obj.GetRepresentation()
 pos = sliderRepres.GetValue() 
 assembly.SetOrientation(0,-pos,0)

 renWin.Render()
def ElbowSliderCallback(obj,event):
 sliderRepres = obj.GetRepresentation()
 pos = sliderRepres.GetValue() 
 actor[1].SetOrientation(0,-pos,0)
 renWin.Render()
def ConfigSlider(sliderRep, TitleText, Yaxes):
 sliderRep.SetMinimumValue(0.0)
 sliderRep.SetMaximumValue(360.0)
 sliderRep.SetValue(0.0) # Specify the current value for the widget
 sliderRep.SetTitleText(TitleText) # Specify the label text for this widget
 sliderRep.GetSliderProperty().SetColor(1,0,0) # Change the color of the knob that slides
 sliderRep.GetSelectedProperty().SetColor(0,0,1) # Change the color of the knob when the mouse is held on it
 sliderRep.GetTubeProperty().SetColor(1,1,0) # Change the color of the bar 
 sliderRep.GetCapProperty().SetColor(0,1,1) # Change the color of the ends of the bar
 #sliderRep.GetTitleProperty().SetColor(1,0,0) # Change the color of the text displaying the value
 # Position the first end point of the slider
 sliderRep.GetPoint1Coordinate().SetCoordinateSystemToDisplay()
 sliderRep.GetPoint1Coordinate().SetValue(50, Yaxes) 
 # Position the second end point of the slider
 sliderRep.GetPoint2Coordinate().SetCoordinateSystemToDisplay()
 sliderRep.GetPoint2Coordinate().SetValue(400, Yaxes) 
 sliderRep.SetSliderLength(0.02) # Specify the length of the slider shape.The slider length by default is 0.05
 sliderRep.SetSliderWidth(0.02) # Set the width of the slider in the directions orthogonal to the slider axis
 sliderRep.SetTubeWidth(0.005)
 sliderRep.SetEndCapWidth(0.03)

 sliderRep.ShowSliderLabelOn() # display the slider text label
 sliderRep.SetLabelFormat("%.1f")

 sliderWidget = vtk.vtkSliderWidget()
 sliderWidget.SetRepresentation(sliderRep)
 sliderWidget.SetAnimationModeToAnimate()

 return sliderWidget
def CreateGround():
 # create plane source
 plane = vtk.vtkPlaneSource()
 plane.SetXResolution(50)
 plane.SetYResolution(50)
 plane.SetCenter(0,0,0)
 plane.SetNormal(0,0,1) 
 # mapper
 mapper = vtk.vtkPolyDataMapper()
 mapper.SetInputConnection(plane.GetOutputPort())
 
 # actor
 actor = vtk.vtkActor()
 actor.SetMapper(mapper)
 actor.GetProperty().SetRepresentationToWireframe()
 #actor.GetProperty().SetOpacity(0.4) # 1.0 is totally opaque and 0.0 is completely transparent
 actor.GetProperty().SetColor(light_grey)
 '''
 # Load in the texture map. A texture is any unsigned char image.
 bmpReader = vtk.vtkBMPReader() 
 bmpReader.SetFileName("ground_texture.bmp") 
 texture = vtk.vtkTexture() 
 texture.SetInputConnection(bmpReader.GetOutputPort()) 
 texture.InterpolateOn() 
 actor.SetTexture(texture)
 '''
 transform = vtk.vtkTransform()
 transform.Scale(2000,2000, 1)
 actor.SetUserTransform(transform)
 return actor 
def CreateScene():
 # Create a rendering window and renderer
 ren = vtk.vtkRenderer()
 #renWin = vtk.vtkRenderWindow()
 renWin.AddRenderer(ren)
 # Create a renderwindowinteractor
 iren = vtk.vtkRenderWindowInteractor()
 iren.SetRenderWindow(renWin)
 style = MyInteractor()
 style.SetDefaultRenderer(ren)
 iren.SetInteractorStyle(style)
 for id, file in enumerate(filenames):
 actor.append(LoadSTL(file))
 #actor[id].GetProperty().SetColor(blue)
 r = vtk.vtkMath.Random(.4, 1.0)
 g = vtk.vtkMath.Random(.4, 1.0)
 b = vtk.vtkMath.Random(.4, 1.0)
 actor[id].GetProperty().SetDiffuseColor(r, g, b)
 actor[id].GetProperty().SetDiffuse(.8)
 actor[id].GetProperty().SetSpecular(.5)
 actor[id].GetProperty().SetSpecularColor(1.0,1.0,1.0)
 actor[id].GetProperty().SetSpecularPower(30.0)
 assembly.AddPart(actor[id])
 # Add the actors to the scene
 #ren.AddActor(actor[id])
 # Also set the origin, position and orientation of assembly in space.
 assembly.SetOrigin(0, 0, 0) # This is the point about which all rotations take place 
 #assembly.AddPosition(0, 0, 0)
 #assembly.RotateX(45)
 actor[1].SetOrigin(274, 0, 0) # initial elbow joint position
 ren.AddActor(assembly)
 # Add coordinates
 axes = CreateCoordinates()
 ren.AddActor(axes)

 # Add ground
 ground = CreateGround()
 ren.AddActor(ground)

 # Add slider to control the robot
 sliderWidget_shoulder = ConfigSlider(slider_shoulder,"Shoulder Joint", 80)
 sliderWidget_shoulder.SetInteractor(iren)
 sliderWidget_shoulder.EnabledOn()
 sliderWidget_shoulder.AddObserver("InteractionEvent", ShoulderSliderCallback)

 sliderWidget_elbow = ConfigSlider(slider_elbow,"Elbow Joint", 160)
 sliderWidget_elbow.SetInteractor(iren)
 sliderWidget_elbow.EnabledOn()
 sliderWidget_elbow.AddObserver("InteractionEvent", ElbowSliderCallback)

 # Set background color
 ren.SetBackground(.2, .2, .2)

 # Set window size
 renWin.SetSize(600, 600)

 # Set up the camera to get a particular view of the scene
 camera = vtk.vtkCamera()
 camera.SetFocalPoint(300, 0, 0)
 camera.SetPosition(300, -400, 350)
 camera.ComputeViewPlaneNormal()
 camera.SetViewUp(0, 1, 0)
 camera.Zoom(0.4)
 ren.SetActiveCamera(camera)
 # Enable user interface interactor
 iren.Initialize()
 iren.Start()
if __name__ == "__main__":
 CreateScene()