# 2012/9/8 09:13
# アームの作成から 2012/09/09 ﾆ完成
#アーム2個 第一アーム水平回転 第二アーム固定
# manipulator10.py
import random
import ode
from visual import *
paraList = []
#box_kosuu = 4
box_kosuu = 4
objList = []
jParaList = []
jointList = []
joint_kosuu = 3
#上のように宣言することにより appendコマンドだけで関数内
#クラス内でグローバル宣言せずにリストに追加できる
#動かすjointの選択
#パラメータ
##c言語のodeの座標からpyodeの座標へ（ｚとｙを入れ替えるだけ）
paraList = [
{"pos":(0, 1.00, 0 ), "m_density":2, "v_x":2.0, "v_y":2.00, "v_z":2.0, "v_color":color.red}, #台座
{"pos":(0, 4.50, 0 ), "m_density":0.01, "v_x":0.5, "v_y":5.00, "v_z":0.5, "v_color":color.white},#第一アーム
{"pos":(2.75, 6.75, 0 ), "m_density":0.01, "v_x":5.0, "v_y":0.50, "v_z":0.5, "v_color":color.blue},
{"pos":(7.75, 6.75, 0 ), "m_density":0.01, "v_x":5.0, "v_y":0.50, "v_z":0.5, "v_color":color.cyan}
]
#objListにデータがはいってからjParaListのリストを作成する
#でないとえらーが出る
#jParaList0は元データ それから 利用するでーたjParaListを作成
#def putJParaList():
# for i in jParaList0[joint_kosuu-1]:
# jParaList.append(jParaList0[i])
#次ここから jParaListのデータから判断してじどうで ジョイントを作成
def metaSetJoint(field):
print "metaSetJoint"
print jParaList
for list in jParaList:
print "list", list
if list["name"]=="fixed":
name = list["name"]
obj1 = list["obj1"]
obj2 = list["obj2"]
jointList.append(SetFixedJoint(field, name, obj1, obj2))
print "metaSetJoint....fixed"
if list["name"]=="hinge":
name = list["name"]
obj1 = list["obj1"]
obj2 = list["obj2"]
anchor0 = list["anchor"]
j_axis0 = list["j_axis"]
hani_kakudo = list["hani_kakudo"]
jointList.append(SetHingeJoint(field, name, obj1, obj2, anchor0, j_axis0, hani_kakudo))
print "metaSetJoint....hinge"
class Field:
scene = display(autoscale=1 ,forward=norm((-2.0,-1.0,-1.0)))
#物理世界を作成
world=ode.World()
world.setGravity((0, -9.81, 0))
#自由落下させたいなら worldのコメントアウトのほうを生かし
# 最後の方の ball.body.addForce((0,-1000,0)) を殺す
#world.setGravity((0, 0, 0))
#地平面を作成？
space=ode.Space()
#衝突関係
jointgroup=ode.JointGroup()
ode_floor=ode.GeomPlane(space,(0,1,0),0)
#（）なかの引数 上の例ではｙ軸方向に重力が発生
#多分 spaceの最後の引数が ｙ軸の座標を示す
ode_floor.setCategoryBits(5)
ode_floor.setCollideBits(5)
ode_floor.viz=box(
pos=(0,-0.03,0),
width=20,length=20,height=0.06,color=(0.5,0.5,1.0))
vball_x=sphere( pos=(5, 0, 0), radius=0.2, color=color.red) #x軸
vball_y=sphere( pos=(0, 5, 0), radius=0.2, color=color.white)#y軸
vball_z=sphere( pos=(0, 0, 5), radius=0.2, color=color.yellow) #z軸
target_fps=30
#target_fps=30だから1秒間に30回の処理をする
dt=1.0/target_fps
def near_callback(self,args,geom1,geom2):
#isBall = ((ground == o1) || (ground == o2));
#if (isGround)//地面だけと衝突判定
for c in ode.collide(geom1,geom2):
c.setBounce(0.0)
c.setMu(50)#摩擦
c.setMu2(50)
#random.seed(1)
j=ode.ContactJoint(self.world,self.jointgroup,c)
j.attach(geom1.getBody(),geom2.getBody())
def tick(self):
print "tick"
for i in range(box_kosuu):
objList[i].update()
time.sleep(0.1)
#self.space.collide((),self.near_callback)
self.world.step(self.dt)
self.jointgroup.empty()
return True
class Box:
def __init__(
self, field, flg=1,
m_density=0.01,
m_x=1, m_y=5, m_z=1,
g_x=1, g_y=5, g_z=1,
v_x=1, v_y=5, v_z=1,
b_pos=vector(0,2.5,0),
b_sp =vector(0,0,0),
v_color=color.cyan
):
if flg==1:
#デフォルトで...(vpthonのパラメータだけですます)
self.m_x = v_x
self.m_y = v_y
self.m_z = v_z
self.g_x = v_x
self.g_y = v_y
self.g_z = v_z
if flg==0:
self.m_x = m_x
self.m_y = m_y
self.m_z = m_z
self.g_x = g_x
self.g_y = g_y
self.g_z = g_z
self.body=ode.Body(field.world)
M=ode.Mass()
M.setBox(m_density, self.m_z, self.m_y, self.m_x)#密度,z,y,x w,h,l
self.body.setMass(M)
self.body.setPosition(b_pos)
self.geom = ode.GeomBox(
space=field.space, lengths=(self.g_z, self.g_y, self.g_x)#w,h,l
)
self.geom.setCategoryBits(1)
self.geom.setCollideBits(4)
#ボール同士の衝突を避ける
self.geom.setBody(self.body)
self.f = frame()
self.f.pos = b_pos
self.vbox = box(frame=self.f,
length=v_x, height=v_y, width=v_z, color=v_color)
mat=self.geom.getRotation()
self.f.axis=(mat[0],mat[3],mat[6])
#self.f.axis=(1, 1, 1)
#self.vbox.up=mat[1],mat[4],mat[7]
#setRotationとmatの表示が微妙にずれるのでうえのコードで
#vpytonのオブジェクトをmatで表示してからまたvpythonの
#回転こまんどで微調整をしよう
#self.geom.setRotation([0.866,0,0.5, 0,1,0, -0.5,0,0.866])
# opacity ：透明度
#BoxのMassの大きさ,geomの大きさ,
# vboxの大きさを同じにする
#print m_x,m_y,m_z
def update(self):
print 'box_update'
pos = self.geom.getPosition()
#boxのジオメトリの位置を得る
#self.vbox.pos = pos
self.f.pos = pos
mat=self.geom.getRotation()
self.f.axis=(mat[0],mat[3],mat[6])
#self.vbox.up=mat[1],mat[4],mat[7]
#self.geom.setRotation(1,0,0, 0,3,0, 0,0,1)
#print mat[0],mat[3],mat[6]
#print mat[1],mat[4],mat[7]
def metaBox(field):
for i in range(box_kosuu):
pos0 = paraList[i]["pos"]
density0 = paraList[i]["m_density"]
v_x0 = paraList[i]["v_x"]
v_y0 = paraList[i]["v_y"]
v_z0 = paraList[i]["v_z"]
v_color0 = paraList[i]["v_color"]
objList.append(Box(field, flg=1, b_pos=pos0,
m_density=density0, v_x=v_x0, v_y=v_y0, v_z=v_z0, v_color=v_color0))
class SetFixedJoint:
def __init__(self, field, name, obj1, obj2):
#FixedJointで固定
self.name = "fixed"
self.joint = ode.FixedJoint(field.world)
if obj2=="env":
self.joint.attach(obj1.body, ode.environment)
else:
self.joint.attach(obj1.body, obj2.body)
self.joint.setFixed()
class SetHingeJoint:
def __init__(self, field, name,obj1, obj2, anchor, j_axis, hani_kakudo):
#theta 関節の目標とする角度 関節をどの方向へ曲げるか決める
self.name = name
self.joint = ode.HingeJoint(field.world)
self.joint.attach(obj1.body, obj2.body)
self.joint.setAnchor(anchor)
self.joint.setAxis(j_axis)
self.tmp1 = 0
self.theta1 = 0
self.z1 = 0
self.PI = 3.141592
self.hani_kakudo = hani_kakudo
self.flg1 = 1
self.flg2 = 1
#現在の角度
print "SetHingeJoint", self.theta1, self.tmp1
def kaitenn_joint(self):
vel = self.joint.getParam(ode.ParamVel)
k = 10.0
self.tmp1 = self.joint.getAngle()
self.z1 = self.theta1 - self.tmp1
print "kaitenn_joint",
print round(self.theta1,4), round(self.tmp1,4), round(self.z1,4), round(vel,4),
print self.flg1, self.flg2
v = k*self.z1
self.joint.setParam(ode.ParamVel, v)
#目標とする角速度 単位はたぶんラジアン 角速度マイナスなら逆向きに回転
self.joint.setParam(ode.ParamFMax, 10)
#目標の角度と現在のジョイントの角度との差を求めている
##dParamVel 目標とする速度または角速度
##dParamFMax dParamVelを達成するために発生する最大の力またはトルク。
##直接 関節を目標の角度にするAPIがないので 上のように角度の差を求め
##て ...とする
def set_kaiten_kakudo(self, field):
#-180と+180以上になるとキー入力をセーブしている。(範囲角以上になると)
print " set_kaiten_kakudo "
if self.theta1>-self.hani_kakudo:
self.flg1 = 1
if self.theta1< self.hani_kakudo:
self.flg2 = 1
if field.scene.kb.keys:
s = field.scene.kb.getkey()
if s=='j' and self.flg1==1:
print "jjjjjjjjjjjjjjjjjjjjjj"
self.theta1 -= self.PI/20
if self.theta1<-self.hani_kakudo:
self.theta1 = -self.hani_kakudo
self.flg1 = 0
elif s=='k' and self.flg2==1:
print "kkkkkkkkkkkkkkkkkkkkkk"
self.theta1 += self.PI/20
if self.theta1> self.hani_kakudo:
self.theta1 = self.hani_kakudo
self.flg2 = 0
print self.theta1
if __name__=='__main__':
index = 1
field = Field()
metaBox(field)
#objListにオブジェクトが入った後jParaListに要素を入れる
jParaList = [
{"name":"fixed", "obj1":objList[0], "obj2":"env"},
{"name":"hinge", "obj1":objList[0], "obj2":objList[1], "anchor":(0, 2, 0), "j_axis":(0, 1, 0), "hani_kakudo":3.1415},#台座と第一アームの関節
{"name":"hinge", "obj1":objList[1], "obj2":objList[2], "anchor":(0.25, 6.75, 0), "j_axis":(0, 0, 1), "hani_kakudo":1.0}, #第一と第二アームの関節
{"name":"hinge", "obj1":objList[2], "obj2":objList[3], "anchor":(5.25, 6.75, 0), "j_axis":(0, 0, 1), "hani_kakudo":1.0}
]
metaSetJoint(field)
#PI = 3.141592
while True:
print "index", index
#jointList[1].kaitenn_joint()
#jointList[2].set_kaiten_kakudo(field)
#すべてのHingeJointで下のコマンドを実行しないと重力でダラーンとなる
jointList[index].set_kaiten_kakudo(field)
for list in jointList:
if list.name=="hinge":
#print "list.name",list.name
list.kaitenn_joint()
if field.scene.kb.keys:
print "index", index
jn = field.scene.kb.getkey()
print "jnjnjnjnjnjnjn"
print "jn", jn
#動かすジョイントの選択
#いささか反応が遅い
if jn=="1" or jn=="2" or jn=="3":
index = int(jn)
lb = label(pos=(0,0,10), text=jn)
#今どんなコマンドがはいっているか表示（キー入力の反応が遅いのをカバー）
field.tick()
0 件のコメント:
コメントを投稿