(11-2) 2019.6.26 電気通信大学大学院情報理工学研究科末廣尚士
- 手先の軌道生成 ( 再掲 ) その都度, 逆運動学計算をするとは少し手間がかかる. 本当に必要か? 分割が小さければ, ニュートン ラフソン法で 収束 させる必要はないかもしれない. 2
- 直線軌道で分割する ( 再掲 ) 3
- 関節角の微少量をもとめる ( 再掲 ) 4
- 分解運動 ( 速度 ) 制御 ( 再掲 ) ik を解く過程の関節角でアームを動かせば OK 目標が遠い場合は, 小さく刻んで, それに応じた関節角の変化量で, アームを小さく動かす. これを分解運動 ( 速度 ) 制御と呼ぶ ik のプログラム 1 ループごとにアームを動かしてみる. 5
20 手先軌道生成 RTC - 基本方針 rmrc RTC ik の途中課程でアームを動かす関数がベース 加速度に上限を設ける アームの構造, 制御パラメタはコンフィギュレーションポートで変更可能とする 6
- 基本処理 現在手先座標系を目標手先座標系に近づけるための微少座標変位を決定する それをヤコビの逆行列を用いて関節角変移に変換する それを関節角度に加えて目標関節角度として出力する 7
- 入出力 入力 : 目標手先座標系 + 近接パラメタ現在関節角度 出力 : 関節角軌道の目標関節角度, 状態情報 8
- 状態 目標への収束状況手先速度, 加速度の限界関節速度, 加速度の限界ヤコビ行列のランク 9
- コンフィギュレーション アームの構造 : arm_conf { 'dof ':6, 'j_conf ':[[[0,0,0.1,0,0,0],5],[[0,0,0.05,0, 0,0],4], [[0,0,0.4,0,0,0],4],[[0,0,0.3,0,0,0],5], [[0,0,0.1,0,0,0],4],[[0,0,0.1,0,0,0],5]], 'wrist ':[0,0, 0.05,0,0,0], 'tool ':[0,0,0.1,pi,0,0]} 制御パラメタ :ctrl_param { 'vmax ':0.5, 'alpha ':1.0, 'kp ': 1.0, 'rt_rate ':1.0, 'r_vmax ':pi/4.0, 'r_alpha ':pi/2, 'tau ':0.1, 'elim ':0.0001, 'rlim ': 0.001} 10
- 手先フィードバック速度の決定 現在位置姿勢と目標位置姿勢の差に比例ゲインをかけて目標手先速度を決定 目標手先速度が最大速度を超えないように制限 現在手先速度と目標手先速度の差が最大加速度を超えないように手先加速度を制限 現在手先速度と手先加速度から手先速度を決定 11
- 目標関節値の決定 手先速度に逆ヤコビ行列をかけて目標関節速度を決定 目標関節速度が最大関節速度を超えないように制限 現在関節速度と目標関節速度の差が最大関節加速度を超えないように制限 現在関節速度と関節加速度から関節速度を決定 現在関節値と関節速度から目標関節値を決定 12
- rmrc.py def read_current(self) : self.c_th=self._current_jointsin.read().data self.c_frame,self.j_mat=calc_fk_jacob(self.c_th,self.arm_conf) self.c_pos=self.c_frame.vec self.c_ori=self.c_frame.mat self.j_mat[3:6]=self.rt_rate*self.j_mat[3:6] self.u_mat,self.w_vec,self.v_trn = la.svd( self.j_mat, compute_uv=1, full_matrices=0) self.u_trn=self.u_mat.transpose() self.v_mat=self.v_trn.transpose() self.rank=0 w_max=max(self.w_vec) w_min=w_max*self.rlim for j in range(self.w_size) : if self.w_vec[j]<w_min : self.w_inv[j]=0.0 else : self.w_inv[j]=1.0/self.w_vec[j] self.rank += 1 現在関節角を読んで, 順運動学, ヤコビ行列およびその特異値分解を行う 13
- rmrc.py def read_target(self) : tmp=self._target_framein.read().data self.t_pos=vector(vec=tmp[9:12]) self.t_ori=matrix() for i in range(3) : for j in range(3) : self.t_ori[i][j]=tmp[3*i+j] if len(tmp) > 12 : self.dlim=tmp[12] else : 目標座標系を読む self.dlim=self.elim self.t_frame=frame(mat=self.t_ori,vec=self.t_pos) 14
- rmrc.py 収束状態 比例則の目標速度 手先制御状態 OnExecute の中身 def onexecute(self, ec_id): if self._target_framein.isnew() : self.read_target() if self._current_jointsin.isnew() : self.read_current() self.state=0 p_diff = self.t_pos - self.c_pos o_diff = ((- self.c_ori) * self.t_ori).rot_axis() o_diff[1]=self.rt_rate*o_diff[0]*(self.c_ori * o_diff[1]) v_tgt=np.concatenate([p_diff,o_diff[1]]) if la.norm(v_tgt) > self.d_lim : self.state=1 v_tgt=self.kp*v_tgt v_norm=la.norm(v_tgt) self.f_state=0 入力ポートに新しいデータがあれば読み込む 目標と現在の座標系の差を計算する 差が許容値より大きければ,state を 1 とする 15
- rmrc.py 目標速度が上限を超えていればトリミング if v_norm > self.vmax : v_tgt=v_tgt/v_norm*self.vmax self.f_state=1 v_tgt=v_tgt-self.v_frm v_norm=la.norm(v_tgt) if v_norm > self.alpha*self.tau : v_tgt=v_tgt/v_norm*self.alpha*self.tau self.f_state=self.f_state+2 self.v_frm=self.v_frm+v_tgt tmp=np.dot(self.u_trn,self.v_frm) tmp=tmp*self.w_inv v_th_tgt=np.dot(self.v_mat,tmp) v_th_max=max(abs(v_th_tgt)) self.j_state=0 if v_th_max > self.r_vmax : v_th_tgt=v_th_tgt/v_th_max*self.r_vmax self.j_state=1 現在速度と目標速度から加速度を計算加速度が上限を超えていればトリミング 速度, 加速度ともに上限を超えないようにトリミングした結果を新たな座標系の速度とする 関節角速度を計算して, 関節角速度が上限を超えていればトリミング 16
- rmrc.py v_th_tgt=v_th_tgt-self.v_th v_th_max=max(abs(v_th_tgt)) if v_th_max > self.r_alpha*self.tau : v_th_tgt=v_th_tgt/v_th_max*self.r_alpha*self.tau self.j_state=self.j_state+2 self.v_th=self.v_th+v_th_tgt tmp=np.dot(self.v_trn,self.v_th) tmp=tmp*self.w_vec self.v_frm=np.dot(self.u_mat,tmp) th=self.c_th+self.tau*self.v_th self._d_target_joints.data=th.tolist() OpenRTM_aist.setTimestamp(self._d_target_joints) self._target_jointsout.write() 現在関節角速度と目標関節角速度から関節角加速度を計算加速度が上限を超えていればトリミング 速度, 加速度ともに上限を超えないようにトリミングした結果を新たな関節角速度とする 関節角速度から座標系の速度を再計算 関節角速度から関節角度を計算してロボット RTC に送る. self._d_state.data=[self.state,self.f_state,self.j_state,self.rank] OpenRTM_aist.setTimestamp(self._d_state) self._stateout.write() return RTC.RTC_OK 17
- rmrc RTC を使う arm_control_4.py subprocess.popen("cd../rmrc; xterm -e python rmrc.py",shell=true) rmrc = env.name_space[ns0].rtc_handles['lecture2018.host_cxt/rmrc0.rtc'] c_rmrc_arm = IOConnector([rmrc.outports['target_joints'], arm.inports['target_joints']]) c_arm_rmrc = IOConnector([arm.outports['current_joints'], rmrc.inports['current_joints']]) make_pipe(comp,rmrc) rmrc.in_pipe['target_frame'].connect() rmrc.out_pipe['state'].connect() 18
- rmrc RTC を使う arm_control_4.py def move_rmrc(rmrc, t_frame, dlim=0.0) : tmp=[] for i in range(3) : for j in range(3) : tmp.append(t_frame.mat[i][j]) for i in range(3) : tmp.append(t_frame.vec[i]) if dlim > 0.0 : tmp.append(dlim) rmrc.in_pipe['target_frame'].write(tmp) 19
- rmrc RTC を使う arm_control_4.py def log_trj_rmrc(rmrc,t_frame,file,lim=0.01): def read_joints(): if arm.out_pipe['current_joints'].pipe.isnew() : return arm.out_pipe['current_joints'].read() else : return None def read_state(): if rmrc.out_pipe['state'].pipe.isnew() : return rmrc.out_pipe['state'].read() else : return None 20
- rmrc RTC を使う arm_control_4.py f = open(file,'w') f.write('[') jj = read_joints() # print jj ss = read_state() while (not jj) or (not ss) : time.sleep(1) jj=read_joints() ss=read_state() jj_old=jj t_joints=ik(t_frame,jj,arm_conf) print ss cc = 0 f.write(str(jj)) dd=max(map(lambda x,y: abs(x-y), t_joints, jj)) 21
- rmrc RTC を使う arm_control_4.py move_rmrc(rmrc,t_frame) while dd > lim : jj = read_joints() ss = read_state() if ss : print cc, ss if jj and (jj!= jj_old): cc += 1 f.write(',') f.write(str(jj)) dd=max(map(lambda x,y: abs(x-y), t_joints, jj)) jj_old=jj else : pass f.write(']') f.close() return 'OK' 22
- arm_control_4.py を使う jupyter notebook arm_control_4 を読み込んで始点, 終点の関節角および手先座標を設定する # coding: utf-8 # In[1]: from arm_control_4 import * # In[2]: j1=[pi/3,pi/4,pi/2,0,pi/4,pi/6] j2=[-pi/3,0,pi/3,pi/12,pi/8,0] t1=calc_fk_jacob(j1,arm_conf)[0] t2=calc_fk_jacob(j2,arm_conf)[0] 23
- 関節角モードで動作 まず関節角モードで動作させてログを取る # In[3]: c_arm_trj.connect() c_trj_arm.connect() # In[4]: arm.activate() trj.activate() # In[5]: move5(trj,j1) # In[6]: log_trj(j2,'test2.dat') 24
- 手先座標モード 手先座標モードで動かしてログを取る # In[7]: trj.deactivate() c_trj_arm.disconnect() # In[8]: c_arm_rmrc.connect() c_rmrc_arm.connect() # In[9]: rmrc.activate() # In[10]: move_rmrc(rmrc,t1) # In[11]: log_trj_rmrc(rmrc,t2,'rmrc.dat',0.01) 25
- 関節角のグラフ 関節角のグラフ表示 # In[12]: get_ipython().magic(u'matplotlib notebook') import numpy as np import matplotlib.pyplot as plt ff=open('test2.dat') aa=ff.read() ff.close() bb=eval(aa) # In[13]: plt.plot(range(len(bb)),bb) plt.legend(["th1","th2","th3","th4","th5","th6"]) 26
- 関節角のグラフ 27
- 関節角速度のグラフ 関節角速度のグラフ表示 # In[14]: cc=[] for i in range(len(bb)-1) : cc.append(map(lambda x,y : x-y, bb[i+1], bb[i])) plt.close() plt.plot(range(len(cc)),cc) plt.legend(["th1","th2","th3","th4","th5","th6"]) 28
- 関節角速度のグラフ 29
- 手先位置のグラフ 手先位置のグラフ表示 # In[15]: dd = map(lambda x : calc_fk_jacob(x,arm_conf)[0], bb) ff=map(lambda x: x.vec, dd) plt.close() plt.plot(range(len(ff)),ff) plt.legend(["x","y","z"]) 30
- 手先位置のグラフ 31
- 手先速度のグラフ 手先速度のグラフ表示 # In[16]: dd = map(lambda x : calc_fk_jacob(x,arm_conf)[0], bb) ff=map(lambda x: x.vec, dd) ee=[] for i in range(len(ff)-1) : ee.append(map(lambda x,y : x-y, ff[i+1], ff[i])) plt.close() plt.plot(range(len(ee)),ee) plt.legend(["x","y","z"]) 32
- 手先速度のグラフ 33
- 3D グラフのスケール調整 3D グラフのスケール調整 # In[17]: from mpl_toolkits.mplot3d import Axes3D def AxisEqual3d(x,y,z): #for 3d axis equal max_range = np.array([x.max()-x.min(), y.max()-y.min(), z.max()- z.min()]).max() Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(x.max()+x.min()) Yb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][1].flatten() + 0.5*(y.max()+y.min()) Zb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][2].flatten() + 0.5*(z.max()+z.min()) for xb, yb, zb in zip(xb, Yb, Zb): ax.plot([xb], [yb], [zb], 'w') 34
- 手先位置の 3D 軌跡 手先位置の 3D 軌跡表示 plt.close() fig = plt.figure() ax = fig.gca(projection='3d') xx=np.array([p.vec[0] for p in dd]) yy=np.array([p.vec[1] for p in dd]) zz=np.array([p.vec[2] for p in dd]) ax.plot(xx,yy,zz) ax.legend(["xyz"]) #ax.set_aspect('equal','datalim') AxisEqual3d(xx,yy,zz) 35
- 手先位置の 3D 軌跡 36
- 手先座標モード ( 関節角のグラフ ) 関節角のグラフ表示 # In[12]: get_ipython().magic(u'matplotlib notebook') import numpy as np import matplotlib.pyplot as plt ff=open( rmrc.dat') aa=ff.read() ff.close() bb=eval(aa) # In[13]: plt.plot(range(len(bb)),bb) plt.legend(["th1","th2","th3","th4","th5","th6"]) 37
- 関節角のグラフ 38
- 関節角速度のグラフ 39
- 手先位置のグラフ 40
- 手先速度のグラフ 41
- 手先位置の 3D 軌跡 42
- 関節角のグラフ 43
- 関節角速度のグラフ 44
- 手先位置のグラフ 45
- 手先速度のグラフ 46
- 手先位置の 3D 軌跡 47
- 次回予告 これまでで, 手先の座標系を指定して動くロボットアームが出来た. 自分のロボットアームを作る ( 次々回 ) 手先の座標系ベースのロボット作業プログラム ( その後 ) 48