概要

Open Dynamics Engine (ODE) を用いて、全方向移動ロボットの位置制御を PID 制御で行う場合のサンプルを記載します。こちらのページに記載の差分駆動型ロボットと比較して、全方向移動ロボットは任意の方向に移動できるため位置制御が容易です。

モータの角速度を操作することでロボットの位置を制御

目標値 $x_d$ と現在時刻における測定値 $x(t)$ の残差 $e(t)$ が 0 になるように、操作量 $u(t)$ であるモータの角速度を入力していきます。$K$ は比例ゲイン、積分ゲイン、微分ゲインです。

$$\begin{eqnarray} e_x(t) &=& x_d - x(t) \\ e_y(t) &=& y_d - y(t) \\ e_{\phi}(t) &=& \phi_d - \phi(t) \\ u(t) &=& K_p e(t) + K_i \int_{\tau=0}^t e(\tau) d\tau + K_d \frac{d e(t)}{d t} \end{eqnarray} $$

P 制御では、目標値を通り過ぎてから逆向きの操作量が発生するため目標値に綺麗に止まれません。
PD 制御では、操作量より大きい逆向きの外力が存在する場合に目標値に到達できません。

クォータニオンから回転ベクトルを取得するために以下のパッケージを利用しています。

pip install scipy

sample.py

#!/usr/bin/python
# -*- coding: utf-8 -*-

import odepy
import drawstuffpy as ds
import ctypes
import numpy as np

from scipy.spatial.transform import Rotation

def Main():
    world, space, ground, contactgroup = CreateWorld()

    # 全方向移動ロボット
    robot = Robot(world, space)

    def __NearCallback(data, o1, o2):

        b1 = odepy.dGeomGetBody(o1)
        b2 = odepy.dGeomGetBody(o2)

        # ジョイントで結合されている「車輪と車体」は衝突判定しません。
        if odepy.dAreConnectedExcluding(b1, b2, odepy.dJointTypeContact):
            return

        # 「車輪と何か」の衝突であるか判定します。その場合は摩擦の方向を取得します。
        wheelCollision = False
        frictionDirection = odepy.dVector3()

        for wheel, joint in zip(robot.GetWheels(), robot.GetJoints()):
            o1IsWheel = ctypes.addressof(wheel.contents) == ctypes.addressof(o1.contents)
            o2IsWheel = ctypes.addressof(wheel.contents) == ctypes.addressof(o2.contents)
            if not (o1IsWheel or o2IsWheel):
                continue
            wheelCollision = True
            odepy.dJointGetHingeAxis(joint, frictionDirection)
            break

        N = 10
        contacts = (odepy.dContact * N)()
        n = odepy.dCollide(o1, o2, N, ctypes.byref(contacts[0].geom), ctypes.sizeof(odepy.dContact))

        for i in range(n):
            contact = contacts[i]

            if wheelCollision:
                contact.surface.mode = odepy.dContactFDir1 | odepy.dContactMu2
                contact.fdir1[0] = frictionDirection[0]
                contact.fdir1[1] = frictionDirection[1]
                contact.fdir1[2] = frictionDirection[2]
                contact.surface.mu = 0.0  # オムニホイールのシミュレーション (車軸方向の摩擦係数は 0 にします)
                contact.surface.mu2 = 99  # 車輪の進行方向

            c = odepy.dJointCreateContact(world, contactgroup, ctypes.byref(contact))
            odepy.dJointAttach(c, b1, b2)

    def __StepCallback(pause):

        # ロボットの位置を PID 制御
        robot.Control(tDelta=0.01)

        odepy.dSpaceCollide(space, 0, odepy.dNearCallback(__NearCallback))
        odepy.dWorldStep(world, 0.01)
        odepy.dJointGroupEmpty(contactgroup)

        for geom in robot.GetGeoms():
            if odepy.dGeomGetClass(geom) == odepy.dBoxClass:
                ds.dsSetColor(1.0, 1.0, 0.0)
                lengths = odepy.dVector3()
                odepy.dGeomBoxGetLengths(geom, lengths)
                ds.dsDrawBoxD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), lengths)
            if odepy.dGeomGetClass(geom) == odepy.dCylinderClass:
                ds.dsSetColor(1.0, 1.0, 1.0)
                r = odepy.dReal()
                l = odepy.dReal()
                odepy.dGeomCylinderGetParams(geom, ctypes.byref(r), ctypes.byref(l))
                ds.dsDrawCylinderD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), l.value, r.value)

    def __Command(cmd):
        if chr(cmd) == 'x':
            robot.SetTargetX(robot.GetTargetX() + 1.0)
        if chr(cmd) == 'X':
            robot.SetTargetX(robot.GetTargetX() - 1.0)
        if chr(cmd) == 'y':
            robot.SetTargetY(robot.GetTargetY() + 1.0)
        if chr(cmd) == 'Y':
            robot.SetTargetY(robot.GetTargetY() - 1.0)
        if chr(cmd) == 's':
            robot.SetTargetX(0.0)
            robot.SetTargetY(0.0)

    RunDrawStuff(__StepCallback, __Command)
    DestroyWorld(world, space)

