モーダルを閉じる工作HardwareHub ロゴ画像

工作HardwareHubは、ロボット工作や電子工作に関する情報やモノが行き交うコミュニティサイトです。さらに詳しく

利用規約プライバシーポリシー に同意したうえでログインしてください。

工作HardwareHub ロゴ画像 (Laptop端末利用時)
工作HardwareHub ロゴ画像 (Mobile端末利用時)

ロボットアームの先端の位置姿勢 (Open Dynamics Engine、Python)

モーダルを閉じる

ステッカーを選択してください

モーダルを閉じる

お支払い内容をご確認ください

購入商品
」ステッカーの表示権
メッセージ
料金
(税込)
決済方法
GooglePayマーク
決済プラットフォーム
確認事項

利用規約をご確認のうえお支払いください

※カード情報はGoogleアカウント内に保存されます。本サイトやStripeには保存されません

※記事の執筆者は購入者のユーザー名を知ることができます

※購入後のキャンセルはできません

作成日作成日
2020/05/17
最終更新最終更新
2024/12/02
記事区分記事区分
一般公開

ロボットアームは複数のリンクがジョイントで結合されており、先端にマニピュレータが存在します。Open Dynamics Engine (ODE) を用いたシミュレーションを行い、ジョイントの関節値とマニピュレータの位置姿勢の関係を把握します。

順運動学によるマニピュレータの位置姿勢の計算

回転行列と回転ベクトルの変換のために以下のパッケージを利用しています。

pip install scipy

3 自由度ロボットアームについて、関節値を指定したときのマニピュレータの位置姿勢の計算は以下のようになります。変換行列による座標変換を利用して計算したマニピュレータの位置姿勢と、ODE の機能を利用して取得したマニピュレータの位置姿勢が一致することを確認します。ジョイント値の制御には PD 制御を用いています。

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

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

from sys import version_info
from scipy.spatial.transform import Rotation

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

    # 3 自由度ロボットアーム
    robot = Robot(world, space)

    def __StepCallback(pause):

        # 関節値の PD 制御
        tDelta = 0.01
        robot.Control(tDelta)

        # 理想的なセンサで取得した値
        posManip = robot.GetManipPos()
        rotManip = robot.GetManipRot()

        # 順運動学で導出した値
        posManip2 = robot.GetManipPos2()
        rotManip2 = robot.GetManipRot2()

        eps = 1e-1
        for i in range(3):
            assert(abs(posManip2[i] - posManip[i]) < eps)
            assert(abs(rotManip2[i] - rotManip[i]) < eps)

        odepy.dWorldStep(world, tDelta)
        odepy.dJointGroupEmpty(contactgroup)

        for geom in robot.GetGeoms():
            r = odepy.dReal()
            l = odepy.dReal()
            if odepy.dGeomGetClass(geom) == odepy.dCylinderClass:
                odepy.dGeomCylinderGetParams(geom, ctypes.byref(r), ctypes.byref(l))
                ds.dsDrawCylinderD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), l.value, r.value)
            if odepy.dGeomGetClass(geom) == odepy.dCapsuleClass:
                odepy.dGeomCapsuleGetParams(geom, ctypes.byref(r), ctypes.byref(l))
                ds.dsDrawCapsuleD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), l.value, r.value)

    def __Command(cmd):
        jointValues = robot.GetJointValues()
        if chr(cmd) == 'a':
            jointValues[0] += 0.1
        if chr(cmd) == 'A':
            jointValues[0] -= 0.1
        if chr(cmd) == 'b':
            jointValues[1] += 0.1
        if chr(cmd) == 'B':
            jointValues[1] -= 0.1
        if chr(cmd) == 'c':
            jointValues[2] += 0.1
        if chr(cmd) == 'C':
            jointValues[2] -= 0.1
        robot.SetJointValues(jointValues)

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

