- プログラム：arm3dof_draft2.py
def arm_sol(self,trans) :
x=trans.vec[0]
y=trans.vec[1]
z=trans.vec[2]
th1_1=atan2(y,x)
th1_2=th1_1-pi
zz=z-self.bh-self.l1h
zz_2=zz**2
xx_2=x**2+y**2
th3=acos((xx_2+zz_2-self.l2h**2-self.l3h**2)/2.0/self.l2h/self.l3h)
psi=asin(zz/sqrt(xx_2+zz_2))
phi=asin(self.l3h*sin(th3)/sqrt(xx_2+zz_2))
th2_1=pi/2-psi-phi
th2_2=pi/2-psi+phi
self.solutions=[[th1_1,th2_1,th3],[th1_1,th2_2,-th3],
[th1_2,-th2_1,-th3],[th1_2,-th2_2,th3]]
return self.solutions
#
arm.solve=new.instancemethod(arm_sol,arm,arm.__class__)
- arm3dof_draft2.pyの使い方
>>> arm.make_shape()
>>> arm.mark()
>>> create_env()
>>> a=arm.solve(box.where(arm.base))
>>> a
[[-0.7853981633974483, 0.9019834836554195, 2.140119970115676],
[-0.7853981633974483, 3.0421034537710954, -2.140119970115676],
[-3.9269908169872414, -0.9019834836554195, -2.140119970115676],
[-3.9269908169872414, -3.0421034537710954, 2.140119970115676]]
>>> for pp in a:
arm.set_joints(pp)
sleep(1)
>>>
まず
env_arm3dof_draft2.pyを走らせる．
- 逆運動学(1)
def arm_sol(self,target) :
self.solutions=[]
pos=target.vec
z_axis=target.mat.col(2)
pos=pos-((self.l5h+self.l6h)*z_axis)
.
.
(1)joint 5の位置を求める。
ハンド座標の
z軸方向lh5+lh6(-0.15)の位置
target
y
x
(self.l5h+self.l6h)*z_axis
pos
- 逆運動学(2)
def arm_sol_pos(self,pos) :
.
.
.
(2)1,2,3は「前のl3h」=l3h+l4h(0.4)とし
たときの
3関節と同様に解くことが出来る。
pos
ここは
3自由度
アームの求め方
- 逆運動学(3)
def arm_sol_ori(self,target,th123) :
T123=self.fk123(th123)
zt=target.mat.col(2)
z3=T123.mat.col(2)
.
.
(3)joint 5の回転軸(y)は、手首z軸
と
link3z軸に垂直、回転角はそれらの
間の角度
(2方向あることに注意)
target
y
x
x
y
z
T123
zt
z3
y5, th5
- 逆運動学(4)
def arm_sol_ori(self,target,th123) :
T123=self.fk123(th123)
zt=target.mat.col(2)
z3=T123.mat.col(2)
.
.
(4)joint 4の回転角は、link5yと
link3yの成す角
target
y
x
x
y
z
T123
y5
y3
th4
- 逆運動学(5)
def arm_sol_ori(self,target,th123) :
T123=self.fk123(th123)
zt=target.mat.col(2)
z3=T123.mat.col(2)
.
.
(5)joint 6の回転角はlink5yと
wrist(=target) yの成す角
target
y
x
x
y
z
T123
y5
yt
th6
- arm6dof_draft2.py(1)
def arm_sol_pos(self,pos) :
x=pos[0]
y=pos[1]
z=pos[2]
th1_1=atan2(y,x)
th1_2=th1_1-pi
lh=self.l3h+self.l4h
zz=z-self.bh-self.l1h
zz_2=zz**2
xx_2=x**2+y**2
th3=acos((xx_2+zz_2-self.l2h**2-lh**2)/2.0/self.l2h/lh)
psi=asin(zz/sqrt(xx_2+zz_2))
phi=asin(lh*sin(th3)/sqrt(xx_2+zz_2))
th2_1=pi/2-psi-phi
th2_2=pi/2-psi+phi
self.solutions_pos=[[th1_1,th2_1,th3],[th1_1,th2_2,-th3],
[th1_2,-th2_1,-th3],[th1_2,-th2_2,th3]]
return self.solutions_pos
#
- arm6dof_draft2.py(2)
def fk123(self,th123):
T1=FRAME(xyzabc=[0,0,self.bh,0,0,th123[0]])
T2=FRAME(xyzabc=[0,0,self.l1h,0,th123[1],0])
T3=FRAME(xyzabc=[0,0,self.l3h+self.l4h,0,th123[2],0])
return T1*T2*T3
#
def arm_sol_ori(self,target,th123) :
T123=self.fk123(th123)
zt=target.mat.col(2)
z3=T123.mat.col(2)
.
.
.
.
return [th4,th5,th6],[th4_2,-th5,th6_2]
(3)(4)(5)
- arm6dof_draft2.py(3)
def arm_sol(self,target) :
self.solutions=[]
pos=target.vec
z_axis=target.mat.col(2)
pos=pos-((self.l5h+self.l6h)*z_axis)
self.solve_pos(pos)
for th123 in self.solutions_pos :
rslt=self.solve_ori(target,th123)
self.solutions.append(th123+rslt[0])
self.solutions.append(th123+rslt[1])
return self.solutions
(1)
(2)
- arm6dof_draft2.pyの使い方
>>> create_env()
>>> arm
<larm_w_hand.LinkedArm instance at 0x0350F5F8>
>>> arm.solve(box.where(arm.base)*arm.wrist.where(arm.hand))
[[-0.8519663271732721, 0.571657122775781, 1.6628010732898744,
1.1903745009548912e-16, 0.907134457524138, 2.289626326416521],
[-0.8519663271732721, 0.571657122775781, 1.6628010732898744,
・
・
[-3.993558980763065, -2.2344581960656553, 1.6628010732898744,
-3.141592653589793, 2.5699355308140124, 2.2896263264165215],
[-3.993558980763065, -2.2344581960656553, 1.6628010732898744,
2.6567490098619347e-16, -2.5699355308140124, -0.8519663271732717]]
>>> show_all_solutions(arm,box)
>>>
まず
env_arm6dof_draft2.pyを走らせる．
