分かりやすく解説いただきありがとうございます。
自分なりに分かる範囲で辿って考えてみたのですが、
以下のようになり、やはりよく分からなくなってしまいました。
特にoutputLinkFlags[index] = true;に関して、
何かご存知でしたらご教授いただけると幸いです。
CrawlerQL_double_Sugimo4.cppでは以下のようにsetJointOutput(int stateTypes)の引数に
シンボルとしてJOINT_TORQUEを指定しております。
ioBody = io->body();
io->setJointInput(JOINT_ANGLE);
io->setJointOutput(JOINT_TORQUE);
io->setLinkInput(ioBody->rootLink(), LINK_POSITION);
for (int i=0;i<N_CTRL;i++){
io->setLinkInput(ioBody->link(JIDS[i]), LINK_POSITION);
}
io->setJointInput(JOINT_ACCELERATION|JOINT_VELOCITY|JOINT_ANGLE|LINK_POSITION);
io->setLinkOutput(ioBody->joint(CRL_LH_ID), JOINT_VELOCITY);
io->setLinkOutput(ioBody->joint(CRL_RH_ID), JOINT_VELOCITY);
setJointOutputは関節の指定なしにすべての全ての関節に対して一括で出力する関数だと存じます。
ioBody->numJoints()を取得して、次のようにsetLinkOutput(ioBody->joint(i), stateTypes)の引数にしております。
iは0からioBody->numJoints()までの値になるはずなのでマイナス値が入ることはないかと存じます。
void SimpleControllerItemImpl::setJointOutput(int stateTypes)
{
const int nj = ioBody->numJoints();
for(int i=0; i < nj; ++i){
setLinkOutput(ioBody->joint(i), stateTypes);
}
}
また、setLinkOutputは次のようにenableOutput(link)の引数として
与えられたioBody->joint(i)をそのまま入れております。
void SimpleControllerItemImpl::setLinkOutput(Link* link, int stateTypes)
{
Link::ActuationMode mode = Link::NO_ACTUATION;
if(stateTypes & SimpleControllerIO::LINK_POSITION){
mode = Link::LINK_POSITION;
} else if(stateTypes & SimpleControllerIO::JOINT_DISPLACEMENT){
mode = Link::JOINT_DISPLACEMENT;
} else if(stateTypes & SimpleControllerIO::JOINT_VELOCITY){
mode = Link::JOINT_VELOCITY;
} else if(stateTypes & SimpleControllerIO::JOINT_EFFORT){
mode = Link::JOINT_EFFORT;
}
if(mode != Link::NO_ACTUATION){
link->setActuationMode(mode);
enableOutput(link);
}
}
最後にenableOutputでindex = ioBody->joint(i)->index()となりますが、
これはおそらくiを参照しているのでしょうか。
そしてoutputLinkFlags[index] = true;として、ここでメモリアクセス違反とのことですが、
こちらの意味がよく分かりません。
void SimpleControllerItemImpl::enableOutput(Link* link)
{
int index = link->index();
if(static_cast(outputLinkFlags.size()) <= index){
outputLinkFlags.resize(index + 1, false);
}
outputLinkFlags[index] = true;
}
使ってるモデルファイルについては、14.04のPCからコピーしたファイル群(DoubleARM_v7.yamlとwrl)を使っており、
読み込みもできていて、リンクやジョイントも同様に表示されています。
そのため、問題ないかと思われるのですが、原因が分かりません。
ちなみにジョイントIDは以下の通りです。
//モデルのジョイントID
const int CRL_LH_ID = 0; //左クローラJoint ID
const int CRL_RH_ID = 1; //右クローラJoint ID
const int MSWG_ID = 2; //MFRAME中段旋回Joint ID
const int BLK_ID = 3; //BLOCKブームヨーJoint ID
const int BOM_ID = 4; //ブームJoint ID
const int ARM_ID = 5; //アームJoint ID
const int HAND_P_ID = 6; //バケットJoint ID
const int WTHUM_L1_ID = 7; //タフ左親指ベース1Joint ID
const int WTHUM_R1_ID = 8; //タフ右親指ベース2Joint ID
const int WFNG_L1_ID = 9; //タフ左指1Joint ID
const int WFNG_R1_ID = 10; //タフ右指2Joint ID
const int WTHUM_L2_ID = 11; //タフ左親指2Joint ID
const int WTHUM_L3_ID = 12; //タフ左親指3Joint ID
const int WTHUM_R2_ID = 13; //タフ右親指2Joint ID
const int WTHUM_R3_ID = 14; //タフ右親指3Joint ID
const int WFNG_L2_ID = 15; //タフ左指2Joint ID
const int WFNG_L3_ID = 16; //タフ左指3Joint ID
const int WFNG_R2_ID = 17; //タフ右指2Joint ID
const int WFNG_R3_ID = 18; //タフ右指3Joint ID
const int USWG_ID = 19; //UFRAME上段旋回Joint ID
const int MNPSWG_ID = 20; //マニピュレータSwing
const int MNPBOMC_ID = 36; //MブームシリンダJoint ID
const int MNPARMC_ID = 38; //MアームシリンダJoint ID
const int MNPELBC_ID = 40; //MエルボシリンダJoint ID
const int MNPBOM_ID = 21; //MブームJoint ID
const int MNPARM_ID = 22; //MアームJoint ID
const int MNPELB_ID = 23; //MエルボJoint ID
const int HAND_Y_ID = 24; //ハンドヨーJoint ID
const int HAND_R_ID = 25; //HANDBASEハンドロールJoint ID
const int FNGROD_ID = 26; //PUSHRODマニピュレータロッドJoint ID
const int HANDSLNK_I_ID=27;
const int HANDSLNK_O_ID=28;
const int MANIFNG_U_ID=29;
const int HANDTIP_U_ID=30;
const int HANDLNK_I_ID=31;
const int MANIFNG_L_ID=32;
const int HANDTIP_L_ID=33;
const int HANDLNK_O_ID=34;
以上がプログラムで定義しているジョイントIDで
以下は上記に加えてChoreonoid上で表示されるダミーの関節のようです。
41 IK_DUMMY_X0
42 IK_DUMMY_Y0
43 IK_DUMMY_X1
44 IK_DUMMY_Y1