公開日

2020/05/17

最終更新

2024/11/30

記事区分

一般公開

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

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

\begin{aligned} 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{aligned}

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()

とろとろねこパン

Dockerの基礎について学びなおし中

記事の執筆者にステッカーを贈る

有益な情報に対するお礼として、またはコメント欄における質問への返答に対するお礼として、記事の読者は、執筆者に有料のステッカーを贈ることができます。

さらに詳しく →

Feedbacks

ログインするとコメントを投稿できます。

Python コードスニペット (条件分岐)
if-elif-else sample.py #!/usr/bin/python # -*- coding: utf-8 -*- # コメント内であっても、ASCII外の文字が含まれる場合はエンコーディング情報が必須 x = 1 # 一行スタイル if x==0: print 'a' # 参考: and,or,notが使用可能 (&&,||はエラー) elif x==1: p...
coderina11/25/2015に更新
0
- Python
Python コードスニペット (リスト、タプル、ディクショナリ)
リスト range 「0から10まで」といった範囲をリスト形式で生成します。 sample.py print range(10) # for(int i=0; i<10; ++i) ← C言語などのfor文と比較 print range(5,10) # for(int i=5; i<10; ++i) print range(5,10,2) # for(int i=5; i<10;...
coderina10/28/2020に更新
0
- Python
ZeroMQ (zmq) の Python サンプルコード
ZeroMQ を Python から利用する場合のサンプルコードを記載します。 Fixing the World To fix the world, we needed to do two things. One, to solve the general problem of "how to connect any code to any code, anywhere". Two, to wra...
coderina10/7/2021に更新
0
- Python
Matplotlib/SciPy/pandas/NumPy サンプルコード
Python で数学的なことを試すときに利用される Matplotlib/SciPy/pandas/NumPy についてサンプルコードを記載します。 Matplotlib SciPy pandas [NumPy](https://www.numpy
パンダ1/26/2025に更新
0
pytest の基本的な使い方
pytest の基本的な使い方を記載します。適宜参照するための公式ドキュメントページ Full pytest documentation API Reference インストール適当なパッケージ
nanodev8/8/2022に更新
0
- Python

PID 制御による全方向移動ロボットの位置制御 (ODE、Python)

目次

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

記事の執筆者にステッカーを贈る

Feedbacks

関連記事