ロボットアームの先端の位置姿勢 (Open Dynamics Engine、Python)
[履歴] [最終更新] (2020/05/19 00:36:34)
最近の投稿
注目の記事

概要

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

Uploaded Image

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

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

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

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

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

関連ページ
    概要 ロボットアームのマニピュレータの位置姿勢が与えられたときに、ジョイントの関節値を求める逆運動学について、3自由度の場合を対象として解析解と数値解を用いる場合を記載します。 解析解 3 自由度の場合は解析的に逆運動学を解くことができます。以下は 4 つ存在する解析解のうちの一つを利用しています。詳細はこちらの書籍をご覧ください。