Unityの衝突検出をhakoniwa-px4sim側で受信して、物理側に反映させてみる about hakoniwa-px4sim

[kenjihiranabe]

ちょっと話がややこしそうなので，また話しましょう．
ぼくの想定は，中心がドローンと一致する大きな球（ドローンを包む）が１つあるイメージでやりました．
でも，基本的にはよさそうな動きですね．

[tmori]

Unityの障害物検出 PDUをhakoniwa-px4sim側で読み込みできることを確認する

Unity側

hakoniwa-px4sim側

Collision: Yes
Contact Number: 1
Relative Velocity: (-0.000508367, -0.000310906,-0.00583442)
Contact Positions[0]: (-0.0118168, -0.752347,-0.00324726)
Restitution Coefficient: 0.2

結果

Unity側の送信データをhakoniwa-px4sim側で受信できていることを確認した。
実験していて気づいたこと。

Unityのシミュレーションは20msec周期で動作する。
一方、こちらは 3msec周期で動作するので、衝突データを打ち消しておかないと、次のタイミングで拾ってしまう。

よって、衝突検出したら、collisionフラグをfalseにして書き込みするようにした。

[tmori]

障害物検出時に、直接、速度方向を変更して跳ね返り現象が観測できるか確認する

Y軸方向に直進させて、以下のコードで跳ね返りさせてみた。

        //collision detection
        if (input.collision.collision) {
            std::cout << "collision velocity.x: " << this->velocity.data.x << std::endl;
            std::cout << "collision velocity.y: " << this->velocity.data.y << std::endl;
            std::cout << "collision velocity.z: " << this->velocity.data.z << std::endl;
            this->velocity.data.y = -this->velocity.data.y;
            this->velocityBodyFrame = drone_physics::velocity_ground_to_body(this->velocity, angle);
        }

実験結果

跳ね返った！

2024-01-08.9.00.09.mov

ログ：

Collision: Yes
Contact Number: 1
Relative Velocity: (0.00203095, -0.0108366,0.00282158)
Contact Positions[0]: (0.00242919, 0.684624,0.00636578)
Restitution Coefficient: 0.2
collision velocity.x: -0.0241458
collision velocity.y: 3.42605
collision velocity.z: -0.062102

その他

Unityのコライダーを中心のところに小さい球を置いているだけなので、見た目、めり込んでしまうのがやっぱり気になる。。

最後の手直しの時に直そう。

[tmori]

velocity_after_contact_with_wall()関数を利用して、跳ね返りの速度計算して自然な動きになるか確認する

やってみた。
Restitution Coefficient が小さいためか、ちょっとバックして、もう一度ぶつかりました。

Collision: Yes
Contact Number: 1
Relative Velocity: (-0.000298463, 0.000389523,0.00584623)
Contact Positions[0]: (-0.00137322, 0.754776,0.00481129)
Restitution Coefficient: 0.2
velocity_before_contact.x: 0.0144767
velocity_before_contact.y: 3.39903
velocity_before_contact.z: -0.0547862
velocity_after_contact.x: 0.0053686
velocity_after_contact.y: -0.679597
velocity_after_contact.z: -0.0196227
Collision: Yes
Contact Number: 1
Relative Velocity: (-0.000158643, 0.000381106,0.00570068)
Contact Positions[0]: (-0.00299864, 0.820429,-0.0272608)
Restitution Coefficient: 0.2
velocity_before_contact.x: 0.0310429
velocity_before_contact.y: 1.47143
velocity_before_contact.z: 0.229295
velocity_after_contact.x: 0.0083461
velocity_after_contact.y: -0.290539
velocity_after_contact.z: 0.0652913
Collision: Yes
Contact Number: 1
Relative Velocity: (0.00577355, -0.16758,0.0641817)
Contact Positions[0]: (0.00019826, 1.09905,-0.0517273)
Restitution Coefficient: 0.2
velocity_before_contact.x: 0.0090139
velocity_before_contact.y: -0.0319912
velocity_before_contact.z: 0.661885
velocity_after_contact.x: 0.00180733
velocity_after_contact.y: 0.0188025
velocity_after_contact.z: 0.131191

[tmori]

良い感じなので、正式実装に移ります。

[kenjihiranabe]

お！できたの？

[tmori]

お！できたの？

はい、できました！

地面との衝突の方はうまくできてないですが、真正面からぶつかって、ちょっと下がるのはうまく行きました。

[tmori]

こんな感じで動きました。

2024-01-08.10.06.54.mov

[kenjihiranabe]

壁側が動かない(質量無限大)想定で作ったので、動きが思ってたのとちがうなあ。ドローン側は速度リセットしてる？それとも力でやってる？
弾性係数100%だとほぼ逆行の速度を出力する想定です。

[tmori]

壁側が動かない(質量無限大)想定で作ったので、動きが思ってたのとちがうなあ。ドローン側は速度リセットしてる？それとも力でやってる？ 弾性係数100%だとほぼ逆行の速度を出力する想定です。

自分も違和感あったので、色々見直し中です。

1. Unityのコライダーが四角形なので、衝突位置が真っ正面にならない問題があるので、球形に変更中。
2. 弾性係数を１００％にしてみる

