11. 力とトルク

ODE (Open Dynamics Engine) 初級講座の第11回目です。

今回は 力とトルクについて勉強しましょう。

1.  ボディに関する力とトルク

ボディにかかる力、トルクを取得するAPIはdBodyGetForce(), dBodyGetTorqueです。ボディに力、トルクを設定するAPIはdBodySetForce, dBodySetTorqueとなっています。座標系は絶対座標系です。

  • const dReal *dBodyGetForce (dBodyID body);
  • const dReal *dBodyGetTorque (dBodyID body);
    ボディbodyの力とトルクベクトルを返す。戻り値はdReal型の要素数3個の配列へのポインタ。
  • void dBodySetForce(dBodyID b, dReal x, dReal y, dReal z);
  • void dBodySetTorque(dBodyID b, dReal x, dReal y, dReal z);
    力とトルクベクトルをボディbodyに設定する。

 

2.ジョイント(関節)に関する力とトルク

(1) 力とトルクの取得

次にジョイントに関する力とトルクについて学びましょう。ODEでは関節に かかるトルクなどをすぐ取得できません.取得するためにはdJointSetFeedback()で関節を指定してからdJointGetFeedback()で 情報を取得します.これはパフォーマンスを向上させるためです.常に全ての関節にかかる力とトルクが必要なわけではありませんよね.

  • void dJointSetFeedback (dJointID, dJointFeedback *);
    力とトルクの情報を取得する関節JointIDにdJointFeedback構造体を設定する. 

    typedef struct dJointFeedback {
    dVector3 f1; // 関節がボディ1に及ぼしている力
    dVector3 t1; // 関節がボディ1に及ぼしているトルク
    dVector3 f2; // 関節がボディ2に及ぼしている力
    dVector3 t2; // 関節がボディ2に及ぼしているトルク
    } dJointFeedback;

  • dJointFeedback *dJointGetFeedback (dJointID);
    dJointIDで指定している関節の力とトルクの情報を取得します.

(2) 力とトルクの設定

ジョイントの種類によって力またはトルクを設定することになります。つまり、ヒンジジョイントのような回転式ジョイントにはトルクを与えれば良いですし、スライダジョイントのような直動式ジョイントには力を与えれば良いわけです。

  • dJointAddHingeTorque(dJointID joint, dReal torque)
    ヒンジジョイントjointに対してトルクtorqueをかけます。ジョイントに結合されているボディ1とボディ2に対して同じトルクを反対方向にかけています。これはdBodyAddTorqueを元に作られています。
  • dJointAddSliderForce(dJointID joint, dReal force)
    スライダジョイントの軸に対して力forceをかけます。ジョイントに結合されているボディ1とボディ2に対して同じ力を反対方向にかけています。これはdBodyAddForceを元に作られています。

次 に,このAPIを使ったサンプルプログラムを紹介します.2つのボックスを固定ジョイント(Fixed Joint)でくっつけ,そこにかかる力を表示するプログラムです.ボックスの重さが各1kgなので,z軸方向(上方向)には9.8Nの力がかかっていれ ば理論どおりです.私の環境では理論どおり9.8の値をたたき出していました.

なお,プログラムでは2つのボックスのうち,下を圧力センサとみなしています.ヒューマノイド足裏の圧力センサをシミュレートする場合は,下のボックスのサイズを小さくし,数を増やせばよいわけです.

次からサンプルプログラムをダウンロードできます。

// sample11.cpp  by Kosei Demura  2006-2008
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"

static dWorldID world;
static dSpaceID space;
static dGeomID  ground;
static dJointID fixed;
static dJointGroupID
contactgroup;
dJointFeedback *feedback = new dJointFeedback;
dsFunctions fn;

typedef struct {
  dBodyID body;
  dGeomID geom;
  dReal   radius,length,width,height,mass;
} myLink;

myLink box,sensor;

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  static int MAX_CONTACTS = 10;
  int i;    // 2つのボディがジョイントで結合されていたら衝突検出しない

  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);

  if (b1 && b2 && dAreConnected (b1,b2)) return;

  dContact contact[MAX_CONTACTS];
  int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,   sizeof(dContact));

  if (numc > 0) {     for (i=0; i < numc; i++) {
    contact[i].surface.mode  =  dContactSoftCFM | dContactSoftERP;
    contact[i].surface.mu       = dInfinity;  // 摩擦係数
    contact[i].surface.soft_cfm = 1e-8;
    contact[i].surface.soft_erp = 1.0;

    dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
    dJointAttach (c,dGeomGetBody(contact[i].geom.g1),        dGeomGetBody(contact[i].geom.g2));
  }
}

