使用VTK與Python實現機械臂三維模型可視化

本文轉載自查看原文 2017-03-02 17:40 13166 python/ VTK

　　三維可視化系統的建立依賴於三維圖形平台，如 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()