LIFE LOG(ここにはあなたのブログ名)

あつあつ備忘録

ソフトやハード、時にはメカの備忘録をまとめていきます

【ROS】urdfでparentとchildの関係性を調べる

wiki.ros.org こちらを見ながら進めていった。

myfirst.urdf

<?xml version="1.0"?>
<robot name="myfirst">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>
</robot>
$ roslaunch urdf_tutorial display.launch model:=myfirst.urdf 

f:id:AtsuyaKoike:20190626191721p:plain:w400

lengthはz軸の長さ、radiusは半径でy軸。

03-origins.urdf

<?xml version="1.0"?>
<robot name="origins">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
    </visual>
  </link>

  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
    <origin xyz="0 -0.22 0.25"/>
  </joint>

</robot>
$ roslaunch urdf_tutorial display.launch model:=03-origins.urdf 

f:id:AtsuyaKoike:20190626192015p:plain:w400

このurdfファイルでこの位置に持っていくことができるみたいだが、rpy、xyzを1つ1つ見ていかないと分からなくなりそうなので、試した結果を載せていく。 いじる部分は

right_legの

<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>

と base_to_right_legの

<origin xyz="0 -0.22 0.25"/>

の部分

パターン1

right_leg:origin rpy="0 0 0" xyz="0 0 0"
base_to_right_leg:origin xyz="0 0 0"

f:id:AtsuyaKoike:20190626195128p:plain:w400

パターン2

right_leg:origin rpy="0 1.57075 0" xyz="0 0 0"
base_to_right_leg:origin xyz="0 0 0"

f:id:AtsuyaKoike:20190626195302p:plain:w400

パターン3

right_leg:origin rpy="0 1.57075 0" xyz="0 -0.3 0"
base_to_right_leg:origin xyz="0 0 0"

f:id:AtsuyaKoike:20190626195433p:plain:w400

パターン4

right_leg:origin rpy="0 1.57075 0" xyz="0 -0.3 0"
base_to_right_leg:origin xyz="0 0.22 0"

f:id:AtsuyaKoike:20190626195548p:plain:w400

パターン5

right_leg:origin rpy="0 1.57075 0" xyz="0 -0.3 0"
base_to_right_leg:origin xyz="0 0.22 0.25"

f:id:AtsuyaKoike:20190626195640p:plain:w400

これで、rpy、xyzを弄るときに参考になるはず。

棒を両サイドに出現させる

04-materials.urdf

<?xml version="1.0"?>
<robot name="materials">

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>

  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>


  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue"/>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
    <origin xyz="0 -0.22 0.25"/>
  </joint>

  <link name="left_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="base_to_left_leg" type="fixed">
    <parent link="base_link"/>
    <child link="left_leg"/>
    <origin xyz="0 0.22 0.25"/>
  </joint>

</robot>

f:id:AtsuyaKoike:20190626200906p:plain

簡単ですね。

【C++】vectorでpairを使うメモ

f:id:AtsuyaKoike:20190524095847p:plain:w300

最初に

vectorではpairが用いられることが多いようなので、C++vectorとpairを使った動作を備忘録として残します。

vector_pair.cpp

#include <iostream>
#include <vector>

int main() {
    std::vector<int> vec;
    vec.push_back(2);    
    vec.push_back(7);    
    vec.push_back(0);    
    vec.push_back(3);    
    vec.push_back(5);    
    vec.push_back(1);    
    vec.push_back(4);    
    vec.push_back(6);    
    vec.push_back(9);    
    vec.push_back(8);    

    typedef std::pair<int, int> pair;
    std::vector<pair> pair_vec;
    
    for ( int i = 0; i < vec.size(); ++i )
        pair_vec.push_back( pair( i, vec[i] ) );
        
    for ( auto& i: pair_vec )
        std::cout << i.first << " " << i.second << std::endl;

    return 0;

}

実行結果

$ g++ -o vector_pair vector_pair.cpp -std=c++11
$ ./vector_pair
0 2
1 7
2 0
3 3
4 5
5 1
6 4
7 6
8 9
9 8

解説

