python カーシム
http://python.matrix.jp/pages/projects/pyogre/ode_test.html#id5
上のソースコードに私の解説をつけて 多分誤りおおしE:\goolgedrive\myprg_main\python_my_prg\ode_prg\car2.py
# coding: UTF-8
# http://python.matrix.jp/projects/pyogre/ode_test.html
# カーシム
# Pyodeのサンプルコードがすくないので 上記のサイトのもので 学ぶ
import sys
import os
import time
import ode
import visual
from visual import controls
# controlsが visual.conrols でなく controlsのままで使用できる
class Field:
world = ode.World()
space = ode.Space()
#ODEでは衝突検出機能を使用するには衝突検出用のスペース(space）と動力学計算
#用のワールド(world)の２つを生成する必要があります
#http://demura.net/9ode/4409.html
jointgroup = ode.JointGroup()
#ジョイントグループは、ジョイントを世界に保持しておく特別なコンテナ
world.setGravity( (0,-9.81,0) )
# 重力の設定
#world.setERP(0.6) 元からのコメント行
#world.setCFM(1E-5) 元からのコメント行
visual.rate(30)
# vpythonの実行を1/30秒停止
scene = visual.display(
title='field', width=640, height=520, autoscale=0,
forward=visual.norm((0.5,-0.5,-1)),
range=(5,5,5))
# range 座標軸それぞれに沿った中心：center から対象領域への広がりを意味
#autoscale = 0 のとき自動的にスケールを設定しません
# http://www.nasuinfo.or.jp/FreeSpace/kenji/sf/visualj/display.html
floor = ode.GeomPlane(space, (0,1,0), 0)
# spaceに衝突計算用のジオメトリを登録します Planeだから平面か
# 球の半径ですが、ボディのサイズと同じにする必要はありません。
# loor = ode.GeomPlane(space, (0,0,1), 0)は(0,0,1)が法線ベクトルとなる、つまりx、y軸に平行な地面のジオメトリを高さ0の位置に生成しています。
# GeomPlane 地面？ 無限に広がった地面のジオメトリを作成？
# http://rbintelligence.hide-yoshi.net/pyODE3.html
floor.setCategoryBits(1)
floor.setCollideBits(3)
# http://www.matzmtok.com/blog/2008/05/box2d%E3%82%B5%E3%83%B3%E3%83%97%E3%83%AB-maskbits%E3%81%AB%E3%82%88%E3%82%8B%E8%A1%9D%E7%AA%81%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%83%AA%E3%83%B3%E3%82%B0/
#衝突判定の設定をしているのだが ？
#geom.setCategoryBits(0x80000000)
#geom.setCollideBits(0xffffffff)
#といったように３２bitの各ビットでカテゴライズと、衝突検出する対象を
# 設定できます。??????
# http://python.matrix.jp/projects/pyogre/ode_test.html
floor.viz = visual.box(
axis=(0,-1,0), pos=(0,-0.05, 0),
width=20, length=0.1, height=20, color=(0.5,0.5,1.0))
# vpythonのbox(車体)
target_fps = 30
dt = 1.0/target_fps
# target_fpsは1秒間のシュミレーションのコマの数だったと思う
collision_check_num = 2
check_time = dt/collision_check_num
lasttime=time.time()
#????????????????????
def __init__(self):
self.objects = []
def near_callback(self, args, geom1, geom2):
for c in ode.collide(geom1, geom2):
# 2つのジオメトリオブジェクトが衝突してるかどうか検知する
# 戻り値は Contactオブジェクトのリストになるみたい
# contacts = ode.collide(geom1, geom2)でgeom1とgeom2の衝突点数を取得します。
# c.setBounce(0.2)は反発係数、c.setMu(5000)は摩擦です。
# http://rbintelligence.hide-yoshi.net/pyODE3.html
c.setBounce(0.5)
c.setMu(50)
c.setMu2(50)
j = ode.ContactJoint(self.world, self.jointgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
# 衝突点にジョイントを作っています。ODEでは接触をジョイントの拘束として計算しています。
# http://rbintelligence.hide-yoshi.net/pyODE3.html
# 接触ジョイントは衝突検出で使用され、その有効期間はシミュレーションの1ステップだけです
# http://www.crystal-creation.com/robot/technical-information/physics-engine/ode/joint/contact.htm
def tick(self):
for obj in self.objects:
obj.update()
t = self.dt - (time.time() - self.lasttime)
if (t > 0):
time.sleep(t)
for i in range(self.collision_check_num):
self.space.collide((), self.near_callback)
self.world.step(self.check_time)
self.jointgroup.empty()
self.lasttime = time.time()
return True
class Cylinder:
def __init__(self, field, **keys):
# 辞書を引数にしているみたいだが このような使用方法は許されるのか
self.body = ode.Body(field.world)
M = ode.Mass()
# Massは全質量、重心位置、慣性モーメントなどのパラメータが格納されている構造体・・・・・・らしいです
# Massのオブジェクト作成
M.setCylinder(
keys.get('density', 10),
keys.get('direction', 3),
keys.get('radius', 0.5),
keys.get('length', 1.0))
# 衝突を表現できる形状 円柱のジオメトリを作成
# 000setCylinder (密度、方向、半径 長さ）
# http://www10.atwiki.jp/bambooflow/pages/226.html#id_8fb8a607
self.body.setMass(M)
# bodyの質量プロパティの設定
self.geom = ode.GeomCCylinder(
space=field.space,
radius=keys.get('radius', 0.5),
length=keys.get('length', 1.0)-keys.get('radius', 0.5)*2)
# spaceに衝突計算用のジオメトリを登録します こんどは円柱か
self.geom.setBody(self.body)
# M.setCylinder などと Mにセットするのとどう違うのか
# 衝突検出用円柱状ジオメトリ に動力学計算用のBodyをセットしているみたい
self.frame = visual.frame()
#組み合わせオブジェクトを作ることによる、複数のオブジェクトを一体の物とし
#て動かしたり回転させたりできます。frame を基準にオブジェクトを生成し配置
#します。
self.visual = visual.cylinder(
frame=self.frame,
radius=keys.get('radius', 0.5),
length=keys.get('length', 1.0))
self.visual.rotate(angle=visual.pi/2.0, axis=(0,1,0))
# visualオブジェクトの回転 引数は角度と acis=(0,1,0)ならy軸で回転か？
# 車輪か？ 「車輪にCappedCylinderを使いました。」とかいてあるけれど みつからないk
self.visual.z += self.visual.length/2.0
field.objects.append(self)
def update(self):
pos = self.body.getPosition()
mat = self.body.getRotation()
self.frame.pos = pos
self.frame.axis = mat[0], mat[3], mat[6]
self.frame.up = mat[1], mat[4], mat[7]
class Box:
def __init__(self, field, **keys):
self.move = keys.get('move', True)
if self.move:
self.body = ode.Body(field.world)
M = ode.Mass()
M.setBox(
keys.get('density', 20),
keys.get('width', 1),
keys.get('height', 1),
keys.get('length', 1))
self.body.setMass(M)
self.geom = ode.GeomBox(
space=field.space,
lengths=(keys.get('width', 1),
keys.get('height', 1),
keys.get('length', 1)))
if self.move:
self.geom.setBody(self.body)
self.frame = visual.frame()
self.visual = visual.box(
frame=self.frame,
width=keys.get('width', 1),
height=keys.get('height', 1),
length=keys.get('length', 1))
self.visual.rotate(angle=visual.pi/2.0, axis=(0,1,0))
field.objects.append(self)
def update(self):
pos = self.geom.getPosition()
mat = self.geom.getRotation()
self.frame.pos = pos
self.frame.axis = mat[0], mat[3], mat[6]
self.frame.up = mat[1], mat[4], mat[7]
class Vehicle:
def __init__(self, field, pos=(0,2,0)):
self.handle = 0.0
self.acceleration = 0.0
self.four_drive = False
self.body = Box(field, width=2.0, height=0.5, length=3.5)
self.body.body.setPosition((pos[0], pos[1], pos[2]))
self.body.geom.setCategoryBits(1)
self.body.geom.setCollideBits(5)
def make_tire(pos, offset, limit=(0,0)):
res = Cylinder(field, radius=0.3, length=0.3)
res.body.setPosition(pos)
res.body.setRotation((0,0,-1,0,1,0,1,0,0))
res.geom.setCategoryBits(2)
res.geom.setCollideBits(6)
res.joint = ode.Hinge2Joint(field.world)
res.joint.attach(self.body.body, res.body)
res.joint.setAnchor((pos[0]+offset[0],pos[1]+offset[1],pos[2]+offset[2]))
res.joint.setAxis1((0,1,0))
res.joint.setAxis2((1,0,0))
res.joint.setParam(ode.ParamHiStop, limit[0])
res.joint.setParam(ode.ParamLoStop, limit[1])
res.joint.setParam(ode.ParamStopCFM, 1.0E-6)
res.joint.setParam(ode.ParamSuspensionCFM, 0.015)#サス硬さ(0で剛体)
res.joint.setParam(ode.ParamSuspensionERP, 0.4)
res.joint.setParam(ode.ParamFudgeFactor, 1.0)#粘性係数
res.joint.setParam(ode.ParamVel, 0.0)#目標角速度
res.joint.setParam(ode.ParamFMax, ode.Infinity)#目標角速度達成のための最大トルク
res.joint.setFeedback(True)
return res
self.tirefl = make_tire((-1, pos[1]-0.5, -1.2), ( 0.2,0,0),
(40.0/180.0*visual.pi, -40.0/180.0*visual.pi))
self.tirefr = make_tire(( 1, pos[1]-0.5, -1.2), (-0.2,0,0),
(40.0/180.0*visual.pi, -40.0/180.0*visual.pi))
self.tirerl = make_tire((-1, pos[1]-0.5, 1.2), ( 0.2,0,0))
self.tirerr = make_tire(( 1, pos[1]-0.5, 1.2), (-0.2,0,0))
field.objects.append(self)
def update(self):
handle = 40.0/180.0*visual.pi*self.handle
target_vel = max(-10.0,min(10.0, handle - self.tirefl.joint.getAngle1()))*3.0
self.tirefl.joint.setParam(ode.ParamVel, target_vel)
target_vel = max(-10.0,min(10.0, handle - self.tirefr.joint.getAngle1()))*3.0
self.tirefr.joint.setParam(ode.ParamVel, target_vel)
if self.four_drive:
self.tirefl.joint.addTorques(0,self.acceleration/4)
self.tirefr.joint.addTorques(0,self.acceleration/4)
self.tirerl.joint.addTorques(0,self.acceleration/4)
self.tirerr.joint.addTorques(0,self.acceleration/4)
else:
self.tirefl.joint.addTorques(0,self.acceleration/2)
self.tirefr.joint.addTorques(0,self.acceleration/2)
if __name__=='__main__':
field = Field()
vehicle = Vehicle(field)
vehicle.body.body.setAngularVel((0,0,0))
c = controls.controls()
slot = controls.slider(
pos=(-50,-20), min=0.0, max=2.0, length=100, axis=(0,1,0))
slot.value=1.0
handle = controls.slider(
pos=(-20,0), min=0.0, max=2.0, length=100, axis=(1,0,0))
handle.value=1.0
obstacle = Box(field, width=2.0, height=0.5, length=0.5, move=False)
obstacle.geom.setPosition((0.0,0.0,-8.0))
obstacle.geom.setRotation((1,0,0,0,0.707,-0.707,0,0.707,0.707))
obstacle.geom.setCategoryBits(4)
obstacle.geom.setCollideBits(6)
while field.tick():
field.scene.center = vehicle.body.body.getPosition()
c.interact()
vehicle.handle = handle.value-1.0
vehicle.acceleration = 1000.0*(slot.value-1.0)
0 件のコメント:
コメントを投稿