手把手地写一个机器人仿真环境---RobotZen

前言

如果对机器人感兴趣,要么很土豪,可以买一个机器人来用用,但是也耐不住机器人型号太多,多买几个,钱不是问题,主要没地方放了。

钱都是装在手机里的,机器人当然也可以是虚拟的。关于机器人的仿真环境已经有很多了,以下是一个不完全的统计:

  • 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文件进行解析。

"""
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__":
    filename = "xxxx.urdf"
    tree = RobotTree(filename)

GetChain() 函数返回机器人从根节点到末端节点经过的所有关节、连杆,同时也可以通过link的属性visual_mesh,拿到3d模型文件的路径。

二 模型导入及控制

panda3d 可以依次导入link模型,导入的模型在panda3d中存为NodePath,NodePath是Node的一个封装。最基础的Node是render,一个Node可以reparentTo到其他的Node,只有直接或者间接reparetTo到render的Node模型,才会被显示出来。同时设置一个Node的位置(pos) 、姿态(hpr,欧拉角),会影响到其下面的所有子Node,利用这一特性,根据机器人的关节链,如果旋转了第一个关节,因为后面的连杆都是第一个关节的子节点,所以会自动跟着旋转,同样的,旋转第二个关节,不会影响前面的节点,只会带动后面的关节跟着旋转。

关于控制,机器人有若干关节,我们可以在界面上生成一些按钮,来控制机器人关节的旋转。最终的界面如图所示:

RobotZen 界面

模型导入及生成控制界面的按钮代码如下:

#! /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路径规划。

©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页