static void simLoop (int pause)
{
  static int steps = 0;
  dSpaceCollide(space,0,&nearCallback);

  dWorldStep(world,0.01);
  dJointGroupEmpty(contactgroup);
  feedback = dJointGetFeedback(fixed); // 力とトルク情報の取得
  printf("%5d Force fx=%6.2f ",steps++,feedback->f1[0]); // x座標成分
  printf("fy=%6.2f ",feedback->f1[1]);       // y座標成分
  printf("fz=%6.2f \n",feedback->f1[2]);      // z座標成分

  // ボックスの描画
  dsSetColor(1.0,0.0,0.0);
  dReal sides1[] = {box.length,box.width,box.height};

  dsDrawBoxD(dBodyGetPosition(box.body),   dBodyGetRotation(box.body),sides1);    // センサの描画
  dsSetColor(0.0,0.0,1.0);
  dReal sides2[] = {sensor.length,sensor.width,sensor.height};

  dsDrawBoxD(dBodyGetPosition(sensor.body),    dBodyGetRotation(sensor.body),sides2);
}

void start()
{
  static float xyz[3] = {0.0,-3.0,1.0};
  static float hpr[3] = {90.0,0.0,0.0};
  dsSetViewpoint (xyz,hpr);
}

void  setDrawStuff()
{
  fn.version = DS_VERSION;
  fn.start   = &start;
  fn.step    = &simLoop;
  fn.command = NULL;
  fn.stop    = NULL;
  fn.path_to_textures = "../../drawstuff/textures";
}

int main (int argc, char **argv)
{
  setDrawStuff();
  dInitODE();
  world = dWorldCreate();
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);
  dWorldSetGravity(world,0,0,-9.8);
  ground = dCreatePlane(space,0,0,1,0);

  dMass m1;
  dReal x0 = 0.0, y0 = 0.0, z0 = 0.0;

  // センサ(下のボックス)
  sensor.length = 0.2;
  sensor.width  = 0.2;
  sensor.height = 0.2;
  sensor.mass   = 1.0;
  sensor.body   = dBodyCreate(world);
  dMassSetZero(&m1);
  dMassSetBoxTotal(&m1,sensor.mass,sensor.length,sensor.width,sensor.height);
  dBodySetMass(sensor.body,&m1);
  dBodySetPosition(sensor.body, x0, y0, 0.5 * sensor.height + z0);
  sensor.geom = dCreateBox(space,sensor.length,sensor.width,sensor.height);
  dGeomSetBody(sensor.geom,sensor.body);

   // ボックス(上のボックス)
  box.length = 0.2;
  box.width  = 0.2;
  box.height = 0.2;
  box.mass   = 1.0;
  box.body   = dBodyCreate(world);
  dMassSetZero(&m1);
  dMassSetBoxTotal(&m1,box.mass,box.length,box.width,box.height);
  dBodySetMass(box.body,&m1);
  dBodySetPosition(box.body, x0, y0, sensor.height + 0.5 * box.height + z0);
  box.geom = dCreateBox(space,box.length,box.width,box.height);
  dGeomSetBody(box.geom,box.body);

  // 固定ジョイント
  fixed = dJointCreateFixed(world,0);
  dJointAttach(fixed,box.body,sensor.body);
  dJointSetFixed(fixed);    // 力とトルク情報を取得する関節を指定
  dJointSetFeedback(fixed,feedback);
  dsSimulationLoop(argc,argv,352,288,&fn);
  dWorldDestroy(world);
  dCloseODE();

  return 0;
}

初めて質問します。

直方体が落下して、床と接触した場合の
床からの反力にどのような値が得られて
いるのかを知りたいのですが、どのよう
に取得すればいいのでしょうか?

  • この間は質問に答えていただいてありがとうございました。

    もうひとつ質問なんですが・・・(ジョイントに関する力とトルクについて)

    ロボットアームの場合、ジョイントに生じる力の検出はたとえば物がぶつかった衝撃でジョイントが動いてなくてもトルクと力の検出はできるのでしょうか?

    何度も質問をしてしまって申し訳ありませんが、よろしければ返答をお願いします。

  • 初書き込みです

    質問なんですが、ロボットアームの各関節に生じるトルクを取得したいのですが、サンプル13にサンプル11のプログラムを組み込んであげればいいんでしょうか?初心者なのでバカな質問かもしれませんが、よろしければ教えてください

  • 先日の固定ジョイントに関する回答ありがとうございました。
    力不足ながら、今回も質問したいのですが
    トルク制御において、
    dJointAddHingeTorque();
    を用いて制御しようとしたのですが、動きません。
    他者の質問を参考にすると、
    dJointSetHingeParam(dJointID, dParamVel,u);
    dJointSetHingeParam(dJointID, dParamFMax, fmax);
    を削除しないといけないらしいのですが、
    これらも用いる必要があって、どうしようか悩んでいるところです。
    なにか、助言があれば教えてください。

  • コメントを残す

    メールアドレスが公開されることはありません。