概要

ロボットアームのマニピュレータの位置姿勢が与えられたときに、ジョイントの関節値を求める逆運動学について、3自由度の場合を対象として解析解と数値解を用いる場合を記載します。

解析解

3 自由度の場合は解析的に逆運動学を解くことができます。以下は 4 つ存在する解析解のうちの一つを利用しています。詳細はこちらの書籍をご覧ください。『簡単!実践!ロボットシミュレーション - Open Dynamics Engineによるロボットプログラミング』

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

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

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

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

    def __StepCallback(pause):

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

        # 目標値と現在値の確認
        posTarget = robot.GetTargetPos()
        pos = robot.GetManipPos()
        err = 0.0
        for target, current in zip(posTarget, pos):
            err += abs(target - current)
        print(err)

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

        if chr(cmd) == 'o':
            # 円軌道に沿って移動します。
            robot.MoveCircleTrajectory()
            return

        x, y, z = robot.GetTargetPos()
        if chr(cmd) == 'x':
            x += 0.1
        if chr(cmd) == 'X':
            x -= 0.1
        if chr(cmd) == 'y':
            y += 0.1
        if chr(cmd) == 'Y':
            y -= 0.1
        if chr(cmd) == 'z':
            z += 0.1
        if chr(cmd) == 'Z':
            z -= 0.1

        lBase, l1, l2, l3, lManip = robot.GetLinkLengthList()

        if z < lManip / 2.0:
            return

        # 解が存在しない入力を無視します。
        pos0 = np.array([0, 0, l1 + lBase], dtype=np.float)
        posTarget = np.array([x, y, z], dtype=np.float)
        if np.linalg.norm(posTarget - pos0) > l2 + l3:
            return
        robot.SetTargetPos(posTarget)

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

class Robot(object):

    def __init__(self, world, space, lBase=0.10, l1=0.90, l2=1.0, l3=1.0, lManip=0.10):

        self.__lBase = lBase
        self.__l1 = l1
        self.__l2 = l2
        self.__l3 = l3
        self.__lManip = lManip

        # ジオメトリの作成
        q = odepy.dQuaternion()
        odepy.dQFromAxisAndAngle(q, 0, 0, 1, 0.0)
        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.__targetPos = [1, 1, 1]

        # IK を解いた結果の目標関節値と現在関節値の残差
        self.__eJointValues = [0.0, 0.0, 0.0]

        # 円軌道を描くための補助変数
        self.__phi = 0.0

    def GetTargetPos(self):
        return self.__targetPos

    def SetTargetPos(self, targetPos):
        self.__targetPos = targetPos

    def GetLinkLengthList(self):
        return self.__lBase, self.__l1, self.__l2, self.__l3, self.__lManip

    def MoveCircleTrajectory(self, x0=1.0, z0=1.2, r=0.5):
        self.__targetPos[0] = x0
        self.__targetPos[1] = r * np.sin(self.__phi)
        self.__targetPos[2] = r * np.cos(self.__phi) + z0
        self.__phi += 0.05

    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 Control(self, tDelta):
        jointValues = self.__SolveIk()
        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 = 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 __SolveIk(self):

        # 逆運動学を解析的に解いてジョイント値を取得します。
        px = self.__targetPos[0]
        py = self.__targetPos[1]
        pz = self.__targetPos[2]

        # 4つある解析解のひとつを利用します。
        tmpL = np.sqrt(px * px + py * py)
        p1p = np.sqrt(px * px + py * py + (pz - (self.__lBase + self.__l1)) * (pz - (self.__lBase + self.__l1)))
        ca = (self.__l2 * self.__l2 + p1p * p1p - (self.__l3 + self.__lManip / 2.0) * (self.__l3 + self.__lManip / 2.0)) / (2 * self.__l2 * p1p)
        phi = np.arctan2(pz - (self.__lBase + self.__l1), tmpL)
        alpha = np.arctan2(np.sqrt(1 - ca * ca), ca)
        cb = (self.__l2 * self.__l2 + (self.__l3 + self.__lManip / 2.0) * (self.__l3 + self.__lManip / 2.0) - p1p * p1p) / (2 * self.__l2 * (self.__l3 + self.__lManip / 2.0))
        beta = np.arctan2(np.sqrt(1 - cb * cb), cb)

        jointValues = np.zeros(3, dtype=np.float)
        jointValues[0] = np.arctan2(py, -px)
        jointValues[1] = np.pi / 2.0 - phi - alpha
        jointValues[2] = np.pi - beta
        return 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()

円軌道を描く様子

数値解

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

pip install scipy

sample.py

#!/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
from scipy.optimize import newton_krylov

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

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

    def __StepCallback(pause):

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

        # 目標値と現在値の確認
        posTarget = robot.GetTargetPos()
        pos = robot.GetManipPos()
        err = 0.0
        for target, current in zip(posTarget, pos):
            err += abs(target - current)
        print(err)

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

        if chr(cmd) == 'o':
            # 円軌道に沿って移動します。
            robot.MoveCircleTrajectory()
            return

        x, y, z = robot.GetTargetPos()
        if chr(cmd) == 'x':
            x += 0.1
        if chr(cmd) == 'X':
            x -= 0.1
        if chr(cmd) == 'y':
            y += 0.1
        if chr(cmd) == 'Y':
            y -= 0.1
        if chr(cmd) == 'z':
            z += 0.1
        if chr(cmd) == 'Z':
            z -= 0.1

        lBase, l1, l2, l3, lManip = robot.GetLinkLengthList()

        if z < lManip / 2.0:
            return

        # 解が存在しない入力を無視します。
        pos0 = np.array([0, 0, l1 + lBase], dtype=np.float)
        posTarget = np.array([x, y, z], dtype=np.float)
        if np.linalg.norm(posTarget - pos0) > l2 + l3:
            return
        robot.SetTargetPos(posTarget)

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

