ロボットアームは複数のリンクがジョイントで結合されており、先端にマニピュレータが存在します。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()

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

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