ちなみに、修正コードはこちら。

     DroneAccelerationBodyFrame acc = drone_physics::acceleration_in_body_frame(
                                                            this->velocityBodyFrame, this->angle, 
                                                            this->angularVelocityBodyFrame,
                                                            thrust.data, this->param_mass, GRAVITY, this->param_drag);
        DroneAngularAccelerationBodyFrame acc_angular = drone_physics::angular_acceleration_in_body_frame(
                                                            this->angularVelocityBodyFrame,
                                                            torque.data.x, torque.data.y, torque.data.z,
                                                            this->param_cx, this->param_cy, this->param_cz);
        //integral to velocity on body frame
        this->velocityBodyFrame.data = integral(this->velocityBodyFrame.data, acc.data);
        this->angularVelocityBodyFrame.data = integral(this->angularVelocityBodyFrame.data, acc_angular.data);

        //convert to ground frame
        this->velocity = this->convert(this->velocityBodyFrame);
        this->angularVelocity = this->convert(this->angularVelocityBodyFrame);

        //collision detection
        if (input.collision.collision) {
            hako::drone_physics::VectorType velocity_before_contact = this->velocity;
            hako::drone_physics::VectorType center_position = this->position;
            hako::drone_physics::VectorType contact_position = { 
                this->position.data.x + input.collision.contact_position[0].x * param_size_x,
                this->position.data.y + input.collision.contact_position[0].y * param_size_y,
                this->position.data.z + input.collision.contact_position[0].z * param_size_z
            };
            double restitution_coefficient = input.collision.restitution_coefficient;
            //std::cout << "velocity_before_contact.x: " << velocity_before_contact.x << std::endl;
            //std::cout << "velocity_before_contact.y: " << velocity_before_contact.y << std::endl;
            //std::cout << "velocity_before_contact.z: " << velocity_before_contact.z << std::endl;
            hako::drone_physics::VectorType col_vel = hako::drone_physics::velocity_after_contact_with_wall(
                    velocity_before_contact, center_position, contact_position, restitution_coefficient);
            //std::cout << "velocity_after_contact.x: " << col_vel.x << std::endl;
            //std::cout << "velocity_after_contact.y: " << col_vel.y << std::endl;
            //std::cout << "velocity_after_contact.z: " << col_vel.z << std::endl;
            this->velocity = col_vel;
            this->velocityBodyFrame = drone_physics::velocity_ground_to_body(this->velocity, angle);
        }

[tmori]

再実験結果

球形のコライダーを全面に配置。
弾性係数を１００％に変更。

2024-01-08.10.42.07.mov

[tmori]

Unityのコライダーの配置を変えた

toppers/hakoniwa-unity-drone-model#8

[tmori]

色々ぶつけてみた

2024-01-08.11.16.33.mov

[kenjihiranabe]

壁側は Unity の物理が計算してくれているの？

[tmori]

壁側は Unity の物理が計算してくれているの？

はい、unityが勝手にやってくれてます

[kenjihiranabe]

不思議．．．．本格的に**量保存で計算するとすると，相手側の質量が必要なはずなのに，Unity で相手側の質量わかるのかな？壁の倒れるの見ると，トルク（角**量変化）も計算しているみたいですね．こちらも Unity で計算させる手もありますね（同じ Unity 内の物理ポリシーで動かした方が違和感がでない）．

[tmori]

情報の整理

Unityの衝突情報を整理します。

Unityドローンのコライダー配置とサイズ

Unity側のサイズとhakoniwa-px4sim側の機体のサイズを合わせました。

コライダー設計

上図の球形コライダー（衝突検出オブジェクト）で、他オブジェクトとの接触検出をします。

ちなみに、１個の球形コライダーにすると全体を覆う半径が0.13mになり、地上と接触してしまうので採用できませんでした。
そのため、４個のコライダーを４個のプロペラ上に配置して、ちょっと浮かせています。浮かせないと、地上と接触してしまうため。
（元々、機体の高さは0.02mなので、球形コライダーの半径が0.05mなので、0.03m浮かせています。）

Unityのコライダーによる衝突検出情報

Unityの OnCollisionEnter()を使うと、コライダーが接触検出すると、接触位置を地上座標系での絶対位置が取得できます。
その値をそのまま、hakoniwa-px4sim側に返します。

この値をベースに、velocity_after_contact_with_wall()を使って、速度の計算をします(col_vel)。

hako::drone_physics::VectorType velocity_before_contact = this->velocity;
hako::drone_physics::VectorType center_position = this->position;
hako::drone_physics::VectorType contact_position = { 
                    input.collision.contact_position[0].x,
                    input.collision.contact_position[0].y,
                    input.collision.contact_position[0].z
};
double restitution_coefficient = input.collision.restitution_coefficient;
hako::drone_physics::VectorType col_vel = hako::drone_physics::velocity_after_contact_with_wall(
                    velocity_before_contact, center_position, contact_position, restitution_coefficient);

計算した値を、そのまま機体の速度に設定します。

this->velocity = col_vel;
this->velocityBodyFrame = drone_physics::velocity_ground_to_body(this->velocity, angle);

あとは、通常の積分をします。

[tmori]

対応できたので、クローズします。