2017/8/26 再考
#アーム同士 アームと床の衝突回避に問題が残る
# 'sキーで「世界」の進行を止め' 'gキーで「世界」の進行を進めたり'
# 0, 1, 2, 3, 4, 9 でジョイント番号を選択して動かすジョイントを選択
# 9 全自動でボールを拾いにいく 予定.....
# ジョイントを動かしている状態でジョイント番号を変えると
# マシンが分解する。
# t でマシンを停止してからジョイント番号を変える
# j で動き kで逆方向へ動く
# 指同士の接触を検知するのが難しいのでデータをファイルに書き込んで
# それを2回目の動作からは参照している。
# 詳しいことは忘れてしまった。 geomの接触検知でしても検知が遅いせいか
# マシンが分解してしまう。
# coding: UTF-8
# 2017/02/01
床にあるボールを拾う場合、指の動きにつれてアームを少し上に上げる動作を
するのがむずかしい。
L:\goolgedrive\myprg_main\python_my_prg\wxpython\manipulator102.py
# coding: UTF-8
# 2017/02/01
import math
import random
import ode
from visual import *
import sys
import numpy as np
import csv
#ここでは、台座、支柱、アームすべてboxで表現している
#paraListからmetaSetJointでジョイントを作成（各種ジョイント作成関数に飛んで
#いる）
#paraListからmetaBoxでboxオブジェクトを作成している（Box関数を利用）
dt_joint23 = [0.0, 0.0]
n = [1]
flg_cj_fu = [False]
flg_world = False
pos4_tmp = [0]
data_angle = [0]
paraList = [] #ここで使われるbox型オブジェクトの全てのデータのリスト
box_kosuu = 17
#box_kosuu = 3 #box型オブジェクトの個数
objList = [] #box型オブジェクトのリスト
#ball_kosuu=500
ballList = []
jParaList = [] #全てのジョイントのパラメータのリスト
jointList = [] #jParaListから作成されたジョイントのリスト
#上のように宣言することにより 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":(5.50, 6.75, 0 ), "m_density":0.01, "v_x":0.5, "v_y":0.50, "v_z":0.5, "v_color":color.white},#手
{"pos":(5.80, 6.95, 0 ), "m_density":0.01, "v_x":0.1, "v_y":0.10, "v_z":0.1, "v_color":color.blue},#指1の台
{"pos":(6.10, 6.95, 0 ), "m_density":0.01, "v_x":0.5, "v_y":0.10, "v_z":0.1, "v_color":color.white},#指1
{"pos":(6.60, 6.95, 0 ), "m_density":0.01, "v_x":0.5, "v_y":0.10, "v_z":0.1, "v_color":color.white},#指11
{"pos":(5.80, 6.55, 0.2 ), "m_density":0.01, "v_x":0.1, "v_y":0.10, "v_z":0.1, "v_color":color.blue},#指2の台
{"pos":(6.10, 6.55, 0.2 ), "m_density":0.01, "v_x":0.5, "v_y":0.10, "v_z":0.1, "v_color":color.white, "kakudo_syoki":-30},#指2
{"pos":(6.60, 6.55, 0.2 ), "m_density":0.01, "v_x":0.5, "v_y":0.10, "v_z":0.1, "v_color":color.white, "kakudo_syoki":-30},#指22
{"pos":(5.80, 6.55, -0.2 ), "m_density":0.01, "v_x":0.1, "v_y":0.10, "v_z":0.1, "v_color":color.blue, "kakudo_syoki":30},#指3の台
{"pos":(6.10, 6.55, -0.2 ), "m_density":0.01, "v_x":0.5, "v_y":0.10, "v_z":0.1, "v_color":color.white, "kakudo_syoki":30},#指3
{"pos":(6.60, 6.55, -0.2 ), "m_density":0.01, "v_x":0.5, "v_y":0.10, "v_z":0.1, "v_color":color.white, "kakudo_syoki":30},#指33
{"pos":(6.90, 6.55, -0.2 ), "m_density":0.01, "v_x":0.1, "v_y":0.10, "v_z":0.1, "v_color":color.red, "kakudo_syoki":30},#指33 センサー
{"pos":(6.90, 6.55, 0.2 ), "m_density":0.01, "v_x":0.1, "v_y":0.10, "v_z":0.1, "v_color":color.red, "kakudo_syoki":30},#指22センサー
{"pos":(6.90, 6.95, 0 ), "m_density":0.01, "v_x":0.1, "v_y":0.10, "v_z":0.1, "v_color":color.red}#指11センサー
]
class Field:
def __init__(self):
self.scene = display(autoscale=0 ,forward=norm((-2.0,-1.0,-1.0)))
#物理世界を作成
self.world=ode.World()
self.world.setGravity((0, -4.81, 0))
#自由落下させたいなら worldのコメントアウトのほうを生かし
# 最後の方の ball.body.addForce((0,-1000,0)) を殺す
#world.setGravity((0, 0, 0))
#地平面を作成？
self.space=ode.Space()
#衝突関係
self.jointgroup=ode.JointGroup()
self.ode_floor=ode.GeomPlane(self.space,(0,1,0),0)
#（）なかの引数 上の例ではｙ軸方向に重力が発生
#多分 spaceの最後の引数が ｙ軸の座標を示す
self.ode_floor.setCategoryBits(1) #0111
self.ode_floor.setCollideBits(1) #0111
self.ode_floor.viz=box(
pos=(0,-0.5,0), width=20,length=20,height=1.0,color=(0.5,0.5,1.0))
self.vball_x=sphere( pos=(5, 0, 0), radius=0.2, color=color.red) #x軸
self.vball_y=sphere( pos=(0, 5, 0), radius=0.2, color=color.white)#y軸
self.vball_z=sphere( pos=(0, 0, 5), radius=0.2, color=color.yellow) #z軸
self.target_fps=20
#target_fps=30だから1秒間に30回の処理をする
self.dt=1.0/self.target_fps
def near_callback(self,args,geom1,geom2):
# 以下の判定をいれないとマシンがこわれてしまう
# 床と指の接触判定
if (geom1==objList[10].geom and geom2==self.ode_floor) or (geom2==objList[10].geom and geom1==self.ode_floor):
flg_cj_fu[0] = True
print "flg_cj_fu"
if (geom1==objList[13].geom and geom2==self.ode_floor) or (geom2==objList[13].geom and geom1==self.ode_floor):
flg_cj_fu[0] = True
print "flg_cj_fu"
#指同士の接触の判定
if (geom1==objList[10].geom and geom2==objList[13].geom) or (geom2==objList[10].geom and geom1==objList[13].geom):
flg_cj_fu[0] = True
print "flg_cj_fu 指"
if (geom1==objList[7].geom and geom2==objList[13].geom) or (geom2==objList[7].geom and geom1==objList[13].geom):
flg_cj_fu[0] = True
print "flg_cj_fu 指"
if (geom1==objList[7].geom and geom2==objList[13].geom) or (geom2==objList[7].geom and geom1==objList[13].geom):
flg_cj_fu[0] = True
print "flg_cj_fu 指"
if (geom1==objList[10].geom and geom2==objList[9].geom) or (geom2==objList[9].geom and geom1==objList[10].geom):
flg_cj_fu[0] = True
print "flg_cj_fu 指 指指指指指指指指指指"
#アーム1と床
if (geom1==objList[2].geom and geom2==self.ode_floor) or (geom2==objList[2].geom and geom1==self.ode_floor):
flg_cj_fu[0] = True
print "flg_cj_fu アーム1"
#アーム2と床
if (geom1==objList[3].geom and geom2==self.ode_floor) or (geom2==objList[3].geom and geom1==self.ode_floor):
flg_cj_fu[0] = True
print "flg_cj_fu アーム2"
#手と床
if (geom1==objList[4].geom and geom2==self.ode_floor) or (geom2==objList[4].geom and geom1==self.ode_floor):
flg_cj_fu[0] = True
print "flg_cj_fu アーム3"
#指と床
if (geom1==objList[15].geom and geom2==self.ode_floor) or (geom2==objList[15].geom and geom1==self.ode_floor):
flg_cj_fu[0] = True
print "flg_cj_fu 指"
for c in ode.collide(geom1,geom2):
#print geom1, geom2
#print "collide0000000000"
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"
#以下のようにupdateを遅らせても、実際は描画が遅れるだけみたい
#で、ボールが遅れて出現しない。
#time_start[1] = time.time()
#print 'time_start[0] time_start[1]',time_start[0] ,time_start[1]
#if time_start[1] - time_start[0] > 5:
# for i in range(len(ballList)):
# ballList[i].update()
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(1) #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 = -5
pos.z = -2
pos.y = 8
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,
kakudo_syoki=30
):
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(1) #0101
#アーム同士の衝突を避ける
self.geom.setBody(self.body)
#3本指でものをつかむため、指を回転させる必要があったが殺す
#L:\goolgedrive\myprg_main\python_my_prg\ode_prg\setRotation5.py
#より
# どれか単独の軸の回転なら、以下のコマンドでできる
#th = 2*pi/360*kakudo_syoki
#self.geom.setRotation([1,0,0, 0,cos(th),-sin(th), 0,sin(th),cos(th)]) #x軸で回転
#self.geom.setRotation([cos(th),0,sin(th), 0,1,0, -sin(th),0,cos(th)]) #y軸で回転 白
#self.geom.setRotation([cos(th),-sin(th),0, sin(th),cos(th),0, 0,0,1]) #z軸で回転 黄
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.up=mat[1],mat[4],mat[7]
#今まで upのコマンドが入っていなかった。これがないとオブジェ
#クトのsetRotationが有効とならない
#L:\goolgedrive\myprg_main\python_my_prg\ode_prg\rotate.py
#vpythonにrotateコマンドがあってvpythonのオブジェクトを回転さ
#せることができるが、まだpyodeとの関連がわからないので使わない。
#回はgeomからvpythonを回転させる
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])
self.f.up=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"]
if "kakudo_syoki" in paraList[i]:
kakudo_syoki0 = paraList[i]["kakudo_syoki"]
else:
kakudo_syoki0 = 0
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, kakudo_syoki=kakudo_syoki0))
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):
#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.v = 0
self.tmp1 = 0
self.tmp11 = 0
self.theta1 = 0
self.theta1_old = 0
self.z1 = 0
self.tmp2 = 0
self.theta2 = 0
self.z2 = 0
self.flg_kaitenn_j = 0
self.flg_kaitenn_k = 0
self.tmp0 = 0
self.kakudo = 0
self.kakudo_j = 0
self.kakudo_k = 0
self.thm = 0
self.thm_j = 0
self.thm_k = 0
self.flg1 = 1
self.flg2 = 1
self.k = 20.0
self.k1 = 1
self.k2 = 1
#現在の角度
#print "SetHingeJoint", self.theta1, self.tmp1
def kaitenn_para(self):
self.joint.setParam(ode.ParamFMax, 500)
# 500ぐらいが最適値、10ぐらいだとふらつく、目標値を行き過ぎ、
# また戻り...を繰り返す。この数値に落ち着くまで、 PID制御など
# いろいろ検討した。
#目標の角度と現在のジョイントの角度との差を求めている
##dParamVel 目標とする速度または角速度
##dParamFMax dParamVelを達成するために発生する最大の力またはトルク。
##直接 関節を目標の角度にするAPIがないので 上のように角度の差を求め
##て ...とする
#指を曲げるのにも使用
#アーム23を曲げる ジョイント1の回転にも使用
def set_kaiten_kakudo23(self, jjkk):
#tを押すとメインループで入力待ちとなりストップする
# jの時 -回転 ｋの時 +回転をする
print "set_kaiten_kakudo23"
self.jjkk = jjkk
# thm_k thm_j はｊｋ方向の制限角度 kは床方向のジョイント2の制限角度
## ジョイントが2か3の一個だけなら以下の制限角度でいいが2つのジョイント
## の角度が影響して、床にぶつからない角度を設定しようとすると...
#self.thm_k = math.pi/4
#self.thm_k = thm_k
#self.thm_j = -math.pi/4
dt1 = 30
dt2 = 10
if self.jjkk=='j':
if (-self.thm_j + self.theta1) > math.pi/dt1:
self.kakudo_j = -math.pi/dt1
else:
self.kakudo_j = -(-self.thm_j + self.theta1) / dt2
self.theta1 += self.kakudo_j
if self.jjkk=='k':
# 目標角度が大きい場合は目標角度増加分をmath.pi/dt1に抑える
if (self.thm_k - self.theta1) > math.pi/dt1:
self.kakudo_k = math.pi/dt1
else:
#制限角度に近くなったらその差のdt2分の1だけ目標角度を増加させる
#利点は 制限角度を超えない設定に簡単にできる
self.kakudo_k = (self.thm_k - self.theta1) / dt2
self.theta1 += self.kakudo_k
#self.theta1 = self.kakudo
self.tmp1 = self.joint.getAngle()
print "現在角度", self.tmp1
print "目標角度",self.theta1
print "目標角度と現在角度の差", self.z1
#self.k = 20.0
self.tmp1 = self.joint.getAngle()
#self.z1 = self.theta1 - self.tmp1 * (pos4[1] - 0.4)
self.z1 = self.theta1 - self.tmp1
self.v = self.k * self.z1
#if self.v>0.4:
# selfv = 0.4
#if self.v<-0.4:
# selfv = -0.4
self.joint.setParam(ode.ParamVel, self.v)
print "getAngle", self.tmp1,
print "self.theta1",self.theta1,
print"------------",
#print "self.pos4_tmp", pos4_tmp[0],
#print "pos_min", pos_min
#初期で指22 指33を120度内側へ回転させるのに使用
#手を水平に保つのに使用
def set_kaiten_kakudo1(self, kakudo):
#目標角度
self.thm = kakudo
## self.kakudo 目標角度増加分
dt1 = 5
dt2 = 3
if self.thm < 0:
if (-self.thm + self.theta1) > math.pi/dt1:
self.kakudo = -math.pi/dt1
else:
self.kakudo = -(-self.thm + self.theta1) / dt2
self.theta1 += self.kakudo
if self.thm > 0:
# 目標角度が大きい場合は目標角度増加分をmath.pi/dt1に抑える
if (self.thm - self.theta1) > math.pi/dt1:
self.kakudo = math.pi/dt1
else:
#制限角度に近くなったらその差のdt2分の1だけ目標角度を増加させる
#利点は 制限角度を超えない設定に簡単にできる
self.kakudo = (self.thm - self.theta1) / dt2
self.theta1 += self.kakudo
self.tmp1 = self.joint.getAngle()
self.z1 = self.theta1 - self.tmp1
print " "
print "set_kaiten_kakudo1"
print 'z1 %03.2f ' %self.z1,
print 'theta1 %03.2f ' %self.theta1,
print 'tmp1 %03.2f ' %self.tmp1, self.tmp1/math.pi*180
self.v = self.k * self.z1
self.joint.setParam(ode.ParamVel, self.v)
#オートの時
#ジョイント1でマシンを水平方向へ回転させるのに使う
def set_kaiten_kakudo2(self, kakudo):
self.theta1 = kakudo
self.tmp1 = self.joint.getAngle()
self.z1 = self.theta1 - self.tmp1
#if self.z1>math.pi*2-0.1 or self.z1<-math.pi*2+0.1:
#自動で回転させた場合、+-360度以上になると逆回転して追随しよう
#とするので、以下でそれ以上でも連続して追いかけられる
if self.z1<-6:
self.z1 = math.pi*2 + self.z1
if self.z1>6:
self.z1 = -math.pi*2 + self.z1
print " "
print 'z1 %03.2f ' %self.z1,
print 'theta1 %03.2f ' %self.theta1,
print 'tmp1 %03.2f ' %self.tmp1
#self.v = self.k * self.z1
self.v = 5 * self.z1
self.joint.setParam(ode.ParamVel, self.v)
def set_kaiten_kakudo3(self, jjkk):
#ここを コンロトールにする
print " set_kaiten_kakudo3:"
#指を曲げる時に使う
self.jjkk = jjkk
pos14 = objList[14].pos
pos15 = objList[15].pos
pos16 = objList[16].pos
a = np.array(pos14)
b = np.array(pos15)
l = np.linalg.norm(b-a)
#l でその3次元の点の間の距離をはかる
print " l = np.linalg.norm(b-a)", l,
#print pos14
#print pos15
#指22 と 指33で距離をはかり近づきすぎないようにする
#if self.jjkk=='j' and self.flg_kaitenn_j==0:
if self.jjkk=='j':
print "jjjj ----------"
self.theta1 -= math.pi/100
self.tmp1 = self.joint.getAngle()
self.flg_kaitenn_k = 0
if l > 1.5:
print " if l > 1.5:"
self.theta1 = self.tmp1
self.flg_kaitenn_j = 1
#if self.jjkk=='k' and self.flg_kaitenn_k==0:
#kで指が閉じる
if self.jjkk=='k':
print "kkkk ++++++++",
self.theta1 += math.pi/100
self.tmp1 = self.joint.getAngle()
self.flg_kaitenn_j = 0
if l < 0.30:
print " if l < 0.20:"
#self.theta1 = self.theta1_old
#回転を確実にとめるつもりなら、以下のようにすべし
#これもいまいち 今度は最後でとまるものの、だんだんと開いて
#いく
self.theta1 = self.tmp1
write_yubi7xx3_angle(self.tmp1)
self.flg_kaitenn_k = 1
print "flg_kaitenn_k",self.flg_kaitenn_k
elif self.jjkk=='t':
print "self.jjkk==t"
#以下のコードで明確に回転がとまるようにする
#このコード重要、以下のようにしないといつまでも原因不明で回り続
#ける
self.tmp1 = self.joint.getAngle()
self.theta1 = self.tmp1
self.v = 0
#self.k = 20
self.z1 = self.theta1 - self.tmp1
self.v = self.k * self.z1
self.joint.setParam(ode.ParamVel, self.v)
print " "
print "set_kaiten_kakudo3"
print 'z1 %03.2f ' %self.z1,
print 'theta1 %03.2f ' %self.theta1,
print 'tmp1 %03.2f ' %self.tmp1,
print 'self.k',self.k
#先端の指の衝突をさける 角度の読み込み
#thm_kはものをつかむほうの動作
def read_yubi7xx3_angle():
f = open('manipulator_yubi69x2_angle.txt', 'r')
for row in f:
data_angle[0] = float(row)
f.close()
jointList[7].thm_j = -math.pi/10
jointList[7].thm_k = data_angle[0]
jointList[10].thm_j = -math.pi/10
jointList[10].thm_k = data_angle[0]
jointList[13].thm_j = -math.pi/10
jointList[13].thm_k = data_angle[0]
#先端の指の衝突をさける 角度の書き込み
def write_yubi7xx3_angle(angle):
f = open('manipulator_yubi69x2_angle.txt', 'w')
angle_str = str(angle)
f.write(angle_str)
f.close()
#ジョイント2と3の操作指令があったとき、床とマシンのハンド部分が激突しないよう
#ジョイント2と3の角度を制限する
#ジョイント1の最大角度をせいげんする(set_kaiten_kakudo23で利用)
def control_j23_yuka_syoutotu():
# th2 3 ジョイント２ 3 の初期値からの移動角度
th2 = jointList[2].joint.getAngle()
th3 = jointList[3].joint.getAngle()
#床に激突しないよう...
jointList[2].thm_k = (math.pi/2 - th3) / 2
jointList[3].thm_k = math.pi/2 - 2*th2
#jointList[2].thm_k = (math.pi/2 + th3) / 2
#jointList[3].thm_k = -math.pi/2 + 2*th2
#仮に適当な値を入れる
jointList[2].thm_j = -math.pi/4
jointList[3].thm_j = -math.pi/4
#ジョイント1
jointList[1].thm_k = math.pi
jointList[1].thm_j = -math.pi
##床に近くなる角度方向ならば...
#if th2 > 0 and th3 > 0:
# #下記式を満足すると床に激突する危険がない
# if j_no==2:
# thm_k = (math.pi/2 - th3) / 2
# if j_no==3:
# thm_k = math.pi/2 - 2*th2
#jointList[j_no].set_kaiten_kakudo23(jjkk, thm_j)
## 参考
## アーム23と床（マシンの台座の高さは無視）で二等辺三角形を作るとして
## 以下の式が成り立つ
## if th3 + 2*th2 > math.pi/2
#pos4 = objList[4].pos
##手の座標 手が上に回転している事もあるので一応-0.3とする
#pos14 = objList[14].pos
#pos15 = objList[15].pos
#pos16 = objList[16].pos
#pos_min = min(pos4[1]-0.3, pos14[1], pos15[1], pos16[1])
#ジョイント2 3 の角度を検知して「手」を水平に保つ
def control_hand_suihei():
print "control_hand_suihei"
jointList[4].joint.getAngle()
kakudo4r = jointList[4].joint.getAngle()
kakudo4 = jointList[4].joint.getAngle()/math.pi*180
print 'jointList[4].joint.getAngle() %5.2f' %kakudo4
print 'jointList[4].joint.getAngle() %5.2f' %kakudo4r
kakudo_syoki_r = math.pi/180*90
#jointList[4].set_kaiten_kakudo11(kakudo_syoki_r)
kakudo2r = jointList[2].joint.getAngle()
kakudo3r = jointList[3].joint.getAngle()
kakudo2 = kakudo2r/math.pi*180
kakudo3 = kakudo3r/math.pi*180
print 'jointList[2].joint.getAngle() %5.2f' %kakudo2
print 'jointList[2].joint.getAngle() %5.2f' %kakudo2r
print 'jointList[3].joint.getAngle() %5.2f' %kakudo3
print 'jointList[3].joint.getAngle() %5.2f' %kakudo3r
#kakudo_syoki_r= kakudo_syoki_r - kakudo2r - kakudo3r - math.pi/2
kakudo_syoki_r= kakudo_syoki_r - kakudo2r - kakudo3r
kakudo_syoki = kakudo_syoki_r/math.pi*180
print 'kakudo_syoki %5.2f' %kakudo_syoki
print 'kakudo_syoki_r %5.2f' %kakudo_syoki_r
#jointList[4].set_kaiten_kakudo11(kakudo_syoki_r)
jointList[4].set_kaiten_kakudo1(kakudo_syoki_r)
# 自動で
# ボールが落ちた角度を検知してそれをset_kaiten_kakudo2に送りその方向に
# 回転させる
#ジョイント 1 をコントロール
def control_j1_auto():
print "control_j1_auto():"
p_b = ballList[0].pos
print 'p_b %03.1f %03.1f %03.1f ' % p_b
kakudo = math.atan2(p_b[2], p_b[0])
print 'kakudo ', kakudo
return kakudo
#床の物体を検知 コマンドにおくりその位置に「手」を移動させる
#http://rmweb.rm.kanagawa-it.ac.jp/~hyodolab/Robot_Control/RC2013_No6.pdf
#物体の上から見たマシンに対する角度は control_j1_autoで出る
#物体の側面から見た位置はここで出す
#"v_x":0.5, "v_y":5.00, "v_z":0.5, "v_
#, "v_x":5.0, "v_y":0.50, "v_z":0.5,
#ボールの座標から
#自動でジョイント 2 3 をコントロール、ボールへ近づける
def control_j23_auto():
print "control_j23_auto():---------------------------------------"
l1 = 5 - 0.25 # 4.75
l2 = 5 + 0.25 # 5.25
p_b = ballList[0].pos
pos14 = objList[14].pos
#以下は逆運動からアームの角度を求める
#マシンが台座にのっているので
#y:逆運動を求めるための最初のジョイントを原点としたときの仮想のｙ
#y0:実際の座標のｙ
#y = y0 - yd
#px = p_b[0]
#<http://shiwasu.ee.ous.ac.jp/2link1.html>
#ジョイント1で水平面上でボールのほうへマシンを回転後、そのボールと原点を
#とおる垂直平面で逆運動を求めるため底辺は以下のpxの式となる
#dt_joint23[0] = 0.0
px = math.sqrt(pow(p_b[0],2) + pow(p_b[2],2))
py = p_b[1] - 2 + 1.6 + dt_joint23[1]
# 2 : マシンが台座1 2にのっていることを考慮
# 1.6: 指 手の台座 センサーを考慮
# dt : ものをつかむための微調整
#if pos14[1] > 0.3:
# print " if pos14[1] > 0.3:"
# dt_joint23[0] = (pos14[1] - 0.3)/100
# #if dt_joint23[0] == (pos14[1] - 0.5)/10:
# #dt_joint23[0] = 0.1
# dt_joint23[1] -= dt_joint23[0]
#elif pos14[1] > 0.2:
# dt_joint23[0] = 0.01
# dt_joint23[1] -= dt_joint23[0]
#elif pos14[1] > 0.1:
# dt_joint23[0] = 0.01
# dt_joint23[1] += dt_joint23[0]
#dt_joint23[1]を決めるのに上のコードを考えたがうまくいかない
#実験的にしたの実数を当てはめる。でもこれでは汎用性がない
#床と指のあいだは0.157 となる
#dt_joint23[1] = -0.3
#実験的にもとめた0.3を利用してそれに限りなく近づくdt_joint23[1]を
#作成
dt_joint23[0] = (0.3 + dt_joint23[1]) / 10
dt_joint23[1] -= dt_joint23[0]
print "pos14[1]33333333333333333333333", pos14[1]
print "dt_joint23[1]", dt_joint23[1]
#if pos14[1] - 0.05 > 0.01:
# dt_joint23[0] = 0.01
#else:
# dt_joint23[0] = (pos14[1] - 0.05)/10
#dt_joint23[1] -= dt_joint23[0]
c2 = (pow(px,2) + pow(py,2) - pow(l1,2) - pow(l2,2)) / (2 * l1 * l2)
s2 = math.sqrt(1 - pow(c2,2))
th2 = math.atan2(-s2, c2)
th1 = math.atan2(py,px) - math.atan2(-l2*s2, l1+l2*c2)
th10 = math.pi/2 - th1
th20 = -math.pi/2 - th2
#初期のマシンのジョイントの状態をかんがえると上の式となる
return th10,th20
#def control_j23_auto_yubi():
# pos14 = objList[14].pos
# #pos15 = objList[15].pos
# #pos16 = objList[16].pos
# #a = np.array(pos14)
# #b = np.array(pos15)
# #l = np.linalg.norm(b-a)
# if pos14[1] < 0.1:
# dt_joint23[0] -= 0.01
# else:
# dt_joint23[0] += 0.01
# ジョイント1をボールの方向へ回転させ、それに近づいたらジョイント23を
# ボールのほうへ回転させる
def control_j123_auto():
print "control_j123_auto"
kakudo1 = control_j1_auto()
jointList[1].set_kaiten_kakudo2(kakudo1)
if -math.pi/100 < jointList[1].z1 and jointList[1].z1 < math.pi/100:
kakudo23 = control_j23_auto()
jointList[2].set_kaiten_kakudo2(kakudo23[0])
jointList[3].set_kaiten_kakudo2(kakudo23[1])
#次ここから 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"]
jointList.append(SetHingeJoint(field, name, obj1, obj2, anchor0, j_axis0))
#print "metaSetJoint....hinge"
#print "len(jointList)++++++++++++++++",len(jointList)
if __name__=='__main__':
field = Field()
MetaBall(field)
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)},#台座1と台座２
{"name":"hinge", "obj1":objList[1], "obj2":objList[2], "anchor":(0, 2, 0), "j_axis":(0, 0, 1)},#台座2と第一アームの関節
{"name":"hinge", "obj1":objList[2], "obj2":objList[3], "anchor":(0.0, 6.75, 0), "j_axis":(0, 0, 1)}, #第一と第二アームの関節
{"name":"hinge", "obj1":objList[3], "obj2":objList[4], "anchor":(5.25, 6.75, 0), "j_axis":(0, 0, 1)}, #手
{"name":"hinge", "obj1":objList[4], "obj2":objList[5], "anchor":(5.75, 6.95, 0), "j_axis":(1, 0, 0)}, #指1の台
{"name":"hinge", "obj1":objList[5], "obj2":objList[6], "anchor":(5.85, 6.95, 0), "j_axis":(0, 0, 1)}, #指1
{"name":"hinge", "obj1":objList[6], "obj2":objList[7], "anchor":(6.35, 6.95, 0), "j_axis":(0, 0, 1)}, #指11
{"name":"hinge", "obj1":objList[4], "obj2":objList[8], "anchor":(5.75, 6.55, 0.2), "j_axis":(1, 0, 0)}, #指2の台
{"name":"hinge", "obj1":objList[8], "obj2":objList[9], "anchor":(5.85, 6.55, 0.2), "j_axis":(0, 0, 1)}, #指2
{"name":"hinge", "obj1":objList[9], "obj2":objList[10],"anchor":(6.35, 6.55, 0.2), "j_axis":(0, 0, 1)}, #指22
{"name":"hinge", "obj1":objList[4], "obj2":objList[11], "anchor":(5.75, 6.55, -0.2), "j_axis":(1, 0, 0)}, #指3の台
{"name":"hinge", "obj1":objList[11], "obj2":objList[12],"anchor":(5.85, 6.55, -0.2), "j_axis":(0, 0, 1)}, #指3
{"name":"hinge", "obj1":objList[12], "obj2":objList[13],"anchor":(6.35, 6.55, -0.2), "j_axis":(0, 0, 1)}, #指33
{"name":"fixed", "obj1":objList[13], "obj2":objList[14],"anchor":(6.85, 6.55, -0.2), "j_axis":(0, 0, 1)}, #指33センサー
{"name":"fixed", "obj1":objList[10], "obj2":objList[15],"anchor":(6.85, 6.55, 0.2), "j_axis":(0, 0, 1)}, #指22センサー
{"name":"fixed", "obj1":objList[7], "obj2":objList[16],"anchor":(6.85, 6.55, 0), "j_axis":(0, 0, 1)} #指22センサー
]
metaSetJoint(field)
for list in jointList:
if list.name=="hinge":
#print "list.name",list.name
list.kaitenn_para()
#これを実行しないとアームがダラーんとなってしまうだろう
#以下を実行すると 手のジョイントがフリーとなる
#jointList[4].joint.setParam(ode.ParamFMax, 0)
#ボール 床 指同士で接触ジョイントが働くようにする
list_yubi = [7,10,13]
for n in list_yubi:
objList[n].geom.setCategoryBits(8)
objList[n].geom.setCollideBits(9)
index_m = 1
jn = "1"
tx = 'joint ' + jn
lb_b = label(pos=(0, 0, 8), text=tx)
print 'sキーで「世界」の進行を止め'
print 'gキーで「世界」の進行を進めたり'
jjkk = "t"
flg_loop_j = False
flg_loop_k = False
foce =0.001
#foce =0.00
n = 0
loop_yubi = 0
flg_read = 0
while True:
#ジョイント2 3 の角度を検知して「手」を水平に保つ
control_hand_suihei()
#指2 3を60 -60 ｘ軸で回転させてものをつかめるようにする
jointList[8].set_kaiten_kakudo1(math.pi/180*-120)
jointList[11].set_kaiten_kakudo1(math.pi/180*120)
#指11 21 31を初期に外へまげて「手のひら」状態にする
#手が大きい方がものをよくつかめるだろう
jointList[6].set_kaiten_kakudo1(math.pi/180*-45)
jointList[9].set_kaiten_kakudo1(math.pi/180*-45)
jointList[12].set_kaiten_kakudo1(math.pi/180*-45)
field.tick()
#ballList[0].body.addForce((0,0,0.005))
#print "index_m-----------------", index_m
if index_m==0:
try:
f = open('manipulator_yubi69x2_angle.txt', 'r')
except:
#データがないなら一回したのコマンドで動かし
#データをファイルに書き込む
for n in [7, 10, 13]:
jointList[n].set_kaiten_kakudo3(jjkk)
else:
f.close
#tryでファイルがオープンできたということは指同士がぶつ
#からないデータがあるとしてそれを読み込む
read_yubi7xx3_angle()
for n in [7, 10, 13]:
#角度でジョイントをコントロールして動かす
#こちらのコマンドのほうが動作を早くできる
#指をつかむ動作させるとき、床にぶつからないように
#するのがむずかしい
control_j123_auto()
jointList[n].set_kaiten_kakudo23(jjkk)
if index_m==9:
tx = 'auto '
lb_b.text = tx
control_j123_auto()
if index_m==2 or index_m==3:
print " index_m==2 or index_m==3: ",
print "index_m", index_m ,
print "jjkk", jjkk
control_j23_yuka_syoutotu()
jointList[index_m].set_kaiten_kakudo23(jjkk)
if index_m==1 or index_m==4:
control_j23_yuka_syoutotu()
jointList[index_m].set_kaiten_kakudo23(jjkk)
print " if index_m==1 or index_m==4: *********************************** ",
print "index_m", index_m ,
print "jjkk", jjkk
print "jointList[index_m].thm_j",jointList[index_m].thm_j
print "jointList[index_m].thm_k",jointList[index_m].thm_k
# キーボードを押し続けないと反応しない。苦し紛れに以下のfor文
# for にしないとgetkey をあまりうけつけない
# for in range以外のコードもいろいろ試してやっとこれに落ち着く
for n in range(20000):
if field.scene.kb.keys:
jn = field.scene.kb.getkey()
#動かすジョイントの選択
#tをおしてジョイントが停止の状態でないと急にジョイント番号を
#変えるとマシンが分解する
#if jjkk=="t":
if jn=="0" or jn=="1" or jn=="2" or jn=="3" or jn=="4" or jn=="9":
jjkk = "t"
index_m = int(jn)
#print "index_m-----------------", index_m
tx = 'joint ' + jn
lb_b.text = tx
print("\007")
#音をならして操作するジョイント
#が変わった事を知らせる
if jn=="j" or jn=="k" or jn=="t":
jjkk = jn
if jn=='s':
flg_world =True
while flg_world:
if field.scene.kb.keys:
s2 = field.scene.kb.getkey()
if s2 == 'g':
flg_world = False
elif s2 != 's':
flg_world = True
else:
flg_world = True
0 件のコメント:
コメントを投稿