class Robot(object):

    def __init__(self, world, space):

        q = odepy.dQuaternion()
        odepy.dQFromAxisAndAngle(q, 0, 0, 1, 0.0)

        lBase = 0.10
        l1 = 0.90
        l2 = 1.0
        l3 = 1.0
        lManip = 0.10

        # ジオメトリの作成
        self.__base = self.__AddCylinder(world, space, r=0.20, l=lBase, m=9.0, px=0.0, py=0.0, pz=(lBase / 2.0), q=q)
        self.__link1 = self.__AddCapsule(world, space, r=0.04, l=l1, m=2.0, px=0.0, py=0.0, pz=(lBase + l1 / 2.0), q=q)
        self.__link2 = self.__AddCapsule(world, space, r=0.04, l=l2, m=2.0, px=0.0, py=0.0, pz=(lBase + l1 + l2 / 2.0), q=q)
        self.__link3 = self.__AddCapsule(world, space, r=0.04, l=l3, m=2.0, px=0.0, py=0.0, pz=(lBase + l1 + l2 + l3 / 2.0), q=q)
        self.__manip = self.__AddCylinder(world, space, r=0.02, l=lManip, m=0.5, px=0.0, py=0.0, pz=(lBase + l1 + l2 + l3 + lManip / 2.0), q=q)

        # 地面に固定します。
        self.__jointBase = odepy.dJointCreateFixed(world, 0)
        odepy.dJointAttach(self.__jointBase, odepy.dGeomGetBody(self.__base), 0)
        odepy.dJointSetFixed(self.__jointBase)

        self.__joint1 = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__link1), [0.0, 0.0, lBase], [0, 0, 1])
        self.__joint2 = self.__AddJoint(world, odepy.dGeomGetBody(self.__link1), odepy.dGeomGetBody(self.__link2), [0.0, 0.0, lBase + l1], [0, 1, 0])
        self.__joint3 = self.__AddJoint(world, odepy.dGeomGetBody(self.__link2), odepy.dGeomGetBody(self.__link3), [0.0, 0.0, lBase + l1 + l2], [0, 1, 0])

        # マニピュレータ
        self.__jointManip = odepy.dJointCreateFixed(world, 0)
        odepy.dJointAttach(self.__jointManip, odepy.dGeomGetBody(self.__link3), odepy.dGeomGetBody(self.__manip))
        odepy.dJointSetFixed(self.__jointManip)

        # 目標値となるジョイント値、およびその残差
        self.__jointValues = [-1.0, -1.0, -1.0]
        self.__eJointValues = [0.0, 0.0, 0.0]

    def GetManipPos(self):
        # マニピュレータの位置を ODE の機能を利用して取得して返します。
        posOde = odepy.dGeomGetPosition(self.__manip)
        pos = np.zeros(3, dtype=np.float)
        for i in range(len(pos)):
            pos[i] = posOde[i]
        return pos

    def GetManipRot(self):
        # マニピュレータの姿勢を ODE の機能を利用して取得して返します。
        rotOde = odepy.dGeomGetRotation(self.__manip)
        rot = np.zeros(12, dtype=np.float)
        for i in range(len(rot)):
            rot[i] = rotOde[i]
        rot = rot.reshape(3, 4)[:3, :3]
        if version_info.major < 3:
            rotVec = Rotation.from_dcm(rot).as_rotvec()
        else:
            rotVec = Rotation.from_matrix(rot).as_rotvec()
        return rotVec

    def GetManipPos2(self):
        # 変換行列によるマニピュレータの位置計算
        tManipToWorld = self.__GetTransformManipToWorld()
        pos = np.array([[0, 0, 0, 1]], dtype=np.float).T
        pos = tManipToWorld.dot(pos)
        pos = pos.reshape(1, 4)[0][:3]
        return pos

    def GetManipRot2(self):
        # 変換行列によるマニピュレータの姿勢計算
        tManipToWorld = self.__GetTransformManipToWorld()
        if version_info.major < 3:
            rot = Rotation.from_dcm(tManipToWorld[:3, :3])
        else:
            rot = Rotation.from_matrix(tManipToWorld[:3, :3])
        rotVec = rot.as_rotvec()
        return rotVec

    def __GetTransformManipToWorld(self):

        # ジョイント値
        angle1 = odepy.dJointGetHingeAngle(self.__joint1)
        angle2 = odepy.dJointGetHingeAngle(self.__joint2)
        angle3 = odepy.dJointGetHingeAngle(self.__joint3)

        # 回転ベクトルの計算
        rot1 = Rotation.from_rotvec(angle1 * np.array([0, 0, 1]))
        rot2 = Rotation.from_rotvec(angle2 * np.array([0, 1, 0]))
        rot3 = Rotation.from_rotvec(angle3 * np.array([0, 1, 0]))

        # 回転行列に変換
        if version_info.major < 3:
            rot1 = rot1.as_dcm()
            rot2 = rot2.as_dcm()
            rot3 = rot3.as_dcm()
        else:
            rot1 = rot1.as_matrix()
            rot2 = rot2.as_matrix()
            rot3 = rot3.as_matrix()

        # リンクの長さを取得
        lBase = odepy.dReal()
        l1 = odepy.dReal()
        l2 = odepy.dReal()
        l3 = odepy.dReal()
        lManip = odepy.dReal()
        r = odepy.dReal()
        odepy.dGeomCylinderGetParams(self.__base, ctypes.byref(r), ctypes.byref(lBase))
        odepy.dGeomCylinderGetParams(self.__link1, ctypes.byref(r), ctypes.byref(l1))
        odepy.dGeomCylinderGetParams(self.__link2, ctypes.byref(r), ctypes.byref(l2))
        odepy.dGeomCylinderGetParams(self.__link3, ctypes.byref(r), ctypes.byref(l3))
        odepy.dGeomCylinderGetParams(self.__manip, ctypes.byref(r), ctypes.byref(lManip))

        # 変換行列の平行移動部分を計算
        trans1 = np.array([0, 0, -(lBase.value + l1.value)], dtype=np.float)
        trans2 = np.array([0, 0, -l2.value], dtype=np.float)
        trans3 = np.array([0, 0, -(l3.value + lManip.value / 2.0)], dtype=np.float)

        # 変換行列として結合
        tWorldToLink1 = np.vstack((np.hstack((rot1, trans1.reshape(3, 1))), np.array([0, 0, 0, 1], dtype=np.float)))
        tLink1ToLink2 = np.vstack((np.hstack((rot2, trans2.reshape(3, 1))), np.array([0, 0, 0, 1], dtype=np.float)))
        tLink2ToManip = np.vstack((np.hstack((rot3, trans3.reshape(3, 1))), np.array([0, 0, 0, 1], dtype=np.float)))

        # 逆変換
        tLink1ToWorld = np.linalg.inv(tWorldToLink1)
        tLink2ToLink1 = np.linalg.inv(tLink1ToLink2)
        tManipToLink2 = np.linalg.inv(tLink2ToManip)

        # マニピュレータ座標系とワールド座標系の変換行列
        tManipToWorld = tLink1ToWorld.dot(tLink2ToLink1).dot(tManipToLink2)
        return tManipToWorld

    def Control(self, tDelta):
        kp = 10.0
        kd = 0.9
        fMax = 100.0
        for i, joint in enumerate([self.__joint1, self.__joint2, self.__joint3]):
            jointValue = odepy.dJointGetHingeAngle(joint)  # pi と -pi を越える際に不連続です。
            eJointValue = self.__jointValues[i] - jointValue
            # jointValue が不連続であることに対応して以下の処理を行います。
            while eJointValue > np.pi:
                eJointValue -= 2.0 * np.pi
            while eJointValue < -np.pi:
                eJointValue += 2.0 * np.pi
            u = kp * eJointValue + kd * (eJointValue - self.__eJointValues[i]) / tDelta
            odepy.dJointSetHingeParam(joint, odepy.dParamVel, u)
            odepy.dJointSetHingeParam(joint, odepy.dParamFMax, fMax)
            self.__eJointValues[i] = eJointValue

    def GetJointValues(self):
        return self.__jointValues

    def SetJointValues(self, jointValues):
        self.__jointValues = jointValues

    def GetGeoms(self):
        return self.__base, self.__link1, self.__link2, self.__link3, self.__manip

    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 __AddCapsule(self, world, space, r, l, m, px, py, pz, q, direction=3):
        geom = odepy.dCreateCapsule(space, r, l)
        body = odepy.dBodyCreate(world)
        mass = odepy.dMass()
        odepy.dMassSetZero(ctypes.byref(mass))
        odepy.dMassSetCapsuleTotal(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.dJointSetHingeAnchor(joint, *pos)
        odepy.dJointSetHingeAxis(joint, *axis)
        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()

逆運動学による関節値の計算

マニピュレータの位置姿勢が与えられた際のジョイント値を求めます。自由度が小さい場合は解析的に解けますが、そうでない場合は数値的に解きます。

0
詳細設定を開く/閉じる
アカウント プロフィール画像 (本文下)

Research Engineer

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

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

さらに詳しく →
ステッカーを贈る コンセプト画像

Feedbacks

Feedbacks コンセプト画像

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

    関連記事