先に書かせていただくと、using namespace std;を使えば楽にかけますが、勉強のため。

  • 5行目:int型のベクトルの型を作成。
  • 6から15行目はvecに適当な縦10で横1の行列を作成。
  • 17行目では、pair<int, int>で2つ数字をセットにできる型を作成。
  • 18行目では、横の要素数が2つのpair_vecの行列の型を作成。
  • 23行目は範囲指定のfor文。C++11の機能を使っている。
  • 24行目は出力。pairの場合、ひとつ目がfirst, 2つめがsecondという扱い

まとめ

  • おそらく、pairを使わない場合はvector<vector>など面倒くさい書き方になるのを考えると、pairを使うと簡潔なプログラムになる
  • コンパイルする際にC++11の機能を使うオプションをつけないといけないため、現在の書き方を知る必要はあるかもです。(調べてもでてこないので、コンパイルオプションをつけるのは一般的?

以上です。

【C++】コピーコンストラクタ

f:id:AtsuyaKoike:20190524095847p:plain:w300

これまでの方法だと

  • オブジェクトを関数に渡す
  • 関数からオブジェクトを返す

ときに問題が発生した。

具体的には

  • オブジェクトを関数に渡すと、そのオブジェクトのコピーが作成され、オブジェクトを受け付ける関数の引数として使用される
    • 割り当てられたメモリのポインタを含むオブジェクトがある場合、コピーオブジェクトは元のオブジェクトと同じメモリを指す
      • コピーオブジェクトでこのメモリに変更を加えると、元のオブジェクトに影響
    • 関数の終了時に、コピーオブジェクトが破棄されデストラクタが呼び出されることで影響

  • 関数からオブジェクトを返す場合
    • 関数から返す値を保持する一時オブジェクトを作成する
      • 呼び出し元のルーチンに返した後は、スコープを外れデストラクタが呼び出される
      • これが動的メモリの場合に問題が生じる

  • コピーコンストラクタは
    • 1つのオブジェクトを使用して他のオブジェクトを初期化する方法を指定できる
    • コピーコンストラクタは、そのオブジェクトを使って他のオブジェクトを初期化するときに呼出される

study152.cpp

#include <iostream>
#include <cstdlib>
using namespace std;

class array {
    int *p;
    int size;
public:
    array( int sz ) {
        p = new int[sz];
        if( !p ) {
            exit(1);
        }
        size = sz;
        cout << "通常のコンストラクタを使う\n";
    }
    ~array() {
        delete [] p;
    }
    
    array( const array &a );

    void put( int i, int j ) {
        if ( i >= 0 && i < size ) {
            p[i] = j;
        }
    }
    int get( int i ) {
        return p[i];
    }
};

array::array( const array &a ) {
    size = a.size;
    p = new int[a.size];
    if( !p ) {
        exit(1);
    }
    for( int i = 0; i < a.size; i++ ) {
        p[i] = a.p[i];
    }
    cout << "コピーコンストラクタを使用\n";
}

int main() {
    array num(10);
    
    for ( int i = 0; i < 10; ++i ) {
        num.put( i, i );
    }

    for ( int i = 0; i < 10; ++i ) {
        cout << num.get(i);
    }
    cout << "\n";

    array x = num;

    for ( int i = 0; i < 10; ++i ) {
        cout << x.get(i);
    }
    cout << "\n";

    return 0;
}
 ./study152 
通常のコンストラクタを使う
0123456789
コピーコンストラクタを使用
0123456789

study156.cpp

#include <iostream>
#include <cstring>
#include <cstdlib>
using namespace std;

class strtype {
    char *p;
public:
    strtype( char *s );
    strtype( const strtype &o );
    ~strtype() {
        delete [] p;
    }
    char *get() {
        return p;
    }
};

strtype::strtype( char *s ) {
    int l;
    l = strlen(s) + 1;

    p = new char [l];
    if ( !p ) {
        cout << "メモリ割り当てエラー\n";
        exit(1);
    }
    strcpy( p, s );
}

strtype::strtype( const strtype &o ) {
    int l;
    l = strlen(o.p) + 1;

    p = new char [l];
    if ( !p ) {
        cout << "メモリ割り当てエラー\n";
        exit(1);
    }
    strcpy( p, o.p );
}

void show( strtype x ) {
    char *s;

    s = x.get();
    cout << s << "\n";
}

int main() {
    strtype a("Hello"), b("There");
    
    show(a);
    show(b);

    return 0;
}
  • 関数へオブジェクトを引き渡すときにコピーが作成されるが、その設定をコピーコンストラクタが行う
  • コピー作成時にコピー用のメモリを割り当てる
    • show()終了時にスコープから外れ、x.pが指すメモリが開放されるが、これは関数に渡した元のオブジェクトのメモリとは異なる。




引用・参考はこちら

f:id:AtsuyaKoike:20190524100759j:plain:w200
独習C++ 第4版 https://www.amazon.co.jp/dp/4798119768

【C++】コンストラクタ関数のオーバーロード

f:id:AtsuyaKoike:20190524095847p:plain:w300

コンストラクタ関数もオーバーロードできる

コンストラクタをオーバーロードするメリット

  1. 柔軟性を得る
  2. 配列のサポート
  3. コピーコンストラクタを作成すること

study145.cpp

#include <iostream>
using namespace std;

class myclass {
    int x;
public:
    myclass() {
        x = 0;
    }
    myclass( int n ) {
        x = n;
    }
    int getx() {
        return x;
    }
};

int main() {
    myclass o1(10);
    myclass o2;

    cout << "o1: " << o1.getx() << "\n";
    cout << "o2: " << o2.getx() << "\n";

    return 0;
}
$ ./study145 
o1: 10
o2: 0



stdudy149.cpp

#include <iostream>
using namespace std;

class myclass {
    int x;
public:
    myclass() {
        x = 0;
    }
    myclass( int n ) {
        x = n;
    }
    int getx() {
        return x;
    }
    void setx( int n ) {
        x = n;
    }
};

int main() {
    myclass *p;
    myclass ob(10);

    p = new myclass[10];
    if( !p ) {
        cout << "メモリ割り当てエラー\n";
        return 1;
    }
    
    for ( int i = 0; i < 10; ++i ) {
        p[i] = ob;
    }
    for ( int i = 0; i < 10; ++i ) {
        cout << "p[" << i << "]: " << p[i].getx();
        cout << "\n";
    }
    return 0;
}
$ ./study149 
p[0]: 10
p[1]: 10
p[2]: 10
p[3]: 10
p[4]: 10
p[5]: 10
p[6]: 10
p[7]: 10
p[8]: 10
p[9]: 10
  • 動的配列の初期化に使用することができる




引用・参考はこちら

f:id:AtsuyaKoike:20190524100759j:plain:w200
独習C++ 第4版 https://www.amazon.co.jp/dp/4798119768

【C++】参照の返し

f:id:AtsuyaKoike:20190524095847p:plain:w300

stdudy135.cpp

#include <iostream>
using namespace std;

int &f();
int x;

int main() {
    f() = 100;
    cout << x << "\n";
    return 0;
}

int &f() {
    return x;
}
$ ./study135 
100
  • f() = 100;はf()によって返される参照に100を代入している
  • return x;はxへの参照(xのアドレス)を返している。
  • 要するに、戻り地に100を代入している。

study137.cpp

#include <iostream>
#include <cstdlib>
using namespace std;

class array {
    int size;
    char *p;
public:
    array( int num );
    ~array() {
        delete [] p;
    }
    char &put( int i );
    char get( int i );
};

array::array( int num ) {
    p = new char [num];
    if( !p ) {
        cout << "メモリ割り当てエラー\n";
        exit(1);
    }
    size = num;
}

char &array::put( int i ) {
    if( i < 0 || i > size-1 ) {
        cout << "境界エラー\n";
        exit(1);
    }
    return p[i];
}

char array::get( int i ) {
    if( i < 0 || i > size-1 ) {
        cout << "境界エラー\n";
        exit(1);
    }
    return p[i];
}

int main() {
    array a(10);

    a.put(3) = 'X';
    a.put(2) = 'R';

    cout << a.get(3) << a.get(2);
    cout << "\n";

    a.put(11) = '!';

    return 0;
}
$ ./study137 
XR
境界エラー
  • array a(10);でa(0)からa(9)まで1文字ずつ代入できるオブジェクトを作成
  • a.put(3) = 'X';で、put()の返り値のp[i]に文字Xを代入。a.put(4)はR.
  • a.get(3)の返り値はp[i]なので、get()の引数に指定した数字のところの文字を取り出せる
  • if( i < 0 || i > size-1 ) で指定した場所が範囲内にあるか確かめることができる。(代入するときも出力するときも)
    • C/C++は配列のオーバーフローなどをチェックできないので、このプログラムでチェックできる。




引用・参考はこちら

f:id:AtsuyaKoike:20190524100759j:plain:w200
独習C++ 第4版 https://www.amazon.co.jp/dp/4798119768

【C++】オブジェクト参照の引き渡し

f:id:AtsuyaKoike:20190524095847p:plain:w300

atsuyakoike.hatenablog.com

  • こちらでは、オブジェクトを関数に渡した場合は、オブジェクトのコピーが作成されることを確認している。
  • コピーなのでコンストラクタは呼ばれないがデストラクタは呼ばれる
    • 動的メモリを2度以上開放することになり問題が発生することがある。


  • オブジェクト参照によって解決できる
    • オブジェクトを参照するとコピーが作成されないためデストラクタは実行されない
    • ただし、関数内の変更が、引数に指定したオブジェクトにも反映される。

study132.cpp

#include <iostream>
using namespace std;

class myclass {
    int who;
public:
    myclass( int n ) {
        who = n;
        cout << "コンストラクタ呼び出し" << who << "\n";
    }
    ~myclass() {
        cout << "デストラクタ呼び出し" << who << "\n";
    }
    int id() {
        return who;
    }
};

void f( myclass &o ) {
    cout << "受け取り" << o.id() << "\n";
}

int main() {
    myclass x(1);
    f(x);
    return 0;
}
$ ./study132 
コンストラクタ呼び出し1
受け取り1
デストラクタ呼び出し1
デストラクタ呼び出し1

こちらは以前と同様。デストラクタが2回呼びだされている。



study133.cpp

#include <iostream>
using namespace std;

class myclass {
    int who;
public:
    myclass( int n ) {
        who = n;
        cout << "コンストラクタ呼び出し" << who << "\n";
    }
    ~myclass() {
        cout << "デストラクタ呼び出し" << who << "\n";
    }
    int id() {
        return who;
    }
};

void f( myclass &o ) {
    cout << "受け取り" << o.id() << "\n";
}

int main() {
    myclass x(1);
    f(x);
    return 0;
}
$ ./study133 
コンストラクタ呼び出し1
受け取り1
デストラクタ呼び出し1

19行目のみ変更している。




引用・参考はこちら

f:id:AtsuyaKoike:20190524100759j:plain:w200
独習C++ 第4版 https://www.amazon.co.jp/dp/4798119768

【C++】参照

f:id:AtsuyaKoike:20190524095847p:plain:w300

参照(reference)

  • すべての面で変数の別名として動作する暗黙的なポインタ(implict pointer)

用途

  1. 参照を関数に渡すことができる
  2. 関数から参照を返すことができる
  3. 独立した参照を作成することができる

study127_1.cpp

#include <iostream>
using namespace std;

void f( int *n );

int main() {
    int i = 0;

    f(&i);

    cout << "iの新しい値:" << i << "\n";

    return 0;
}

void f( int *n ) {
    *n = 100;
}

Cの場合。 これは、f()がiのアドレスを引き渡していて、そのアドレスのところに100を代入しているので、iの値が変わる。



study127_2.cpp

#include <iostream>
using namespace std;

void f( int &n );

int main() {
    int i = 0;
    f(i);
    
    cout << "iの新しい値:" << i << "\n";

    return 0;
}

void f( int &n ) {
    n = 100;
}

C++ではこのように完結に書くことができる。

study129.cpp

#include <iostream>
using namespace std;

void swapargs( int &x, int &y );

int main() {
    int i, j;

    i = 10;
    j = 19;

    cout << "i:" << i << "、";
    cout << "j:" << j << "\n";

    swapargs( i, j );

    cout << "交換後:";
    cout << "i:" << i << "、";
    cout << "j:" << j << "\n";

    return 0;
}

// 参照の場合
void swapargs( int &x, int &y ) {
    int t;

    t = x;
    x = y;
    y = t;
}

// ポインタの場合
/*void swapargs( int *x, int *y ) {
    int t;

    t = *x;
    *x = *y;
    *y = t;
}*/




引用・参考はこちら

f:id:AtsuyaKoike:20190524100759j:plain:w200
独習C++ 第4版 https://www.amazon.co.jp/dp/4798119768