前言
如果對機器人感興趣，要麼很土豪，可以買一個機器人來用用，但是也耐不住機器人型號太多，多買幾個，錢不是問題，主要沒地方放了。
錢都是裝在手機裏的，機器人當然也可以是虛擬的。關於機器人的仿真環境已經有很多了，以下是一個不完全的統計：
- AirSim - Simulator based on Unreal Engine for autonomous vehicles [github ]
- ARGoS - Physics-based simulator designed to simulate large-scale robot swarms [github ]
- ARTE - Matlab toolbox focussed on robotic manipulators [github ]
- CARLA - Open-source simulator for autonomous driving research [github ]
- Gazebo - Dynamic multi-robot simulator [bitbucket]
- GraspIt! - Simulator for grasping research that can accommodate arbitrary hand and robot designs [github ]
- Isaac - Nvidia's virtual simulator for robots
- MORSE - Modular open robots simulation engine [github ]
- Neurorobotics Platform - Internet-accessible simulation of robots controlled by spiking neural networks [bitbucket]
- PyBullet - An easy to use simulator for robotics and deep reinforcement learning [github ]
- V-REP - Virtual robot experimentation platform [github ]
- Webots - Robot simulator that provides a complete development environment [github ]
（更多參考）
gazebo 是ROS的標配，V-REP易用簡單。但是都還不夠靈活，我們能夠拿到的信息或者想要設置的信息往往並不提供。
這篇文章的目的，是自己寫一個機器人仿真器。
仿真器的名字暫定爲 RobotZen，項目地址，zen的意思是禪，RobotZen 的主旨是 發現工業世界裏面的禪。
一 URDF解析
RobotZen 基於panda3d 實現，panda3d是一個易用的python 3d 遊戲設計庫，直接 `pip install panda3d` 安裝。
機器人模型一般分爲urdf文件，以及各個連桿的幾何mesh文件，urdf（universal robot description file, 通用機器人描述文件）文件中會包含機器人的關節、連桿等所有信息，mesh文件是各種3d設計軟件導出來的3d模型，比如stl、obj、dae、blend、3ds等，如果不是很熟悉，可以參考以下ros的urdf教程。ROS的各種包中也找到各種機器人的模型文件。
panda3d 可以導入上述提到的所有類型的3d文件，實際上是用到了assimp這個庫，它可以導入超過40種不同3d格式的模型。
要導入機器人模型，首先就要解析機器人的urdf文件，以下代碼是利用python的xml庫對urdf文件進行解析。
#! /usr/bin/python3
"""
XmlParser.py
機器人urdf解析
"""
import os
import xml.etree.cElementTree as ET
from IPython import embed
import logging
logging.basicConfig(level=logging.WARN,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')
logger = logging.getLogger(__name__)
CWD = os.path.dirname(__file__)
class RobotTree:
def __init__(self, path):
self.m_urdf_path = path
self.m_urdf_dir = os.path.dirname(self.m_urdf_path)
self.m_tree = ET.parse(self.m_urdf_path)
self.m_robot = self.m_tree.getroot()
self.joint_names = []
self.link_names = []
self.m_links = dict()
self.m_joints = dict()
self.m_dof = 0
self.__Init()
def GetRGBA(self):
material = self.m_robot.find("material")
rgba = material.find("color").get("rgba", "0.12941 0.14902 0.74902 1")
rgba = [float(v) for v in rgba.split()]
return rgba
def __GetLinks(self):
# TODO: check the sequence
self.m_links = {}
links = self.m_robot.findall("link")
for l in links:
link = {}
link["name"] = l.get("name")
link["parent"] = ""
link["child"] = ""
link["visual_mesh"] = ""
link["collision_mesh"] = ""
if link["name"] != "ee_link":
f = l.find("visual").find(
"geometry").find("mesh").get("filename").split("//")[1]
link["visual_mesh"] = os.path.join(self.m_urdf_dir, f)
f = l.find("collision").find(
"geometry").find("mesh").get("filename").split("//")[1]
link["collision_mesh"] = os.path.join(self.m_urdf_dir, f)
self.m_links[link["name"]] = link
def __GetJoints(self):
# TODO: check the sequence
self.m_joints = {}
self.m_dof = 0
joints = self.m_robot.findall("joint")
for j in joints:
joint = {"limit": {}}
joint["name"] = j.get("name")
joint["type"] = j.get("type")
joint["parent"] = j.find("parent").get("link")
joint["child"] = j.find("child").get("link")
self.m_links[joint["child"]]["parent"] = joint["name"]
self.m_links[joint["parent"]]["child"] = joint["name"]
if joint["type"] != "fixed":
self.m_dof += 1
joint["origin_xyz"] = [
float(v) for v in j.find("origin").get("xyz").split()]
joint["origin_rpy"] = [
float(v) for v in j.find("origin").get("rpy").split()]
joint["axis"] = [float(v)
for v in j.find("axis").get("xyz").split()]
joint["limit"]["effort"] = float(
j.find("limit").get("effort", 0))
joint["limit"]["upper"] = float(
j.find("limit").get("upper", 3))
joint["limit"]["lower"] = float(
j.find("limit").get("lower", -3))
joint["limit"]["velocity"] = float(
j.find("limit").get("velocity", 1))
joint["limit"]["acceleration"] = float(
j.find("limit").get("acceleration", 5))
self.m_joints[joint["name"]] = joint
def __Init(self):
self.__GetLinks()
self.__GetJoints()
for l in self.m_links.values():
if not l["parent"]:
self.m_root_link = l["name"]
logger.info("Robot chain: {}".format(self.GetChain()))
logger.info("Robot links: {}".format(self.link_names))
logger.info("Robot joints: {}".format(self.joint_names))
def GetRootLink(self):
return self.m_root_link
def GetLinks(self):
return self.m_links
def GetJoints(self):
return self.m_joints
def GetChain(self):
chain = []
node = self.m_links[self.GetRootLink()]
self.joint_names = []
self.link_names = []
while node["child"]:
chain.append(node["name"])
child = node["child"]
node = self.m_joints.get(child, False)
if node:
self.joint_names.append(node["name"])
else:
node = self.m_links.get(child, False)
if node:
self.link_names.append(node["name"])
else:
logger.error("node child {} not found!".format(child))
break
chain.append(node["name"])
return chain
if __name__ == "__main__":
CWD = os.path.dirname(__file__)
upper_dir = os.path.dirname(CWD)
filename = os.path.join(upper_dir, "Models/GP12/GP12.urdf")
tree = RobotTree(filename)
# print(tree.GetLinks())
# print(tree.GetJoints())
# print("root: ", tree.GetRootLink())
# print(tree.GetChain())
GetChain() 函數返回機器人從根節點到末端節點經過的所有關節、連桿，同時也可以通過link的屬性visual_mesh，拿到3d模型文件的路徑。
二 模型導入及控制
panda3d 可以依次導入link模型，導入的模型在panda3d中存爲NodePath，NodePath是Node的一個封裝。最基礎的Node是render，一個Node可以reparentTo到其他的Node，只有直接或者間接reparetTo到render的Node模型，纔會被顯示出來。同時設置一個Node的位置（pos) 、姿態（hpr，歐拉角），會影響到其下面的所有子Node，利用這一特性，根據機器人的關節鏈，如果旋轉了第一個關節，因爲後面的連桿都是第一個關節的子節點，所以會自動跟着旋轉，同樣的，旋轉第二個關節，不會影響前面的節點，只會帶動後面的關節跟着旋轉。
關於控制，機器人有若干關節，我們可以在界面上生成一些按鈕，來控制機器人關節的旋轉。最終的界面如圖所示：
模型導入及生成控制界面的按鈕代碼如下：
#! /usr/bin/python3
"""
ZenWorld.py
模型導入及控制界面
"""
from math import pi, sin, cos
import os
import random
import sys
from threading import Thread
import numpy as np
from panda3d.core import TextNode
from direct.actor.Actor import Actor
from direct.gui.DirectGui import DirectButton, DirectEntryScroll, DirectEntry
import direct.gui.DirectGuiGlobals as DGG
from direct.gui.OnscreenText import OnscreenText
from direct.showbase.ShowBase import (
Filename, LVecBase3f, NodePath, ShowBase, Task)
from XmlParser import RobotTree
def addInstructions(pos, msg):
return OnscreenText(text=msg, style=1, fg=(0, 0, 1, 1), scale=.05,
shadow=(0, 0, 0, 0), parent=base.a2dBottomLeft,
pos=(0.08, pos), align=TextNode.ABoxedLeft)
CWD = os.path.dirname(__file__)
upper_dir = os.path.dirname(CWD)
model_path = os.path.join(upper_dir, "Models/GP12/GP12.urdf")
class ZenWorld(ShowBase):
def __init__(self):
ShowBase.__init__(self)
self.set_background_color(0.9, 0.9, 0.9, 0)
class RobotWorld(ZenWorld):
def __init__(self):
ZenWorld.__init__(self)
self.m_models = [self.render]
# load robot from urdf
robot_tree = RobotTree(model_path)
self.robot_tree = robot_tree
rgba = robot_tree.GetRGBA()
self.joints = robot_tree.GetJoints()
self.links = robot_tree.GetLinks()
for node in robot_tree.GetChain():
link = self.links.get(node, False)
if not link or link.get("visual_mesh", "") == "":
continue
model = self.loader.loadModel(
Filename.from_os_specific(link["visual_mesh"]))
model.reparent_to(self.m_models[-1])
model.set_color(*rgba)
self.m_models.append(model)
if link["parent"]:
parent_joint_pos = self.joints[link["parent"]]["origin_xyz"]
parent_joint_rpy = self.joints[link["parent"]]["origin_rpy"]
# panda3d has a different xyz sequence
model.set_pos(parent_joint_pos[0],
parent_joint_pos[2], parent_joint_pos[1])
model.set_hpr(parent_joint_rpy[0],
parent_joint_rpy[1], parent_joint_rpy[2])
self.joints_move_direction = [0, 0, 0, 0, 0, 0]
self.disable_mouse()
self.camera.set_pos(0, -5, 5)
self.camera.set_hpr(0, -45, 0)
self.oobe(cam=self.camera)
self.key_map = {}
self.accept("d", self.SetKey, ["cam_xplus", True])
self.accept("d-up", self.SetKey, ["cam_xplus", False])
self.accept("a", self.SetKey, ["cam_xminus", True])
self.accept("a-up", self.SetKey, ["cam_xminus", False])
self.accept("w", self.SetKey, ["cam_yplus", True])
self.accept("w-up", self.SetKey, ["cam_yplus", False])
self.accept("s", self.SetKey, ["cam_yminus", True])
self.accept("s-up", self.SetKey, ["cam_yminus", False])
self.accept("arrow_up", self.SetKey, ["cam_zplus", True])
self.accept("arrow_up-up", self.SetKey, ["cam_zplus", False])
self.accept("arrow_down", self.SetKey, ["cam_zminus", True])
self.accept("arrow_down-up", self.SetKey, ["cam_zminus", False])
self.accept("q", self.SetKey, ["cam_hplus", True])
self.accept("q-up", self.SetKey, ["cam_hplus", False])
self.accept("e", self.SetKey, ["cam_hminus", True])
self.accept("e-up", self.SetKey, ["cam_hminus", False])
self.accept("i", self.SetKey, ["cam_pplus", True])
self.accept("i-up", self.SetKey, ["cam_pplus", False])
self.accept("k", self.SetKey, ["cam_pminus", True])
self.accept("k-up", self.SetKey, ["cam_pminus", False])
self.accept("arrow_left", self.SetKey, ["cam_rplus", True])
self.accept("arrow_left-up", self.SetKey, ["cam_rplus", False])
self.accept("arrow_right", self.SetKey, ["cam_rminus", True])
self.accept("arrow_right-up", self.SetKey, ["cam_rminus", False])
self.accept("space", self.SetKey, ["cam_origin", True])
self.accept("space-up", self.SetKey, ["cam_origin", False])
self.accept("escape", sys.exit, [0])
self.accept("p", self.ExecCmdNewThrd)
self.accept("f1", self.JointControlPanel)
addInstructions(0.1, "[f1]: Show/Toggle joint control panel")
addInstructions(0.2, "[space]: Look at origin")
addInstructions(0.3, "[w/s]: zoom in/out")
self.joint_panel_show = False
# # Add the spinCameraTask procedure to the task manager.
# self.taskMgr.add(self.spinCameraTask, "SpinCameraTask")
self.taskMgr.add(self.UpdateCameraTask, "UpdateCameraTask")
self.taskMgr.add(self.UpdateJointTask, "UpdateJointTask")
self.move_js_frame = []
# entry = DirectEntry(parent=self.a2dBottomLeft, text="",
# pos=LVecBase3f(0.1, 0.2), scale=0.5, text_scale=0.5)
# self.editor = DirectEntryScroll(entry=entry, parent=self.a2dBottomLeft)
def JointControlPanel(self):
if self.joint_panel_show:
self.DestroyJointControlPanel()
self.joint_panel_show = False
else:
self.ShowJointControlPanel()
self.joint_panel_show = True
def DestroyJointControlPanel(self):
for button in self.move_js_frame:
button.destroy()
def ShowJointControlPanel(self):
for i in range(6):
self.move_js_frame.append(DirectButton(text=("J{}+".format(i+1), "Moving", "J{}+".format(i+1), "disabled"),
scale=.08, command=lambda x=i: self.SetJointMoveDirection(x, 1),
pos=LVecBase3f(0.1, -0.1 - i * 0.11), relief=DGG.GROOVE, parent=self.a2dTopLeft, frameSize=(-1.5, 1.5, -0.4, 0.9)
))
for i in range(6):
self.move_js_frame.append(DirectButton(text=("J{}-".format(i+1), "Moving", "J{}-".format(i+1), "disabled"),
scale=.08, command=lambda x=i: self.SetJointMoveDirection(x, -1),
pos=LVecBase3f(0.32, -0.1 - i * 0.11), relief=DGG.GROOVE, parent=self.a2dTopLeft, frameSize=(-1.5, 1.5, -0.4, 0.9)
))
for i in range(6):
self.move_js_frame.append(DirectButton(text=("Stop J{}".format(i+1), "Stop", "Stop J{}".format(i+1), "disabled"),
scale=.08, command=lambda x=i: self.SetJointMoveDirection(x, 0),
pos=LVecBase3f(0.6, -0.1 - i * 0.11), relief=DGG.GROOVE, parent=self.a2dTopLeft, frameSize=(-2.0, 2.0, -0.4, 0.9)
))
def UpdateCameraTask(self, task):
# time since last frame
dt = globalClock.getDt()
step = 5
if self.key_map.get("cam_xplus", False):
self.camera.setX(self.camera, step*dt)
if self.key_map.get("cam_xminus", False):
self.camera.setX(self.camera, -step*dt)
if self.key_map.get("cam_yplus", False):
self.camera.setY(self.camera, step*dt)
if self.key_map.get("cam_yminus", False):
self.camera.setY(self.camera, -step*dt)
if self.key_map.get("cam_zplus", False):
self.camera.setZ(self.camera, step*dt)
if self.key_map.get("cam_zminus", False):
self.camera.setZ(self.camera, -step*dt)
if self.key_map.get("cam_hplus", False):
self.camera.setH(self.camera, step*dt)
if self.key_map.get("cam_hminus", False):
self.camera.setH(self.camera, -step*dt)
if self.key_map.get("cam_pplus", False):
self.camera.setP(self.camera, step*dt)
if self.key_map.get("cam_pminus", False):
self.camera.setP(self.camera, -step*dt)
if self.key_map.get("cam_rplus", False):
self.camera.setR(self.camera, step*dt)
if self.key_map.get("cam_rminus", False):
self.camera.setR(self.camera, -step*dt)
if self.key_map.get("cam_origin", False):
self.camera.setPos(0, -1, 1)
self.camera.lookAt(self.render)
return Task.cont
def UpdateJointTask(self, task):
step = 0.4
for i, direction in enumerate(self.joints_move_direction):
if direction > 0:
self.MoveJ(step, i)
elif direction < 0:
self.MoveJ(-step, i)
return Task.cont
def ExecCmdNewThrd(self):
Thread(target=self.ExecCmd).start()
def SetKey(self, key, val):
self.key_map[key] = val
def SetJointMoveDirection(self, joint_num, direction):
self.joints_move_direction[joint_num] = direction
def ExecCmd(self):
while True:
s = input("input cmd:")
if s == "exit":
print("bye!")
break
try:
print(eval(s))
except Exception as e:
print(e)
def SpinCameraTask(self, task: Task):
angleDegrees = task.time * 6.0
angleRadians = angleDegrees * (pi / 180.0)
distance = 10
x = distance * sin(angleRadians)
y = - distance * cos(angleRadians)
z = distance
self.camera.setPos(x, y, z)
self.camera.setHpr(angleDegrees, -45, 0)
return Task.cont
def MoveJ(self, degree=1, joint_num=0):
J = self.joints[self.robot_tree.joint_names[joint_num]]
x, y, z = J.get("axis", np.zeros(0))
joints_inc = LVecBase3f(y, x, z) * degree
node_J = self.m_models[2+joint_num]
node_J.setHpr(node_J, joints_inc)
def GetJointPosition(self):
joints = []
for l in self.m_models[2:]:
joints.append(sum(l.getHpr()))
return joints
if __name__ == "__main__":
app = RobotWorld()
app.run()
其中w/s/a/d和上下箭頭控制視角的前後左右上下移動，q/e視角左右旋轉，i/k視角上下旋轉，空格鍵使視角正對着原點（機器人），運行軟件後，可能看不到機器人，這是因爲視角問題，按一個空格鍵就可以，鼠標中鍵按住可以旋轉視角，左鍵可以移動模型。
總結
利用panda3d這個方便的遊戲軟件庫，我們卻實現了一個機器人仿真環境，當然目前只是實現了最簡單的模型導入及關節控制，後續還需要對模型的顯示進行優化，提供不同的顯示風格。機器人控制要考慮關節的極限，能夠向外部提供穩定可靠的控制接口。此外，最好能夠以服務端/客戶端模型運行，RobotZen 開啓一個socket服務端，客戶端連接到服務端後，可以發送關節位置，控制機器人，這就有點類似ROS了。
對機器人運動規劃有興趣的不妨看一下這個PathPlanning項目，實現了基於A*，RRT，勢場法的2d路徑規劃。