Open Dynamics Engine


角度センサ(関節用)

double ja[1]; // 角度[rad]
ja[0]=dJointGetHingeAngle (joint[0]);

角速度センサ(関節用)

double jar[1]; // 角速度[rad/s]
jar[0]=dJointGetHingeAngleRate (joint[0]);

タッチセンサ(物体の接触判定)

グルーピングした物体無効化手法(各物体が各々判定可能)によるタッチセンサ

 物体間衝突の状態を示すnearCallbackルーチン内で,判定を行う.2物体を関節にする場合,2物体間の衝突を無効にするためグルーピングしがちであるが,グルーピングしてしまうと,グルーピングしたものの物体判定ができなくなってしまう.そのため,以下のプログラムにおいては,グルーピングせずに衝突判定の冒頭部で指定する物体間の衝突計算を行わないようにして衝突を無効化し,関節においての物体接触判定を実現している.
ただし,力センサのように『センサ部となる物体』を作り出し,その部分で接触判定することにすれば,このような無効化プログラムを記述しなくて良い.

  • 宣言部
    // 各物体の接触状態を検出:タッチセンサ
    int touch_box=0;
    int touch_box2=0;
    int touch_sphere=0;
    int touch_ccylinder=0;
  • 衝突部
    static void nearCallback (void *data, dGeomID o1, dGeomID o2)
    {
    int i,n;
     
    // 指定する2物体の衝突を無効化.
    if((o1==box[0])&&(o2==box[1])||(o2==box[0])&&(o1==box[1]))return;
    
    const int N = 10;
    dContact contact[N];
    n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    if (n > 0) {
     for (i=0; i<n; i++) {
     //----------- 
     // 接触設定コマンド省略
     //----------- 
     }
    }
    // 地面と物体との接触判定フラグ
    if((o1==box[0] && o2==ground)||(o2==box[0] && o1==ground))touch_box=1;
    if((o1==box[1] && o2==ground)||(o2==box[1] && o1==ground))touch_box2=1;
    if((o1==sphere[0] && o2==ground)||(o2==sphere[0] && o1==ground))touch_sphere=1;
    if((o1==ccylinder[0] && o2==ground)||(o2==ccylinder[0] && o1==ground))touch_ccylinder=1;
    }
  • simloop部
    static void simLoop (int pause)
    {
     int i;
     if (!pause) {
      dSpaceCollide (space,0,&nearCallback);
      dWorldStep (world,0.001);
      dJointGroupEmpty (contactgroup);
     }
     // 各物体の接触フラグ
     printf("box1:%d	box2:%d	sphere:%d	ccylinder:%d\n;",touch_box,touch_box2,touch_sphere,touch_ccylinder);
     touch_box=0;
     touch_box2=0;
     touch_sphere=0;
     touch_ccylinder=0;
    //----------------------------
    // 描画コマンド省略
    //----------------------------
    }
  • 物体&関節設定部
    body[1] = dBodyCreate (world);			
    dBodySetPosition (body[1],sphere_pos[0],sphere_pos[1],sphere_pos[2]);	
    dMassSetSphere (&m,1,sphere_radius);	
    dMassAdjust (&m,sphere_mass);				
    dBodySetMass (body[1],&m);
    sphere[0] = dCreateSphere (space,sphere_radius);
    dGeomSetBody (sphere[0],body[1]);					
    
    body[0]=dBodyCreate(world);					
    dBodySetPosition(body[0],box_pos[0],box_pos[1],box_pos[2]);
    dMassSetBox(&m,1,box_length,box_width,box_height);
    dMassAdjust(&m,box_mass);
    dBodySetMass(body[0],&m);
    box[0]=dCreateBox(space,box_length,box_width,box_height);
    dGeomSetBody (box[0],body[0]);					
     
    body[3]=dBodyCreate(world);
    dBodySetPosition(body[3],box_pos[0]+0.1,box_pos[1],box_pos[2]);	
    dMassSetBox(&m,1,box_length,box_width,box_height);		
    dMassAdjust(&m,box_mass);					
    dBodySetMass(body[3],&m);						
    box[1]=dCreateBox(space,box_length,box_width,box_height);		
    dGeomSetBody (box[1],body[3]);					
    
    joint[0] = dJointCreateHinge (world,0);
    dJointAttach (joint[0],body[0],body[3]);
    dJointSetHingeAnchor (joint[0],box_pos[0],box_pos[1],box_pos[2]);
    dJointSetHingeAxis (joint[0],0,1,0);
    dJointSetHingeParam (joint[0],dParamLoStop,-3.14/2.);
    dJointSetHingeParam (joint[0],dParamHiStop,+3.14/2.);

