C++とPython間の通信構造体
C++とPython間で同じ構造体を用いてTCP通信をしたかったのでまとめ(ROS最高!!!)
環境
macOS 11.6
構造体定義
#pragma pack(push, 1) struct TcpHeader { uint32_t size; uint8_t type; TcpHeader(uint32_t size = 0, uint8_t type = 0) : size(size), type(type) {} }; struct PacketControlDataReq { TcpHeader header; uint8_t arm_id; ControlData control_data; PacketControlDataReq() : header(sizeof(PacketControlDataReq), MAINTE_TO_ROBOT_CONTROL_DATA) {} }; struct PacketRobotInfoReq { TcpHeader header; uint8_t num_arm; uint8_t num_joint; PacketRobotInfoReq() : header(sizeof(PacketRobotInfoReq), ROBOT_TO_MAINTE_ROBOT_INFO), num_arm(ArmId::NUM_ARM), num_joint(NUM_JOINT) {} }; #pragma pack(pop)
class TcpHeader(Structure): _pack_ = 1 _fields_ = [ ('size', c_uint32), ('type', c_uint8) ] def __init__(self, size = 0, type = 0): self.size = size self.type = type class PacketControlDataReq(Structure): _pack_ = 1 _fields_ = [ ('header', TcpHeader), ('arm_id', c_uint8), ('control_data', ControlData) ] def __init__(self): self.header = TcpHeader(sizeof(PacketControlDataReq), PacketType.MAINTE_TO_ROBOT_CONTROL_DATA) class PacketArmInfoReq(Structure): _pack_ = 1 _fields_ = [ ('header', TcpHeader), ('num_arm', c_uint8), ('num_joint', c_uint8) ] def __init__(self): self.header = TcpHeader(sizeof(PacketArmInfoReq), PacketType.ROBOT_TO_MAINTE_ARM_INFO) self.num_arm = 0 self.num_joint = 0
C++ >> Python
送信側(C++)
PacketRobotInfoReq robot_info_req; int err = send(sock, &robot_info_req, sizeof(PacketRobotInfoReq), 0);
受信側(python)
arm_info = PacketArmInfoReq() memmove(addressof(arm_info), data, sizeof(arm_info)) print("num_arm = {}, num_joint = {}".format(arm_info.num_arm, arm_info.num_joint))
Python >> C++
送信側(python)
control_data = PacketControlDataReq() client.send(control_data)
受信側(C++)
std::array<char, 2048> rx_buffer; int len = recv(sock, rx_buffer.data(), rx_buffer.size(), 0); PacketControlDataReq control_data_req; memmove(&control_data_req, rx_buffer.data(), sizeof(control_data_req)); printf("arm_id = %d", control_data_req.arm_id);
外乱抑制について備忘録
3Dプリンタが使えないのでpython-controlで遊んでみました
Python Control Systems Library — Python Control Systems Library dev documentation
FF制御のみ,PIDによるFB制御のみ,FFとPIDを組み合わせた制御(FF+FB),外乱オブザーバーを追加した制御(FF+FB+EX)を比較
外乱オブザーバーを追加した制御が収束が早く,外乱を抑制できている(ゲインは統一)
ソース
robot-ws/system.py at master · Libra23/robot-ws · GitHub
環境
macOS Big Sur : 11.4
Python : 3.9.5
- control==0.9.0
- matplotlib==3.4.2
- numpy==1.20.3
- PyYAML==5.4.1
- scipy==1.6.3
外乱オブザーバーフィードバックによる外乱抑制
外乱オブザーバーとは
簡単に言うとプラントの出力から入力を推定して,実際の入力との差から外乱を推定する
外乱オブザーバの紹介 - Qiita
ブロック線図
フィードフォワード制御とPID制御に外乱オブザーバー補償を追加したコントローラでストップ応答を確認する
ブロック線図は下図
パラメータ
- R : 入力
- Y : 出力
- D : 外乱
- D_est : 推定外乱
- FF : フィードフォワードコントローラ
- PID : PIDコントローラ
- T : FFとPIDの出力
- P : プラント
- P_n^-1 : 逆プラントモデル
- Q :フィルタ
- F : フィルタ
自重補償バネを設計する
週一ブログチャレンジはやっていませんが,先週に引き続きロボットの脚について
この脚の膝部分にはばねが入っていて,自重補償に良さそうなばねを選定する(摩擦が支配的な気もするけど
ばねのポテンシャルエネルギを追加
ラグランジュの運動方程式
ボディ全体の質量をmとして,脚が設置した状態で加速度a = 0と仮定してボディを上下させる
の場合
横軸が関節角度],縦軸が必要トルク](真ん中のニョロニョロは移動開始時のスムージングの跡なので無視)
]の場合(真ん中のニョロニョロは,,,
必要なトルクを1/6程度まで減らすことができる
今回はばね定数1.8 [N mm/deg.]のねじりスプリングをモノタロウさんで購入
線径 1Φmm、ばね定数(N・mm[deg]) 1.8、33-0834、1袋(2個) - ねじりスプリング 1袋(2個) サミニ 【通販モノタロウ】 07500115
歩行する場合は常に設置しているわけではないので,脚の本数と歩容から妥当な値を設定するのが良さそう
ESP32の開発環境を整える
特異点低感度行列を用いた逆運動学 アルゴリズム編
運動学周りを一度ちゃんとまとめよう
Arduino(ESP32 only)と標準的なC++環境で併用できるKinematicライブラリを作っておきましたー
github.com
ESP32のコンパイルオプションで別れているので,コピペでPC環境でも使える
ESP32だと下記の線形代数ライブラリが必要
GitHub - tomstewart89/BasicLinearAlgebra: A library for using matrices and linear algebra on Arduino
Standard C++だとEigenが必要+Kinematicライブラリ内のAffine3d.cpp & Affine3d.hppを削除(アフィン変換)
続きを読む