2017/8/26再考
#アーム同士 アームと床の衝突回避に問題が残る
# 'sキーで「世界」の進行を止め' 'gキーで「世界」の進行を進めたり'
# 0, 1, 2, 3, 4, 9 でジョイント番号を選択して動かすジョイントを選択
# 9 全自動でボールを拾いにいく 予定.....
# ジョイントを動かしている状態でジョイント番号を変えると
# マシンが分解する。
# t でマシンを停止してからジョイント番号を変える
# j で動き kで逆方向へ動く
# 指同士の接触を検知するのが難しいのでデータをファイルに書き込んで
# それを2回目の動作からは参照している。
# 詳しいことは忘れてしまった。 geomの接触検知でしても検知が遅いせいか
# マシンが分解してしまう。
# coding: UTF-8
# 2017/02/01
= pyode manipulator
[2016-12-15 04:55]
L:\goolgedrive\myprg_main\python_my_prg\wxpython\manipulator13_yubi.py
上ファイルにて、真上から見た平面上のランダム位置からおちてくる、一個のボール
のマニピュレータに対する角度を検知して、その方向にアームが向くようにす
る。
要するに ボールの落ちた方向にアームが向く。
if __name__=='__main__':
.........
.........
while True:
kakudo = control()
jointList[1].set_kaiten_kakudo2(kakudo)
#jointList[1].set_kaiten_kakudo2(0.011)
#jointList[index_m].set_kaiten_kakudo()
controlで一個のボールの位置をkakudoでかえしてやる。
vpythonでは上方向がｙ軸。それから右手方向へｘ軸。手前側へｚ軸。
で、ボールのposのｘ、ｚ成分をかえすことになる。
それを set_kaiten_kakudo2(self, theta2)に引数として送ると、それを目標
角度として回転する。
controlの中の以下のコードで角度を得ている。
kakudo = math.atan2(p_b[2], p_b[0])
atan2(y,x)は、tan=x/yの逆関空？になっている。 座標のｙとｘをいれたら
角度が求まる。引数の順序がｙから始まっているので注意。
アームは初期状態が角度0。
j入力でマイナス方向で、うえからみて反時計まわり
k入力でプラス方向で、 うえからみて 時計まわり
これはvpythonのhingejointの設定によるものか....
これにtanの通常の座標をどうあてはめたらいいか、わからなかった。
ｘ軸はそのままとしたら、-ｚの方向が通常のｙ+方向になるから....
結局カット＆トライで、上のcontrolの引数の順序、-を付け加えなければいけ
ないのか、-90度しなければいけないのか、いろいろして上のものに落ち着
く。
#jointList[index_m].set_kaiten_kakudo()
として、手動でアームを回転させるコマンドをころしている。
そうしないと「自動」でボールの位置にアームが回転しない。
以下 全コード
# coding: UTF-8
# 2012/9/8 09:13
#マニピュレータ アームの作成から 2012/09/09 ﾆ完成
#アーム2個 第一アーム水平回転 第二アーム固定
# j k で回転（正転、逆転） 1 2 ...でジョイントを動かすジョイントを選択
#初期起動時 キーボードになかなか反応しない。ずーと押していると反応する
#台座 支柱のジョイントを回転運動＋ヒンジの関節運動にしたいが
#hing2 UniversalJointはgetAngle2のコマンドがないため、台座2をつけて
#それを HingeJoint2つで実現してみる
# 2016/12/10
# 「指」の取り付け。これでものをはさむ
# これをどうやってつけるか？
# ランダムな位置にボールを一個落とす。その位置にアームを自動で動かす事
# ができた。
import math
import random
import ode
from visual import *
#ここでは、台座、支柱、アームすべてboxで表現している
#paraListからmetaSetJointでジョイントを作成（各種ジョイント作成関数に飛んで
#いる）
#paraListからmetaBoxでboxオブジェクトを作成している（Box関数を利用）
index = 1 # 動かすジョイントの選択
paraList = [] #ここで使われるbox型オブジェクトの全てのデータのリスト
#box_kosuu = 5
box_kosuu = 4
#box_kosuu = 3 #box型オブジェクトの個数
objList = [] #box型オブジェクトのリスト
#ball_kosuu=500
ballList = []
jParaList = [] #全てのジョイントのパラメータのリスト
jointList = [] #jParaListから作成されたジョイントのリスト
joint_kosuu = 4
#joint_kosuu = 3
#上のように宣言することにより appendコマンドだけで関数内
#クラス内でグローバル宣言せずにリストに追加できる
#パラメータ このマニピュレータで使われるbox型オブジェクトのデータを、
#paraListで設定
##c言語のodeの座標からpyodeの座標へ（ｚとｙを入れ替えるだけ）
paraList = [
{"pos":(0, 0.50, 0 ), "m_density":2, "v_x":2.0, "v_y":1.00, "v_z":2.0, "v_color":color.red}, #台座1
{"pos":(0, 1.50, 0 ), "m_density":2, "v_x":2.0, "v_y":1.00, "v_z":2.0, "v_color":color.orange}, #台座2
{"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}#第2アーム
]
#{"pos":(6.0, 6.75, 0 ), "m_density":0.01, "v_x":10.0, "v_y":0.50, "v_z":4.0, "v_color":color.blue},#第2アーム
#{"pos":(2.75, 13.50, 0 ), "m_density":2, "v_x":2.0, "v_y":2.00, "v_z":2.0, "v_color":color.yellow}
#{"pos":(7.75, 6.75, 0 ), "m_density":2, "v_x":5.0, "v_y":0.50, "v_z":0.5, "v_color":color.cyan} #実際には無効なコード
class Field:
scene = display(autoscale=0 ,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(6) #0111
ode_floor.setCollideBits(6) #0111
ode_floor.viz=box(
pos=(0,-0.5,0),
width=20,length=20,height=1.0,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.5)
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(len(ballList)):
ballList[i].update()
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
#落ちるボール一個分のクラス MetaBallクラスでこれを利用する
class Ball:
def __init__(
self,field,
m_density=1,
m_radius=0.2,
g_radius=0.2,
v_radius=0.2,
b_pos=vector(0,0,0),
b_sp =vector(0,0,0),
v_color=color.cyan
):
self.pos = (0, 0, 0)
self.body=ode.Body(field.world)
M=ode.Mass()
M.setSphere(m_density, m_radius)
#(密度,半径)
self.body.setMass(M)
self.body.setPosition(b_pos)
#self.body.addForce(b_sp)
self.geom=ode.GeomSphere(
space=field.space,
radius=g_radius
)
#このradiusが実際のバウンドを決める
#多分 sphereの座標が0で radiusが0.3だから
#posが0.3ぐらいのバウンド点になる
self.geom.setCategoryBits(1) #0001
self.geom.setCollideBits(3) #0011
#ボール同士の衝突を避ける
#上は Boxのジオメトリを作成
self.geom.setBody(self.body)
# 剛体にジオメトリをセット
#self.frame = frame()
#self.vball=sphere(
# frame=self.frame, radius=v_radius,
# color=v_color
# )
self.vball=sphere(
radius=v_radius,
color=v_color
)
def update(self):
#pos_tup = self.body.getPosition()
#boxのジオメトリの位置を得る
#self.frame.pos = pos_tup
#mat = self.body.getRotation()
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]
self.vball.pos = self.pos
self.vball.axis = mat[0], mat[3], mat[6]
self.vball.up = mat[1], mat[4], mat[7]
class MetaBall:
#落ちるボールを4×4個作成、落下させてgeomの状態をみる
def __init__(self, field):
pos=vector(0,0,0)
#for x in range(-6, 6):
# for z in range(-6, 6):
# pos.x = x
# pos.z = z
# pos.y = 10
# ballList.append(Ball(field, b_pos=pos))
pos.x = random.uniform(-6, 6) #-6以上6未満の実数を発生
pos.z = random.uniform(-6, 6)
#pos.x = 0
#pos.z = 4
pos.y = 10
ballList.append(Ball(field, b_pos=pos))
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.pos = (0, 0, 0)
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_x, self.g_y, self.g_z)#w,h,l
)
self.geom.setCategoryBits(2) #0010
self.geom.setCollideBits(5) #0101
#アーム同士の衝突を避ける
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'
#boxのジオメトリの位置を得る
self.pos = self.geom.getPosition()
mat=self.geom.getRotation()
#pos = self.body.getPosition()
#mat=self.body.getRotation()
self.f.pos = self.pos
self.f.axis=(mat[0],mat[3],mat[6])
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.tmp2 = 0
self.theta2 = 0
self.z2 = 0
self.hani_kakudo = hani_kakudo
self.flg1 = 1
self.flg2 = 1
self.k = 20.0
#現在の角度
#print "SetHingeJoint", self.theta1, self.tmp1
def kaitenn_para(self):
self.joint.setParam(ode.ParamLoStop, -self.hani_kakudo+0.1)
self.joint.setParam(ode.ParamHiStop, self.hani_kakudo-0.1)
#目標とする角速度 単位はたぶんラジアン 角速度マイナスなら逆向きに回転
self.joint.setParam(ode.ParamFMax, 500)
# 500ぐらいが最適値、10ぐらいだとふらつく、目標値を行き過ぎ、
# また戻り...を繰り返す。この数値に落ち着くまで、 PID制御など
# いろいろ検討した。
#目標の角度と現在のジョイントの角度との差を求めている
##dParamVel 目標とする速度または角速度
##dParamFMax dParamVelを達成するために発生する最大の力またはトルク。
##直接 関節を目標の角度にするAPIがないので 上のように角度の差を求め
##て ...とする
def set_kaiten_kakudo(self):
#-180と+180以上になるとキー入力をセーブしている。(範囲角以上になると)
print " set_kaiten_kakudo ",
#if self.theta1>-self.hani_kakudo:
# self.flg1 = 1
#if self.theta1< self.hani_kakudo:
# self.flg2 = 1
self.tmp1 = self.joint.getAngle()
self.z1 = self.theta1 - self.tmp1
v = self.k * self.z1
self.joint.setParam(ode.ParamVel, v)
if field.scene.kb.keys:
s = field.scene.kb.getkey()
#if s=='j' and self.flg1==1:
if s=='j':
print "jjjjj --------"
print "self.hani_kakudo", self.hani_kakudo
print "現在角度", self.tmp1
print "目標角度",self.theta1
print "目標角度と現在角度の差", self.z1
self.theta1 -= math.pi/20
if self.theta1<-self.hani_kakudo:
self.theta1 = -self.hani_kakudo + 0.1
#self.flg1 = 0
#elif s=='k' and self.flg2==1:
elif s=='k':
print "kkkk ++++++++"
print "self.hani_kakudo", self.hani_kakudo
print "getAngle", self.tmp1
print "self.theta1",self.theta1
print "目標角度と現在角度の差", self.z1
self.theta1 += math.pi/20
if self.theta1> self.hani_kakudo:
self.theta1 = self.hani_kakudo - 0.1
#self.flg2 = 0
#print self.theta1
##動かすジョイントの選択
##いささか反応が遅い
#if s=='1' or s=='2' or s=='3':
# index = int(s)
def set_kaiten_kakudo2(self, theta2):
print "222222222222222"
#-180と+180以上になるとキー入力をセーブしている。(範囲角以上になると)
self.theta2 = theta2
self.tmp2 = self.joint.getAngle()
print self.tmp2
self.z2 = self.theta2 - self.tmp2
v = 5 * self.z2
self.joint.setParam(ode.ParamVel, v)
#self.joint.setParam(ode.ParamVel, 10000)
#次ここから 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"
def control():
p_b = ballList[0].pos
print 'p_b %03.1f %03.1f %03.1f ' % p_b,
p_a = objList[3].pos
print 'p_a %03.1f %03.1f %03.1f' % p_a,
#kakudo = math.atan2(p_b[0]-p_a[0], p_b[1]-p_a[1])
#kakudo = math.atan2(p_b[1]-p_a[1], p_b[0]-p_a[0])
#kakudo = math.atan2(p_b[0]-1, p_b[1]-0)
kakudo = math.atan2(p_b[2], p_b[0])
#kakudo = math.atan2(p_b[0], -p_b[2])
print 'kakudo ', kakudo
return kakudo
if __name__=='__main__':
field = Field()
ball = MetaBall(field)
box = metaBox(field)
#objListにオブジェクトが入った後jParaListに要素を入れる
#そうでないとえらーが出る
jParaList = [
{"name":"fixed", "obj1":objList[0], "obj2":"env"}, #台座固定
{"name":"hinge", "obj1":objList[0], "obj2":objList[1], "anchor":(0, 1, 0), "j_axis":(0, 1, 0), "hani_kakudo":3.1415},#台座1と台座２
{"name":"hinge", "obj1":objList[1], "obj2":objList[2], "anchor":(0, 2, 0), "j_axis":(0, 0, 1), "hani_kakudo":3.1415},#台座2と第一アームの関節
{"name":"hinge", "obj1":objList[2], "obj2":objList[3], "anchor":(0.35, 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)}
#{"name":"hinge", "obj1":objList[2], "obj2":objList[3], "anchor":(0.25, 6.75, 0), "j_axis":(0, 0, 1), "hani_kakudo":1.0} #第一と第二アームの関節
#objList[3].geom.setCategoryBits(2)
#objList[3].geom.setCollideBits(2)
metaSetJoint(field)
for list in jointList:
if list.name=="hinge":
#print "list.name",list.name
list.kaitenn_para()
index_m = 1
#lb_a = label(pos=(0, 0, 12), text='joint')
#lb_b = label(pos=(0, 0, 8), text='1')
jn = '1'
tx = 'joint ' + jn
lb_b = label(pos=(0, 0, 8), text=tx)
# aキーで「世界」の進行を止めたり進めたり
print 'sキーで「世界」の進行を止め'
print 'gキーで「世界」の進行を進めたり'
Flg = False
while True:
kakudo = control()
jointList[1].set_kaiten_kakudo2(kakudo)
#jointList[1].set_kaiten_kakudo2(0.011)
#jointList[index_m].set_kaiten_kakudo()
# キーボードを押し続けないと反応しない。苦し紛れに以下のfor文
# for にしないとgetkey をあまりうけつけない
# for in range以外のコードもいろいろ試してやっとこれに落ち着く
for n in range(2000):
if field.scene.kb.keys:
jn = field.scene.kb.getkey()
#動かすジョイントの選択
#いささか反応が遅い
if jn=="1" or jn=="2" or jn=="3":
index_m = int(jn)
tx = 'joint ' + jn
lb_b.text = tx
print("\007")
#音をならして操作するジョイント
#が変わった事を知らせる
#for n in range(20):
# elif jn=="s":
# Flg = True
for n in range(20000):
if field.scene.kb.keys:
s1 = field.scene.kb.getkey()
if s1 == 's':
Flg =True
while Flg:
if field.scene.kb.keys:
s2 = field.scene.kb.getkey()
if s2 == 'g':
Flg = False
elif s2 != 's':
Flg = True
else:
Flg = True
#for n in range(200):
# if field.scene.kb.keys:
# s1 = field.scene.kb.getkey()
# if s1 == 's':
# Flg =True
# print 'soto_s'
#while Flg:
# print '----------------------------'
# if field.scene.kb.keys:
# s2 = field.scene.kb.getkey()
# if s2 == 'g':
# Flg = False
# print 'g'
# elif s2 != 's':
# Flg = True
# else:
# Flg =True
field.tick()
0 件のコメント:
コメントを投稿