グルーピングされた物体も判定するタッチセンサ

摩擦&接触編でも説明したが,グルーピングした物体群(dSpaceID変数)をdGeomID変数として扱い,物体の衝突の判定を行うためには,dSpaceID変数とdGeomID変数を共にint型に変換すればよい.そのため,nearcallbackのdGeomID変数o1,o2をint型変数o1i,o2iに変換している.

  • 宣言部
    static dGeomID box[2], sphere[1], ccylinder[1], touch[1];
    static dSpaceID geom_group;
  • 衝突部
    static void nearCallback (void *data, dGeomID o1, dGeomID o2)
    {
      int i,n;
      int o1i,o2i;
      o1i=(int)o1;
      o2i=(int)o2;
    
      const int N = 10;
      dContact contact[N];
      n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
      if (n > 0) {
     //----------- 
     // 接触設定コマンド省略
     //-----------   } 
     if((o1i==(int)box[1] && o2i==(int)ground)||(o2i==(int)box[1] && o1i==(int)ground))touch_box2=1;
     if((o1i==(int)sphere[0] && o2i==(int)ground)||(o2i==(int)sphere[0] && o1i==(int)ground))touch_sphere=1;
     if((o1i==(int)ccylinder[0] && o2i==(int)ground)||(o2i==(int)ccylinder[0] && o1i==(int)ground))touch_ccylinder=1;
     if((o1i==(int)geom_group && o2i==(int)ground)||(o2i==(int)geom_group && o1i==(int)ground))touch_joint=1;
    }
  • 物体&関節設定部
    body[0]=dBodyCreate(world);	
    dBodySetPosition(body[0],box_pos[0],box_pos[1],box_pos[2]);
    dMassSetBox(&m,1,box_length,box_width,box_height);
    dMassAdjust(&m,box_mass);		
    dBodySetMass(body[0],&m);
    box[0]=dCreateBox(0,box_length,box_width,box_height);  // ←spaceに登録しない
    dGeomSetBody (box[0],body[0]);	
body[3]=dBodyCreate(world);
dBodySetPosition(body[3],box_pos[0]+0.1,box_pos[1],box_pos[2]);	
dMassSetBox(&m,1,box_length,box_width,box_height);		
dMassAdjust(&m,box_mass);					
dBodySetMass(body[3],&m);						
box[1]=dCreateBox(0,box_length,box_width,box_height);  // ←spaceに登録しない		
dGeomSetBody (box[1],body[3]);

// 関節を構成する物体群のグルーピング
// box[0]とbox[1]は,ここでspaceにの登録
geom_group = dSimpleSpaceCreate (space);  
dSpaceSetCleanup (geom_group,0);
dSpaceAdd(geom_group,box[0]);
dSpaceAdd(geom_group,box[1]);

重心センサ

double tpos[3]; // 重心座標(x,y,z)
double mass[3]; // 3物体の重量
// ロボットを構成する物体が3個の場合,まず各物体の中心座標を求める.
const dReal *pos0= dBodyGetPosition(body[0]);
const dReal *pos1= dBodyGetPosition(body[1]);
const dReal *pos2= dBodyGetPosition(body[2]);
// 重心座標の1要素は,すべての物体における[『物体中心座標の1要素』*『重量』]を算出し,
// その総和を総重量で割る.
tpos[0]=(pos0[0]*mass[0]+pos1[0]*mass[1]+pos2[0]*mass[2])/(mass[0]+mass[1]+mass[2]);
tpos[1]=(pos0[1]*mass[0]+pos1[1]*mass[1]+pos2[1]*mass[2])/(mass[0]+mass[1]+mass[2]);
tpos[2]=(pos0[2]*mass[0]+pos1[2]*mass[1]+pos2[2]*mass[2])/(mass[0]+mass[1]+mass[2]);

転倒センサ

90度転倒用

 初期設定時に『物体中心座標よりも上ある物体上座標』を指定し,シミュレーション中,常に指定した物体上座標の絶対座標を求め,中心座標と比較することで,転倒を検地する.

int upsidedown=0;	// 転倒フラグ
const dReal *pos0= dBodyGetPosition(body[0]); // 物体中心座標
// 物体座標系(相対座標系)における座標値(0,0,1)の絶対座標系における座標値を取得
// 座標値(0,0,1)は中心座標よりも高い位置にある.
dVector3 pos_up;	
dBodyGetRelPointPos (body[0], 0, 0, 1,pos_up);
// 上記した2個の絶対座標値の高さを比較.初期設定において中心座標値より上部にあった
// 相対座標値が高い位置となったとき,物体が90度以上転倒することを意味するため,
// upsidedownフラグを1とする.
if(pos_up[2]<pos[2]){upsidedown=1;} 
else {upsidedown=0;}

力センサ

タイプ1:固定関節の関節状態計測を利用した力センサ

  • 宣言部
    // 関節状態変数の宣言
    dJointFeedback *joint_data0;
  • simloop部
    // Z軸方向の力データ出力
    printf("%fn",joint_data0->f1[2]);
  • 物体&関節設定部
    // 力計測対象物体 
    body[i]=dBodyCreate(world);	
    dBodySetPosition(body[i],box_x[i],box_y[i],box_z[i]);
    dMassSetBox(&m,1,box_l[i],box_w[i],box_h[i]);
    dMassAdjust(&m,box_mass[i]);					
    dBodySetMass(body[i],&m);
    box[i]=dCreateBox(0,box_l[i],box_w[i],box_h[i]);
    dGeomSetBody (box[i],body[i]);					
    
    // センサ部(ポイントは,重量無しにすること)
    body[i+1]=dBodyCreate(world);
    dBodySetPosition(body[i+1],box_x[i+1],box_y[i+1],box_z[i+1]);
    dMassSetZero(&m);
    dMassSetBox(&m,1,box_l[i+1],box_w[i+1],box_h[i+1]);
    dBodySetMass(body[i+1],&m);			
    box[i+1]=dCreateBox(0,box_l[i+1],box_w[i+1],box_h[i+1]);
    dGeomSetBody (box[i+1],body[i+1]);	
    
    // 固定関節(関節状態変数の設定)
    joint[i] = dJointCreateFixed (world,0);
    dJointAttach (joint[i],body[i],body[i+1]);
    dJointSetFixed  (joint[i]);
    joint_data0 = (dJointFeedback *)malloc(sizeof(dJointFeedback));
    dJointSetFeedback(joint[i], joint_data0);
    joint_data0 = dJointGetFeedback(joint[i]);
    

タイプ2:スライダ粘弾性関節を利用した力センサ

 物体間衝突の粘弾特性を利用して力を計測する.具体的な計測方法としては,「物体の中心位置」と「物体とスライダ粘弾性関節により接続した物体の中心位置」の距離に粘弾性係数を掛け,力を算出する方法である.なお,ここでは計算を簡単にするため変位量のみを利用している.より正確な値を求めるためには,変位だけでなく変位速度を利用して,力の換算を行う方が良い.

// 粘弾度は,力センサの感度に相当.
erp=DT*KP/(DT*KP+KD);
cfm=1/(DT*KP+KD);
force1.jpg
force2.jpg
  • 宣言部
    // 刻み時間
    #define DT 0.001
    // 力センサの反応感度調整(粘弾性調整)
    #define KP 4000
    #define KD 100
  • Simloop部
    // スライダ関節の位置測定(押された場合だけ,センサが反応)
    dReal pre0 = dJointGetSliderPosition (joint[1]);
    dReal pre;
    if(pre0>=0){
    	pre=0;
    }
    else {
    	pre=fabs(pre0);
    }
    // 力(=押された距離*弾性係数)データ出力
    printf("%f\n",pre*KP);
  • 物体&関節設定部
    // 力計測対象物体 
    body[i]=dBodyCreate(world);	
    dBodySetPosition(body[i],box_x[i],box_y[i],box_z[i]);
    dMassSetBox(&m,1,box_l[i],box_w[i],box_h[i]);
    dMassAdjust(&m,box_mass[i]);					
    dBodySetMass(body[i],&m);
    box[i]=dCreateBox(0,box_l[i],box_w[i],box_h[i]);
    dGeomSetBody (box[i],body[i]);					
    
    // センサ部(ポイントは,重量無しにすること)
    body[i+1]=dBodyCreate(world);
    dBodySetPosition(body[i+1],box_x[i+1],box_y[i+1],box_z[i+1]);
    dMassSetZero(&m);
    dMassSetBox(&m,1,box_l[i+1],box_w[i+1],box_h[i+1]);
    dBodySetMass(body[i+1],&m);			
    box[i+1]=dCreateBox(0,box_l[i+1],box_w[i+1],box_h[i+1]);
    dGeomSetBody (box[i+1],body[i+1]);	
    
    // スライダ関節(Z軸,移動範囲0の粘弾性関節)
    joint[i] = dJointCreateSlider (world,0);
    dJointAttach (joint[i],body[i],body[i+1]);
    dJointSetSliderAxis (joint[i],0,0,1);
    dJointSetSliderParam (joint[i],dParamLoStop,-0);
    dJointSetSliderParam (joint[i],dParamHiStop,0);
    dJointSetSliderParam (joint[i],dParamStopERP,DT*KP/(DT*KP+KD));
    dJointSetSliderParam (joint[i],dParamStopCFM,1/(DT*KP+KD));

サンプルプログラムコード:2種類の力センサの比較

 上記した2種類の力センサの性能比較.性能として,タイプ1の方は関節力状態をそのまま出力するので即応性は良いが定常状態になると二値を振動する.それに対して,タイプ2の方は粘弾性調整が必要であり,即応性がタイプ1より劣るが,定常状態になると安定した値が得られる.

加速度センサ

タイプ1:固定関節の関節状態計測を利用した加速度センサ

 基本的に,『固定関節の関節状態計測を利用した力センサ』と同様である.違う点は,計測値を求めるためにセンサ部の重量で割ることである.

// スライダ関節の位置測定(力を受けた分だけ,センサが反応)
dReal acc1 = dJointGetSliderPosition (joint[1]);
// 加速度(=押された距離*弾性係数/センサ部微小重量)データ出力
printf("%f\n",acc1*KP/mass);

タイプ2:スライダ粘弾性関節を利用した加速度センサ

 加速度センサは,スライダー関節を利用したものです.スライダー関節を粘弾性関節(スライダ移動範囲は零に設定すること)にし,物理力を受けて微小変位する距離を計測するものである.この方法は,実際の加速度センサと同じコンセプトのはずである.

  • 関節初期設定部
    // 粘弾性は,加速度センサの感度に相当.
    erp=DT*KP/(DT*KP+KD);
    cfm=1/(DT*KP+KD);
    // スライダー0(Joint2):x軸方向の加速度検出用
    joint[2] = dJointCreateSlider  (world,0);
    dJointAttach (joint[2],body[1],body[3]);
    dJointSetSliderAxis  (joint[2],1,0,0);
    dJointSetSliderParam (joint[2],dParamLoStop,-0);
    dJointSetSliderParam (joint[2],dParamHiStop,0);
    dJointSetSliderParam (joint[2],dParamStopERP,erp);
    dJointSetSliderParam (joint[2],dParamStopCFM,cfm);
    dJointSetSliderParam (joint[2],dParamFMax,0.1);
    // スライダー1(Joint3):z軸方向の加速度検出用
    joint[3] = dJointCreateSlider  (world,0);
    dJointAttach (joint[3],body[1],body[4]);
    dJointSetSliderAxis  (joint[3],0,0,1);
    dJointSetSliderParam (joint[3],dParamLoStop,-0);
    dJointSetSliderParam (joint[3],dParamHiStop,0);
    dJointSetSliderParam (joint[3],dParamStopERP,erp);
    dJointSetSliderParam (joint[3],dParamStopCFM,cfm);
    dJointSetSliderParam (joint[3],dParamFMax,0.1);
  • Simloop部
    // スライダ関節の移動距離計測
    dReal acc_x= dJointGetSliderPosition (joint[2]);
    dReal acc_z = dJointGetSliderPosition (joint[3]);
    // 力を重量で割って,加速度を算出している.
    printf("%f	%f\n",-acc_z*KP/box_mass[3],-acc_x*KP/box_mass[4]);

サンプルプログラムコード:2種類の加速度センサの比較

 基本的には,力センサと同じ.違う点は,センサ物体も微小重量を持っている点である.なぜなら重量を持たない場合,センサが力を受けないため,加速度検出ができないためである.サンプルプログラムは,上記した2種類の力センサの性能比較を行っている.結果は,力センサと同様な結果となっている.

距離センサ

 RAYを使用した距離センサ.RAYは基本的にGEOMとして扱われるものであるので,物体との衝突位置を取得することができる.そのため,『RAY』開始座標と『RAYと物体の衝突』座標の距離を測定するものである.このプログラムで注目すべき点は,衝突判定後に衝突位置データだけを取り出し,衝突の物理作用を起こす前に衝突ルーチンを終了する点です.

  • 宣言部
    static dGeomID ir_ray;
    int ir_flag=0;
    double ir_length;
    dVector3 pos0,pos1,pos2;
  • 衝突部(nearCallback内)
    // 省略
    n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    if (n > 0) {
    	if(o1==ir_ray || o2==ir_ray){
    		ir_flag=1;
    		pos2[0]=contact[0].geom.pos[0];
    		pos2[1]=contact[0].geom.pos[1];
    		pos2[2]=contact[0].geom.pos[2];
    		return;
    	}
    	for (i=0; i<n; i++) {
    // 省略
  • (simLoop内)
    // 省略
    dBodyGetRelPointPos(body[0],LENGTH/2.,0,0,pos0);
    dBodyGetRelPointPos(body[0],LENGTH/2.+2,0,0,pos1);
    dGeomRaySet (ir_ray, pos0[0],pos0[1],pos0[2],pos1[0]-pos0[0], pos1[1]-pos0[1], pos1[2]-pos0[2]);
    if(ir_flag==1){
    	ir_length=sqrt(pow((pos2[0]-pos0[0]),2)+pow((pos2[1]-pos0[1]),2)+pow((pos2[2]-pos0[2]),2));
    ir_flag=0;
    }
    else {
    	ir_length=2.;
    }
    printf("距離:%2.3f[m]\n",ir_length);
    dSpaceCollide (space,0,&nearCallback);
    dWorldStep (world,0.05);
    // 省略
  • 描画部(simLoop内)
    // 省略
    dVector3 ss;
    dGeomBoxGetLengths (ground_box,ss);
    dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
    if(ir_flag==0){dsSetColor (1,1,1);}
    if(ir_flag==1){dsSetColor (1,0,0);}
    dsDrawLine (pos0,pos1);
    }
  • 物体生成部(main内)
    // 省略
    dGeomSetRotation (ground_box,R);
    ir_ray=dCreateRay (space,2);
    // run simulation
    dsSimulationLoop (argc,argv,640,480,&fn);
    // 省略

サンプルプログラムコード:test_buggyに距離センサを装備


添付ファイル: filesensor_force.cpp 1807件 [詳細] filesensor_accelerometer.cpp 1879件 [詳細] fileforce2.jpg 1022件 [詳細] filesensor_distance.cpp 2217件 [詳細] fileforce1.jpg 959件 [詳細]

トップ   編集 凍結解除 差分 バックアップ 添付 複製 名前変更 リロード   一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2007-01-16 (火) 10:35:11 (3932d)