class Robot(object):

    def __init__(self, world, space, lBase=0.10, l1=0.90, l2=1.0, l3=1.0, lManip=0.10):

        self.__lBase = lBase
        self.__l1 = l1
        self.__l2 = l2
        self.__l3 = l3
        self.__lManip = lManip

        # ジオメトリの作成
        q = odepy.dQuaternion()
        odepy.dQFromAxisAndAngle(q, 0, 0, 1, 0.0)
        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.__targetPos = [1, 1, 1]

        # IK を解いた結果の目標関節値と現在関節値の残差
        self.__eJointValues = [0.0, 0.0, 0.0]

        # 円軌道を描くための補助変数
        self.__phi = 0.0

    def GetTargetPos(self):
        return self.__targetPos

    def SetTargetPos(self, targetPos):
        self.__targetPos = targetPos

    def GetLinkLengthList(self):
        return self.__lBase, self.__l1, self.__l2, self.__l3, self.__lManip

    def MoveCircleTrajectory(self, x0=1.0, z0=1.2, r=0.5):
        self.__targetPos[0] = x0
        self.__targetPos[1] = r * np.sin(self.__phi)
        self.__targetPos[2] = r * np.cos(self.__phi) + z0
        self.__phi += 0.05

    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 Control(self, tDelta):
        jointValues = self.__SolveIk()
        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 = 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 __GetTransformManipToWorld(self, theta1, theta2, theta3):

        # 回転ベクトルの計算
        rot1 = Rotation.from_rotvec(theta1 * np.array([0, 0, 1]))
        rot2 = Rotation.from_rotvec(theta2 * np.array([0, 1, 0]))
        rot3 = Rotation.from_rotvec(theta3 * 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, l1, l2, l3, lManip = self.GetLinkLengthList()
        trans1 = np.array([0, 0, -(lBase + l1)], dtype=np.float)
        trans2 = np.array([0, 0, -l2], dtype=np.float)
        trans3 = np.array([0, 0, -(l3 + lManip / 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 __SolveIk(self):

        def g(arg):
            tManipToWorld = self.__GetTransformManipToWorld(*arg)
            posInManipCoordinate = np.array([0, 0, 0, 1], dtype=np.float)
            posInWorldCoordinate = tManipToWorld.dot(posInManipCoordinate.T)
            return self.__targetPos - posInWorldCoordinate[:3]

        # 現在の関節値をニュートン法の初期値として利用します。
        guess = [odepy.dJointGetHingeAngle(self.__joint1), odepy.dJointGetHingeAngle(self.__joint2), odepy.dJointGetHingeAngle(self.__joint3)]
        sol = newton_krylov(g, guess, method='lgmres')
        return sol

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

解析解を利用した場合との違いは __GetTransformManipToWorld と __SolveIk のみです。マニピュレータの先端が円軌道を描くように、ジョイント値を数値的に解いています。4つある解析解のうちの一つを見つけた様子です。

ジョイント値を引数としてマニピュレータの位置姿勢を返す関数

こちらのページに記載した def __GetTransformManipToWorld(self) が返す tManipToWorld は、関節値 $\theta$ を引数としてマニピュレータの位置姿勢を返す、順運動学の関数として扱えます。関連ページ

$$\left( \begin{array}{c} P_r \\ \Phi \end{array} \right) = \left( \begin{array}{c} x \\ y \\ z \\ \phi_x \\ \phi_y \\ \phi_z \end{array} \right) = f(\theta_1, \theta_2, \theta_3) $$

特にマニピュレータの位置だけに注目して以下の関数として扱うこともできます。

$$P_r = \left( \begin{array}{c} x \\ y \\ z \end{array} \right) = g(\theta_1, \theta_2, \theta_3) $$

上記プログラム例では、関数 g の値が目標となるマニピュレータの位置と一致するように、ニュートン法によってジョイント値を解いています。

ニュートン法によるジョイント値の計算

反復法の一つであるニュートン法を用いて、マニピュレータの位置 $x, y, z$ を与えたときに、それを満足する関節値 $\theta_1, \theta_2, \theta_3$ を求めます。ニュートン法にはヘッセ行列を用いるものとヤコビ行列を用いるガウス・ニュートン法があります。上記プログラム例では Scipy に実装されている scipy.optimize.newton_krylov を用いてヤコビ行列の逆行列を計算しています。

特異姿勢

ヤコビ行列を用いるとジョイント値の微小変化から、マニピュレータの位置姿勢の微小変化を計算できます。ヤコビ行列の逆行列を用いると、マニピュレータの位置姿勢の微小変化からジョイント値の微小変化を計算できます。ロボットにはマニピュレータの位置姿勢が微小変化した場合にジョイント値を大きく変化させないと位置姿勢を実現できない場合 (特異点、四元数に関連するジンバルロックと区別します) があります。このような姿勢を特異姿勢とよび、角速度は無限大となり、ヤコビ行列の逆行列は存在しません。

疑似逆行列

逆行列が存在しない場合であっても逆行列と似たような性質をもつ行列を考えることができ、疑似逆行列とよばれます。逆行列が存在する場合は逆行列と一致します。逆行列が存在しない場合は、ヤコビ行列の場合はマニピュレータの先端の位置姿勢が微小変化したときに、ジョイントの角速度が最小となるような行列となります。

計算機で解析解を求める

OpenRAVE に実装されている IKFast を用いると、計算機を用いて解析解を計算することができます。解析解の計算には時間がかかりますが、解析解であるため、数値解と比較して IK を解くための時間はかかりません。