class Robot(object):

    def __init__(self, world, space, lx=0.4, ly=0.4, lz=0.2, r=0.11, w=0.02, mBase=36.4, mWheel=0.2, px=0.0, py=0.0, pz=0.3):
        self.__base = self.__AddBox(world, space, lx=lx, ly=ly, lz=lz, m=mBase, px=px, py=py, pz=pz)

        pxWheel = (lx + w) / 2.0
        pyWheel = (ly + w) / 2.0
        pzWheel = pz - lz / 2.0

        q1 = odepy.dQuaternion()
        q2 = odepy.dQuaternion()
        odepy.dQFromAxisAndAngle(q1, 0, 1, 0, np.pi / 2.0)
        odepy.dQFromAxisAndAngle(q2, 1, 0, 0, np.pi / 2.0)

        axis1 = [1, 0, 0]
        axis2 = [0, -1, 0]

        self.__wheels = [
            self.__AddCylinder(world, space, r=r, l=w, m=mWheel, px=-pxWheel, py=0.0, pz=pzWheel, q=q1),
            self.__AddCylinder(world, space, r=r, l=w, m=mWheel, px=pxWheel, py=0.0, pz=pzWheel, q=q1),
            self.__AddCylinder(world, space, r=r, l=w, m=mWheel, px=0.0, py=-pyWheel, pz=pzWheel, q=q2),
            self.__AddCylinder(world, space, r=r, l=w, m=mWheel, px=0.0, py=pyWheel, pz=pzWheel, q=q2),
        ]
        self.__joints = [
            self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheels[0]), odepy.dGeomGetPosition(self.__wheels[0]), axis1),
            self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheels[1]), odepy.dGeomGetPosition(self.__wheels[1]), axis1),
            self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheels[2]), odepy.dGeomGetPosition(self.__wheels[2]), axis2),
            self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheels[3]), odepy.dGeomGetPosition(self.__wheels[3]), axis2),
        ]

        # 制御対象
        self.__x = px
        self.__y = py
        self.__phi = 0.0

        # 前のステップにおける残差
        self.__eX = 0.0
        self.__eY = 0.0
        self.__ePhi = 0.0

        # 残差の積分値
        self.__sX = 0.0
        self.__sY = 0.0
        self.__sPhi = 0.0

    def Control(self, tDelta):

        # ロボットの現在位置と姿勢を何らかのセンサで正確に取得できた状況を考えます。
        pos = odepy.dGeomGetPosition(self.__base)
        x = pos[0]
        y = pos[1]

        q = odepy.dQuaternion()
        odepy.dGeomGetQuaternion(self.__base, q)
        rot = Rotation.from_quat([q[1], q[2], q[3], q[0]])
        rotvec = rot.as_rotvec()
        phi = rotvec[2]

        # 残差
        eX = self.__x - x
        eY = self.__y - y
        ePhi = self.__phi - phi

        # 残差の積分値
        self.__sX += (self.__eX + eX) * tDelta / 2.0
        self.__sY += (self.__eY + eY) * tDelta / 2.0
        self.__sPhi += (self.__ePhi + ePhi) * tDelta / 2.0

        # ゲイン
        kp = 20.0
        kd = 10.0
        ki = 1.0

        # 操作量
        uX = kp * eX + kd * (eX - self.__eX) / tDelta + ki * self.__sX
        uY = kp * eY + kd * (eY - self.__eY) / tDelta + ki * self.__sY
        uPhi = kp * ePhi + kd * (ePhi - self.__ePhi) / tDelta + ki * self.__sPhi

        # モータの最大トルク
        fMax = 100

        # 操作量である角速度を入力します。

        odepy.dJointSetHingeParam(self.__joints[0], odepy.dParamVel, uY - uPhi)
        odepy.dJointSetHingeParam(self.__joints[0], odepy.dParamFMax, fMax)

        odepy.dJointSetHingeParam(self.__joints[1], odepy.dParamVel, uY + uPhi)
        odepy.dJointSetHingeParam(self.__joints[1], odepy.dParamFMax, fMax)

        odepy.dJointSetHingeParam(self.__joints[2], odepy.dParamVel, uX + uPhi)
        odepy.dJointSetHingeParam(self.__joints[2], odepy.dParamFMax, fMax)

        odepy.dJointSetHingeParam(self.__joints[3], odepy.dParamVel, uX - uPhi)
        odepy.dJointSetHingeParam(self.__joints[3], odepy.dParamFMax, fMax)

        # 次のステップで利用するため残差を保存します。
        self.__eX = eX
        self.__eY = eY
        self.__ePhi = ePhi

        # 残差を出力します。
        print('{}\t{}\t{}'.format(eX, eY, ePhi))

    def GetWheels(self):
        return self.__wheels

    def GetJoints(self):
        return self.__joints

    def GetGeoms(self):
        return [self.__base] + self.__wheels

    def GetTargetX(self):
        return self.__x

    def GetTargetY(self):
        return self.__y

    def SetTargetX(self, x):
        self.__x = x

    def SetTargetY(self, y):
        self.__y = y

    def __AddBox(self, world, space, lx, ly, lz, m, px, py, pz):
        geom = odepy.dCreateBox(space, lx, ly, lz)
        body = odepy.dBodyCreate(world)
        mass = odepy.dMass()
        odepy.dMassSetZero(ctypes.byref(mass))
        odepy.dMassSetBoxTotal(ctypes.byref(mass), m, lx, ly, lz)
        odepy.dGeomSetBody(geom, body)
        odepy.dBodySetMass(body, ctypes.byref(mass))
        odepy.dBodySetPosition(body, px, py, pz)
        q = odepy.dQuaternion()
        odepy.dQFromAxisAndAngle(q, 0, 0, 1, 0.0)
        odepy.dBodySetQuaternion(body, q)
        return geom

    def __AddCylinder(self, world, space, r, l, m, px, py, pz, q, direction=3):
        geom = odepy.dCreateCylinder(space, r, l)
        body = odepy.dBodyCreate(world)
        mass = odepy.dMass()
        odepy.dMassSetZero(ctypes.byref(mass))
        odepy.dMassSetCylinderTotal(ctypes.byref(mass), m, direction, r, l)
        odepy.dGeomSetBody(geom, body)
        odepy.dBodySetMass(body, ctypes.byref(mass))
        odepy.dBodySetPosition(body, px, py, pz)
        odepy.dBodySetQuaternion(body, q)
        return geom

    def __AddJoint(self, world, body1, body2, pos, axis):
        joint = odepy.dJointCreateHinge(world, 0)
        odepy.dJointAttach(joint, body1, body2)
        odepy.dJointSetHingeAxis(joint, *axis)
        odepy.dJointSetHingeAnchor(joint, pos[0], pos[1], pos[2])
        return joint

