プログラム（C#）で2リンクの逆運動学を解く
数年前に作ったC#で2リンクの逆運動学を解いたプログラムを載せようと思います。
(計算とか作ったときの記憶とかが曖昧なところがありますがそのまま載せます）
[pythonの方はこちらの記事を参考にしてください]
pythonで2リンク逆運動学を解いて表示する - ロボット、電子工作、IoT、AIなどの開発記録
pythonで3リンク逆運動学を解いて表示する - ロボット、電子工作、IoT、AIなどの開発記録
環境
win7 64bit
Visualstudio15
opencvsharp-x86
描画系でopencvを利用しています。
opencvの入れ方は、visualstudioでC#formアプリケーションでまずﾌﾟﾛｼﾞｪｸﾄを制作して、
ツール→Nugetﾊﾟｯｹｰｼﾞマネージャ→ソリューションのﾊﾟｯｹｰｼﾞの管理で検索窓にOpenCvSharp-x86を選択します。
（2017/08/21現在はOpenCVShapしかないようですね。。。）
cs本体は以下になります。
using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Linq; using System.Text; using System.Windows.Forms; using OpenCvSharp; using OpenCvSharp.Blob; namespace scalarobo1 { public partial class Form1 : Form { public static double deg1, deg2; public static double width = 640; public static double height = 480; int widthP2 = (int)width / 2; int heightP2 = (int)height / 2; static int oldX, oldY; const double radToDeg = 57.295779513082320876798154814105; public IplImage vis = new IplImage(new CvSize((int)width, (int)height), BitDepth.U8, 3); public Form1() { InitializeComponent(); init(); } private void Form1_Load(object sender, EventArgs e) { } private void init() { //軸描写 Cv.Zero(vis); Cv.Line(vis, new CvPoint(0, heightP2), new CvPoint((int)width, heightP2), new CvScalar(255, 255, 255), 1); Cv.Line(vis, new CvPoint(widthP2, 0), new CvPoint(widthP2, (int)height), new CvScalar(255, 255, 255), 1); //描画実行 pictureBoxIpl2.ImageIpl = vis; l1TextBox.Text = "80"; l2TextBox.Text = "80"; xTextbox.Text = "100"; ytextbox.Text = "80"; } //逆運動学計算 public void scala_calc2(double x, double y, double l1, double l2) { double tht1, tht2; double fai1, fai2, fai0, l3; double operationability; l3 = Math.Sqrt((x * x) + (y * y)); fai2 = Math.Acos(((l1 * l1) + (l2 * l2) - (l3 * l3)) / (2 * l1 * l2)); tht2 = Math.PI - fai2; fai1 = Math.Acos(((l1 * l1) + (l3 * l3) - (l2 * l2)) / (2 * l1 * l3)); fai0 = Math.Atan((y / x)); //tht1 = fai0 - fai1; //これでも良いがata2にする tht1 = Math.Atan2(y, x) - fai1; deg1 = tht1 * (180 / Math.PI); deg2 = tht2 * (180 / Math.PI); operationability = l1 * l2 * Math.Sin(tht2); view(l1, l2 , x, y, tht1, tht2); } public void view(double l1, double l2,double x,double y,double tht1,double tht2) { double fowerd_x0,fowerd_y0,fowerd_x1,fowerd_y1; double inv_x0, inv_y0; double late; Cv.Line(vis, new CvPoint(0, heightP2), new CvPoint((int)width, heightP2), new CvScalar(255, 255, 255), 1); Cv.Line(vis, new CvPoint(widthP2, 0), new CvPoint(widthP2, (int)height), new CvScalar(255, 255, 255), 1); //順運動学計算 fowerd_x0 = (l1 * Math.Cos(tht1)) + widthP2; fowerd_y0 = (l1 * Math.Sin(tht1)) + heightP2; fowerd_x1 = fowerd_x0 + (l2 * Math.Cos(tht1+tht2)); fowerd_y1 = fowerd_y0 + (l2 * Math.Sin(tht1+tht2)); //可操作性楕円対体 //% 関節速度の最大値 double wmax = 0.20;//%[rad/s] int ii = 101; double[] w1 = new double[ii]; double[] w2 = new double[ii]; double[] v_x = new double[ii]; double[] v_y = new double[ii]; //%リンクの長さ //L=1.0;%[m] //% 関節速度円の生成 for (int nn = 1; nn < ii; nn++) { w1[nn] = wmax * Math.Cos(2 * Math.PI / (ii - 1) * (nn - 1)); w2[nn] = wmax * Math.Sin(2 * Math.PI / (ii - 1) * (nn - 1)); } //ヤコビ 可操作性楕円体 double[,] J = new double[3, 3]; J[1, 1] = -l1 * Math.Sin(tht1) - l2 * Math.Sin(tht1 + tht2); J[1, 2] = -l2 * Math.Sin(tht1 + tht2); J[2, 1] = l1 * Math.Cos(tht1) + l2 * Math.Cos(tht1 + tht2); J[2, 2] = l2 * Math.Cos(tht1 + tht2); //可操作楕円体の生成 for (int nnn = 1; nnn < ii; nnn++) { v_x[nnn] = J[1, 1] * w1[nnn] + J[1, 2] * w2[nnn] + fowerd_x1; v_y[nnn] = J[2, 1] * w1[nnn] + J[2, 2] * w2[nnn] + fowerd_y1; if (nnn == 25 || nnn == 50 ||nnn == 100 ) Cv.Circle(vis, new CvPoint((int)v_x[nnn], (int)v_y[nnn]), 1, new CvScalar(0, 205, 255), 1, Cv.AA); else if(nnn == 75) Cv.Circle(vis, new CvPoint((int)v_x[nnn], (int)v_y[nnn]), 2, new CvScalar(0, 255, 5), 1, Cv.AA); else Cv.Circle(vis, new CvPoint((int)v_x[nnn], (int)v_y[nnn]), 1, new CvScalar(100, 105, 105), 1, Cv.AA); } Cv.Line(vis, new CvPoint(widthP2, heightP2), new CvPoint((int)fowerd_x0, (int)fowerd_y0), new CvScalar(0, 255, 0), 2); Cv.Line(vis, new CvPoint((int)fowerd_x0, (int)fowerd_y0), new CvPoint((int)fowerd_x1, (int)fowerd_y1), new CvScalar(0, 255, 255), 2); oldX = (int)fowerd_x1; oldY = (int)fowerd_y1; inv_x0 = fowerd_x0 + (l2 * Math.Cos(tht1 + tht2)); inv_y0 = fowerd_y0 + (l2 * Math.Sin(tht1 + tht2)); Cv.Circle(vis, new CvPoint((int)inv_x0, (int)inv_y0), 3, new CvScalar(250, 255, 255)); Cv.Circle(vis, new CvPoint((int)x+widthP2, (int)y+heightP2), 2, new CvScalar(0, 255, 255)); pictureBoxIpl2.ImageIpl = vis; tht1label.Text = Convert.ToString(0+(-tht1* radToDeg)); tht2label.Text = Convert.ToString(tht2 * radToDeg); } private void button2_Click(object sender, EventArgs e) { double l1,l2,x, y; l1 = Convert.ToDouble(l1TextBox.Text); l2 = Convert.ToDouble(l2TextBox.Text); x = Convert.ToDouble(xTextbox.Text); y = Convert.ToDouble(ytextbox.Text); Cv.Zero(vis); //稼動範囲 Cv.Circle(vis, new CvPoint(widthP2, heightP2), (int)(l1 + l2), new CvScalar(155, 55, 55), 1); scala_calc2(x, y*-1, l1, l2); pictureBoxIpl2.ImageIpl = vis; } } }
formデザイン文は以下になります。
namespace scalarobo1 { partial class Form1 { /// <summary> /// 必要なデザイナー変数です。 /// </summary> private System.ComponentModel.IContainer components = null; /// <summary> /// 使用中のリソースをすべてクリーンアップします。 /// </summary> /// <param name="disposing">マネージ リソースが破棄される場合 true、破棄されない場合は false です。</param> protected override void Dispose(bool disposing) { if (disposing && (components != null)) { components.Dispose(); } base.Dispose(disposing); } #region Windows フォーム デザイナーで生成されたコード /// <summary> /// デザイナー サポートに必要なメソッドです。このメソッドの内容を /// コード エディターで変更しないでください。 /// </summary> private void InitializeComponent() { this.xTextbox = new System.Windows.Forms.TextBox(); this.ytextbox = new System.Windows.Forms.TextBox(); this.label1 = new System.Windows.Forms.Label(); this.label2 = new System.Windows.Forms.Label(); this.pictureBoxIpl2 = new OpenCvSharp.UserInterface.PictureBoxIpl(); this.button2 = new System.Windows.Forms.Button(); this.label3 = new System.Windows.Forms.Label(); this.label4 = new System.Windows.Forms.Label(); this.tht1label = new System.Windows.Forms.Label(); this.tht2label = new System.Windows.Forms.Label(); this.l2label = new System.Windows.Forms.Label(); this.l1label = new System.Windows.Forms.Label(); this.l2TextBox = new System.Windows.Forms.TextBox(); this.l1TextBox = new System.Windows.Forms.TextBox(); ((System.ComponentModel.ISupportInitialize)(this.pictureBoxIpl2)).BeginInit(); this.SuspendLayout(); // // xTextbox // this.xTextbox.Location = new System.Drawing.Point(69, 206); this.xTextbox.Name = "xTextbox"; this.xTextbox.Size = new System.Drawing.Size(100, 19); this.xTextbox.TabIndex = 2; // // ytextbox // this.ytextbox.Location = new System.Drawing.Point(69, 256); this.ytextbox.Name = "ytextbox"; this.ytextbox.Size = new System.Drawing.Size(100, 19); this.ytextbox.TabIndex = 3; // // label1 // this.label1.AutoSize = true; this.label1.Location = new System.Drawing.Point(12, 209); this.label1.Name = "label1"; this.label1.Size = new System.Drawing.Size(42, 12); this.label1.TabIndex = 4; this.label1.Text = "座標X："; // // label2 // this.label2.AutoSize = true; this.label2.Location = new System.Drawing.Point(12, 259); this.label2.Name = "label2"; this.label2.Size = new System.Drawing.Size(42, 12); this.label2.TabIndex = 5; this.label2.Text = "座標Y："; // // pictureBoxIpl2 // this.pictureBoxIpl2.Location = new System.Drawing.Point(218, 89); this.pictureBoxIpl2.Name = "pictureBoxIpl2"; this.pictureBoxIpl2.Size = new System.Drawing.Size(640, 480); this.pictureBoxIpl2.TabIndex = 8; this.pictureBoxIpl2.TabStop = false; // // button2 // this.button2.Location = new System.Drawing.Point(69, 310); this.button2.Name = "button2"; this.button2.Size = new System.Drawing.Size(75, 23); this.button2.TabIndex = 15; this.button2.Text = "calc"; this.button2.UseVisualStyleBackColor = true; this.button2.Click += new System.EventHandler(this.button2_Click); // // label3 // this.label3.AutoSize = true; this.label3.Location = new System.Drawing.Point(23, 392); this.label3.Name = "label3"; this.label3.Size = new System.Drawing.Size(31, 12); this.label3.TabIndex = 16; this.label3.Text = "tht1："; // // label4 // this.label4.AutoSize = true; this.label4.Location = new System.Drawing.Point(23, 421); this.label4.Name = "label4"; this.label4.Size = new System.Drawing.Size(31, 12); this.label4.TabIndex = 17; this.label4.Text = "tht2："; // // tht1label // this.tht1label.AutoSize = true; this.tht1label.Location = new System.Drawing.Point(67, 392); this.tht1label.Name = "tht1label"; this.tht1label.Size = new System.Drawing.Size(11, 12); this.tht1label.TabIndex = 18; this.tht1label.Text = "0"; // // tht2label // this.tht2label.AutoSize = true; this.tht2label.Location = new System.Drawing.Point(67, 421); this.tht2label.Name = "tht2label"; this.tht2label.Size = new System.Drawing.Size(11, 12); this.tht2label.TabIndex = 19; this.tht2label.Text = "0"; // // l2label // this.l2label.AutoSize = true; this.l2label.Location = new System.Drawing.Point(23, 162); this.l2label.Name = "l2label"; this.l2label.Size = new System.Drawing.Size(23, 12); this.l2label.TabIndex = 23; this.l2label.Text = "L2："; // // l1label // this.l1label.AutoSize = true; this.l1label.Location = new System.Drawing.Point(23, 112); this.l1label.Name = "l1label"; this.l1label.Size = new System.Drawing.Size(23, 12); this.l1label.TabIndex = 22; this.l1label.Text = "L1："; // // l2TextBox // this.l2TextBox.Location = new System.Drawing.Point(69, 159); this.l2TextBox.Name = "l2TextBox"; this.l2TextBox.Size = new System.Drawing.Size(100, 19); this.l2TextBox.TabIndex = 21; // // l1TextBox // this.l1TextBox.Location = new System.Drawing.Point(69, 109); this.l1TextBox.Name = "l1TextBox"; this.l1TextBox.Size = new System.Drawing.Size(100, 19); this.l1TextBox.TabIndex = 20; // // Form1 // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 12F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.ClientSize = new System.Drawing.Size(935, 707); this.Controls.Add(this.l2label); this.Controls.Add(this.l1label); this.Controls.Add(this.l2TextBox); this.Controls.Add(this.l1TextBox); this.Controls.Add(this.tht2label); this.Controls.Add(this.tht1label); this.Controls.Add(this.label4); this.Controls.Add(this.label3); this.Controls.Add(this.button2); this.Controls.Add(this.pictureBoxIpl2); this.Controls.Add(this.label2); this.Controls.Add(this.label1); this.Controls.Add(this.ytextbox); this.Controls.Add(this.xTextbox); this.Name = "Form1"; this.Text = "Form1"; this.Load += new System.EventHandler(this.Form1_Load); ((System.ComponentModel.ISupportInitialize)(this.pictureBoxIpl2)).EndInit(); this.ResumeLayout(false); this.PerformLayout(); } #endregion private System.Windows.Forms.TextBox xTextbox; private System.Windows.Forms.TextBox ytextbox; private System.Windows.Forms.Label label1; private System.Windows.Forms.Label label2; private OpenCvSharp.UserInterface.PictureBoxIpl pictureBoxIpl2; private System.Windows.Forms.Button button2; private System.Windows.Forms.Label label3; private System.Windows.Forms.Label label4; private System.Windows.Forms.Label tht1label; private System.Windows.Forms.Label tht2label; private System.Windows.Forms.Label l2label; private System.Windows.Forms.Label l1label; private System.Windows.Forms.TextBox l2TextBox; private System.Windows.Forms.TextBox l1TextBox; } }
