作成日
2020/05/17最終更新
2024/11/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()
逆運動学による関節値の計算
マニピュレータの位置姿勢が与えられた際のジョイント値を求めます。自由度が小さい場合は解析的に解けますが、そうでない場合は数値的に解きます。
関連記事
- Python コードスニペット (条件分岐)if-elif-else sample.py #!/usr/bin/python # -*- coding: utf-8 -*- # コメント内であっても、ASCII外の文字が含まれる場合はエンコーディング情報が必須 x = 1 # 一行スタイル if x==0: print 'a' # 参考: and,or,notが使用可能 (&&,||はエラー) elif x==1: p...
- Python コードスニペット (リスト、タプル、ディクショナリ)リスト range 「0から10まで」といった範囲をリスト形式で生成します。 sample.py print range(10) # for(int i=0; i<10; ++i) ← C言語などのfor文と比較 print range(5,10) # for(int i=5; i<10; ++i) print range(5,10,2) # for(int i=5; i<10;...
- ZeroMQ (zmq) の Python サンプルコードZeroMQ を Python から利用する場合のサンプルコードを記載します。 Fixing the World To fix the world, we needed to do two things. One, to solve the general problem of "how to connect any code to any code, anywhere". Two, to wra...
- Matplotlib/SciPy/pandas/NumPy サンプルコードPython で数学的なことを試すときに利用される Matplotlib/SciPy/pandas/NumPy についてサンプルコードを記載します。 Matplotlib SciPy pandas [NumPy](https://www.numpy
- pytest の基本的な使い方pytest の基本的な使い方を記載します。 適宜参照するための公式ドキュメントページ Full pytest documentation API Reference インストール 適当なパッケージ