VTK與Python實現(xiàn)機械臂三維模型可視化詳解

三維可視化系統(tǒng)的建立依賴于三維圖形平臺， 如 OpenGL、VTK、OGRE、OSG等， 傳統(tǒng)的方法多采用OpenGL進行底層編程，即對其特有的函數(shù)進行定量操作， 需要開發(fā)人員熟悉相關(guān)函數(shù)， 從而造成了開發(fā)難度大、 周期長等問題。VTK、 ORGE、OSG等平臺使用封裝更好的函數(shù)簡化了開發(fā)過程。下面將使用Python與VTK進行機器人上位機監(jiān)控界面的快速原型開發(fā)。

完整的上位機程序需要有三維顯示模塊、機器人信息監(jiān)測模塊（位置/角度/速度/電量/溫度/錯誤信息...）、通信模塊（串口/USB/WIFI/藍牙...）、控制模塊等功能模塊。三維顯示模塊主要用于實時顯示機器人的姿態(tài)（或位置）信息。比如機器人上肢手臂抬起，程序界面中的虛擬機器人也會同時進行同樣的動作。三維顯示模塊也可以用于對機器人進行控制，實現(xiàn)良好的人機交互。比如在三維圖像界面中可以點擊拾取機器人某一關(guān)節(jié)，拖拽部件(肢體)控制真實的機器人完成同樣的運動。Aldebaran Robotics的圖形化編程軟件Choregraphe可以完成上述的一些功能對NAO機器人進行控制。

對于簡單的模型可以自己編寫函數(shù)進行創(chuàng)建，但這種方法做出來的模型過于簡單不夠逼真。因此可以先在SolidWorks、Blender、3DMax、Maya、Rhino等三維設(shè)計軟件中建立好模型，然后導出為通用的三維文件格式，再使用VTK將其讀入并進行渲染。

在SolidWorks等三維設(shè)計軟件中設(shè)計好機器人的大臂（upperarm）和小臂（forearm），然后創(chuàng)建裝配體如下圖所示。在將裝配體導出為STL文件前需要注意幾點：

1. 當從外界讀入STL類型的模型時，其會按照它內(nèi)部的坐標位置進行顯示，因此它的位置和大小是確定的。為了以后的定位以及移動、旋轉(zhuǎn)等操作的方便，需要先在SolidWorks中創(chuàng)建一個坐標系。如下圖所示，坐標系建立在大臂關(guān)節(jié)中心點。

2. 如果將裝配體整體輸出為一個STL文件，則導入VTK后無法控制零部件進行相對運動。因此，需要將裝配體各可動部件分別導出。

在SolidWorks的另存為STL對話框中，點開輸出選項卡，如下圖所示。注意之前提到的幾點：如果勾選“在單一文件中保存裝配體的所有零部件”則會將整個裝配體導出為一個STL文件，否則就是分別命名的兩個STL文件；輸出坐標系下拉列表中選擇之前創(chuàng)建的坐標系1，并勾選“不要轉(zhuǎn)換STL輸出數(shù)據(jù)到正的坐標空間”。

下面的Python代碼簡單實現(xiàn)了一個2自由度機械臂的三維仿真，可以拖動滑塊或按鍵盤上的方向鍵控制肩關(guān)節(jié)或肘關(guān)節(jié)運動。當然程序還存在一些問題有待完善...

#!/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()

下面是使用MFC搭建的機器

下面是使用MFC搭建的機器人上位機監(jiān)控平臺，可以實現(xiàn)上述的一些基本功能。這個GIF動畫使用開源軟件ScreenToGif生成，非常好用！

總結(jié)

以上就是本文關(guān)于VTK與Python實現(xiàn)機械臂三維模型可視化詳解的全部內(nèi)容，希望對大家有所幫助。