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

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

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

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

ロボットアームの逆運動学をヤコビ行列で数値的に解く (Open Dynamics Engine、Python)

モーダルを閉じる

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

モーダルを閉じる

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

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

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

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

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

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

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

目次

    アカウント プロフィール画像 (サイドバー)

    MySQLの運用やレプリケーション設定など、実用的なノウハウを共有します。

    0
    ステッカーを贈るとは?

    ロボットアームのマニピュレータの位置姿勢が与えられたときに、ジョイントの関節値を求める逆運動学について、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 を引数としてマニピュレータの位置姿勢を返す、順運動学の関数として扱えます。関連ページ

    (PrΦ)=(xyzϕxϕyϕz)=f(θ1,θ2,θ3)\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)

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

    Pr=(xyz)=g(θ1,θ2,θ3)P_r = \left( \begin{array}{c} x \\ y \\ z \end{array} \right) = g(\theta_1, \theta_2, \theta_3)

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

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

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

    特異姿勢

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

    疑似逆行列

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

    計算機で解析解を求める

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

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

    MySQLの運用やレプリケーション設定など、実用的なノウハウを共有します。

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

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

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

    Feedbacks

    Feedbacks コンセプト画像

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

      関連記事