def RunDrawStuff(stepCallback, command, pathToTextures='/usr/local/share/ode-0.16.1-drawstuff-textures', w=400, h=225, cameraXyz=[3.0, 0.0, 1.0], cameraHpr=[-180.0, 0.0, 0.0]):
    def __StartCallback():
        xyz = (ctypes.c_float * 3)()
        hpr = (ctypes.c_float * 3)()
        for i, v in enumerate(cameraXyz):
            xyz[i] = v
        for i, v in enumerate(cameraHpr):
            hpr[i] = v
        ds.dsSetViewpoint(xyz, hpr)
    fn = ds.dsFunctions()
    fn.version = ds.DS_VERSION
    fn.start = ds.dsStartCallback(__StartCallback)
    fn.step = ds.dsStepCallback(stepCallback)
    fn.path_to_textures = ctypes.create_string_buffer(pathToTextures.encode('utf-8'))
    fn.command = ds.dsCommandCallback(command)
    ds.dsSimulationLoop(0, ctypes.byref(ctypes.POINTER(ctypes.c_char)()), w, h, fn)

def CreateWorld():
    odepy.dInitODE()
    world = odepy.dWorldCreate()
    odepy.dWorldSetGravity(world, 0.0, 0.0, -9.8)
    space = odepy.dHashSpaceCreate(0)
    ground = odepy.dCreatePlane(space, 0, 0, 1, 0)
    contactgroup = odepy.dJointGroupCreate(0)
    return world, space, ground, contactgroup

def DestroyWorld(world, space):
    odepy.dSpaceDestroy(space)
    odepy.dWorldDestroy(world)
    odepy.dCloseODE()

if __name__ == '__main__':
    Main()