Dynamixel MX-64Rでの魅惑的な機能、トルクモード。
C#のFormアプリケーションの基礎テクニックとともに学んできたこの連載もいよいよ最終回。
いよいよ魅惑のトルクモードを使ってみようではありませんか。
トルクモードはMX-64シリーズ、MX-106シリーズから使えるモードです。（MX-28シリーズにはついていません。）
なぜ魅惑か？
最近の世界の研究トレンドはトルク制御に移りつつあります。
これまでの人型ロボット研究をはじめとする制御は、位置制御がほとんどでした。日本でもロボットアームなどではトルク制御が取り入れられていますが、人型に関しては日本ではほぼ位置制御です。位置制御は直感的な制御に向いていて、ロバストな制御はトルク制御が適していると考えられています。
アールティでは位置制御でやれることはけっこうやれてしまっている感があるので、ここのところトルク制御に果敢に取り組んでいます。（意外とトルク制御は難しいんですよ？）
そして、トルク制御をするためには、サーボをトルクモードで動かす必要があります。このモードがばっちり実装されているのは今のところ、ROBOTISのDynamixel MX-64とMX-106のシリーズだけですので、今回の連載はちょっとお高いMX-64Rを使っています。
今回は、手順として次のようにトルクモードで動かす方法を実装します。
- チェックボックスでトルクモードと位置制御モード切り替え
- 危険予防のためリセット（停止）ボタンをつけて実装
- Trackbarの動作をモードごとの動作に変更
- 動かしてみる
では実際にプログラムしましょう。
チェックボックスでモードを切り替える
チェックボックスを実装します。
ToolboxからCheckboxをドラッグアンドドロップします。
TextをTorque mode (check=torque mode, none=position control)に変更します。（トルクモード＝チェック、位置制御＝なし）
これでこのチェックボックスが何のためにあるかを明示できました。
次にイベントを実装しますが、その前に、トルクモードを切り替える関数を下記のように実装します。
下記のプログラムを追加します。
public const int GOAL_TORQUE_L = 0x47; //サーボの目標トルク
public const int TORQUE_CONTROL_MODE_ENABLE = 0x46; //トルクモード on/off
private int torque_mode_change( int mode)
{
int id = 1;
byte [] data = new byte[2]; //モード設定のバイト数は2
int size = 0;
if (mode > 1 && mode < 0) return 0; //エラー処理 01以外の数値がきたら何もしない
data[size++] = TORQUE_CONTROL_MODE_ENABLE; // TORQUE MODE ENABLE トルクモードを設定するマップを指定
if (mode == 1) //トルクモードON
{
data[size++] = ( byte )1;
}
else if (mode == 0) //トルクモードOFF
{
data[size++] = ( byte )0;
}
make_packet(id, INST_WRITE, data, size); //パケットを作って送信
return 1;//成功したことを返す
}
次にイベントを実装します。
checkbox1のプロパティの雷マークのイベントの一覧からCheckedChangedを探し、ダブルクリックして、checkBox1_CheckedChanged()の空の関数を用意します。
下記のようにコードを追加します。
トルクモードにするときに、急に動き出さないように初期値を必ずトルク=0になるように実装します。
private void checkBox1_CheckedChanged(object sender, EventArgs e)
{
//torquemode on = torque control トルクモードがonのとき
if (checkBox1.Checked == true )
{
trackBar1.Maximum = 2047; //スライダーの最大値をトルクの最大値に設定する
trackBar1.Value = 1023; //トルク0に設定する（危険防止のため必ず書いてください）
torque_mode_change(1); //トルクモードをONする関数を呼ぶ
button1_Click(sender, e); //トルクモードであることがわかるようにLEDを点灯する
}
//torquemode off = position control mode
if (checkBox1.Checked == false )
{
trackBar1.Maximum = 4095; //スライダーの最大値を位置制御の最大値に設定する
torque_mode_change(0); //トルクモードをOFFに
button2_Click(sender, e); //LEDも消灯
}
}
これで、トルクモードのON/OFFは実装できました。
トルクモードは、位置制御と違って、指定位置で止まるわけではありません。トルクで制御するためのモードは、言い換えれば、力加減で制御ということになります。
書き言葉だけでは実感がわかないと思いますが、MX-64RやMX-106Rでは60kgf・cm以上の大きなトルクを出せるため、下手に間違うと指なんて簡単に折れてしまう力が加わります。下手な場所で使えば指切断なんてこともありえる強力さです。
そこで、危険防止のために、あらかじめリセットボタンを実装しておきます。
危険予防のためリセットボタンを実装
ToolboxからButtonをドラッグアンドドロップして、0 Resetボタンを作ります。
TEXTを”0 SET”として、場所をスライダーの真下あたりにおきます。（あわてそうだなと思う方は、このボタンの大きさを大きくしてもいいでしょう）このボタンの挙動は、モードによって動作が変わりますが、動作の実装はTrackbarのほうでやります。
ここではボタンとクリックしたときの挙動だけを実装します。
例によって、Button4のプロパティのイベントから、Clickをダブルクリックして、空の関数を作り、次の実装をします。
ここではトルクモードのとき、トルク=0になるようにtrackBar1.Value=1023;に設定します。（マニュアルにはCWのときは1024でトルク0設定と書いてありますが、実際には1023にしないと止まりません。CCWのときは0で停止とマニュアルにありますが、1023で0になる計算になるように実装してあります。ここは危険防止のためにも要注意）
private void button4_Click( object sender, EventArgs e)
{
//torquemode on = torque control トルクモードのとき
if (checkBox1.Checked == true )
{
trackBar1.Value = 1023; //トルク=0に設定
}
//torquemode off = position control mode 位置制御モードのとき
if (checkBox1.Checked == false )
{
//何もしない（自動で読み取られるため、急に動かないようにその位置を保持する）
}
trackBar1_Scroll(sender, e); //スライダーの動作を呼び出す
}
trackbarの動作を実装
trackBar1は、実際にはtimer1_Tickで動かされています。
trackBar1では、動かされたときにフラグを立てているだけですので、動作はtimer1_Tickで実装します。
private void timer1_Tick( object sender, EventArgs e)
{
int id = 1;
byte [] data = new byte[3];
int size = 0;
if (packet_flag == 1) //スライダーが動いたら実行
{
if (checkBox1.Checked == true ) //トルクモードのとき
{
data[size++] = GOAL_TORQUE_L; //トルクの目標値のマップ設定
if (degree <= 1023) { degree = 1023-degree; } //トルクの値を設定 1023以下は0から始まるがスライダーは1023から始まるので引き算する data[size++] = ( byte )(degree & 0xFF); //下位バイト data[size++] = ( byte )((degree & 0xFF00) >> 8); //上位バイト
}
else
{
data[size++] = GOAL_POSITION_L; //位置制御の目標位置マップ設定
data[size++] = ( byte )(degree & 0xFF); //下位バイト
data[size++] = ( byte )((degree & 0xFF00) >> 8); //上位バイト
}
make_packet(id, INST_WRITE, data, size);
}
}
動かしてみる
ここまで実装できましたか？
最後にプログラム全文を掲載しますので参照して下さい。
F5を押して、アプリを起動します。MX-64Rのキットを接続して実行してみます。
普通に位置制御モードでは指定した角度に動きますね？
【危険防止のために重要】
ぐるぐる回ってしまったら、あせらずに、先ほど実装した0 resetボタンを押すか、チェックボックスを位置制御に切り替えてください。
もしくは、電源を切りましょう。（USBは抜いても動作は残るので意味ないです。）
それでは、チェックボックスをonにして、トルクモードに切り替えて、ちょっとずつ動かしてください。
トルクモードは位置制御と違って、トルクがベースになっていますから、いきなり、がつっとスライダーを動かすと急に強いトルクで動き出してぐるぐる回り、とても危険です。
スライダーではうまくとめられないと思いますから、0 SETボタンで停止させます。
怖くない速度がどの辺かつかめたら、スライダーをほんの少し動かして、手でサーボのバーをとめてみてください。
ぐいぐい押してくる感触が楽しめると思います。そして、手を離すとずっとぐるぐる回り続けますね。
必要なトルク分だけで制御するため、このような挙動になります。ですから、かかる力を見極めて必要な分だけトルクを出してあげるようにプログラムを変える必要があります。これがトルク制御です。
トルク制御にはいろいろな方式があり、よく使われるものにトルクと位置のハイブリッド制御などがあげられますが、そのあたりは卒論、修論などで研究テーマにしている人も多いのでここでは取り上げません。（ここで書いてしまったら卒論、修論でそれ以上のものを書かないといけなくなるでしょう？ｗ）
Dynamixelは、サーボを複数同時に動かす設定をパケットにすることももちろんできます。
通信マニュアルやマニュアルを見ながら、これまでのここでの経験を生かして、自分で開発してぜひよいロボットシステムを作り上げてください。
理論と実装がきちんとしていれば、サーボはかならずその挙動で応えてくれるはずです。
さて、これでDynamixel MXシリーズを動かすC#連載は終了です。
アールティではほぼ毎週ROBOTISに発注をかけていますので、納品も比較的早いです。（サーボはある程度在庫していますのであれば即納。）
ぜひ皆さんが、弊社からROBOTISサーボを買ってくださることを願って筆をおきます。
ここまでお付き合いいただき、ありがとうございました。
※ご質問、ご要望のある方はコメント欄にご記入ください。（答えられるものとそうでないものがありますが、極力回答したいとおもいます。また、spamやトラブルが多くなるようならコメント欄は予告無く閉鎖しますのであらかじめご了承ください。）
【関連商品】
 MX-64Rスターターセット購入ページへ |
 Dynamixelショップページ |
 ROBOTIS関連商品ページ |
プログラム全文
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO.Ports;
using System.ServiceProcess;
namespace SerialCommunication
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
}
//ROBOTIS用のチェックサムの計算
private byte calc_checksum_robotis( byte[] packet)
{
int checksum = 0;
for (int i = 2; i < packet.Length - 1; i++)
{
checksum += ( int )packet[i];
}
checksum = (~checksum) & 0xFF;
return (byte )(checksum&0xFF);
}
private void button1_Click( object sender, EventArgs e)
{
byte [] param = new byte[8];
int size = 0;
int id = 4;
//パラメータのセット
param[size++] = 0xFF; //ヘッダー
param[size++] = 0xFF; //ヘッダー
param[size++] = ( byte )(int )(id & 0xFF); //ID
param[size++] = ( byte )4; //パラメータのバイト数＋2
param[size++] = ( byte )3; //2: READ, 3: WRITE
param[size++] = 0x19; //LED map address
param[size++] = 1; // 1: on 0: off
param[size++] = calc_checksum_robotis(param);
//サーボの書き込み
if (serialPort1.IsOpen == true )
{
serialPort1.Write(param, 0, size);
}
}
private void button2_Click( object sender, EventArgs e)
{
byte [] param = new byte[8];
int size = 0;
int id = 4;
//パラメータのセット
param[size++] = 0xFF; //ヘッダー
param[size++] = 0xFF; //ヘッダー
param[size++] = ( byte )(int )(id & 0xFF); //ID
param[size++] = ( byte )4; //パラメータのバイト数＋2
param[size++] = ( byte )3; //2: READ, 3: WRITE
param[size++] = 0x19; //LED map address
param[size++] = 0; // 1: on 0: off
param[size++] = calc_checksum_robotis(param);
//サーボの書き込み
if (serialPort1.IsOpen == true )
{
serialPort1.Write(param, 0, size);
}
}
private void Form1_Load( object sender, EventArgs e)
{
// if (serialPort1.IsOpen == false)
// {
// serialPort1.Open();
// }
}
private void Form1_FormClosed( object sender, FormClosedEventArgs e)
{
if (serialPort1.IsOpen == true )
{
serialPort1.Close();
}
}
private void comboBox1_DropDown( object sender, EventArgs e)
{
// すべてのシリアル・ポート名を取得する
string [] ports = SerialPort .GetPortNames();
// シリアルポートを毎回取得して表示するために表示の度にリストをクリアする
comboBox1.Items.Clear();
foreach (string port in ports)
{
// 取得したシリアル・ポート名を出力する
comboBox1.Items.Add(port);
}
}
private void button3_Click( object sender, EventArgs e)
{
//もしcomboBox1に設定があればシリアルポートを開く
if (comboBox1.Text != "" )
{
serialPort1.PortName = comboBox1.Text;
serialPort1.BaudRate = 1000000;
if (serialPort1.IsOpen == true )
{
serialPort1.Close();
}
if (serialPort1.IsOpen == false )
{
serialPort1.Open();
if (serialPort1.IsOpen == true )
{
MessageBox .Show("Open Success\n" + serialPort1.PortName.ToString() + " " + serialPort1.BaudRate.ToString());
}
else
{
MessageBox .Show("COM Port error" );
}
}
}
}
//角度のグローバル変数
private int degree=0;
private int packet_flag = 0;
//Dynamixel MX-64Rのアドレスマップより
public const int GOAL_POSITION_L = 0x1E; //サーボの目標角度
public const int GOAL_TORQUE_L = 0x47; //サーボの目標トルク
public const int TORQUE_CONTROL_MODE_ENABLE = 0x46; //トルクモード on/off
public const int INST_WRITE = 3; //書き込み
private void trackBar1_Scroll( object sender, EventArgs e)
{
degree = trackBar1.Value; //設定位置の取得
packet_flag = 1;
}
private void make_packet( int id, int com, byte [] data, int size)
{
byte [] packet = new byte[6+size];
int offset = 0;
int len = size+2;
packet[offset++] = 0xFF;
packet[offset++] = 0xFF;
packet[offset++] = ( byte )(id&0xFF);
packet[offset++] = ( byte )(len&0xFF);
packet[offset++] = ( byte )com;
//データ分のパケット作成
for (int i = 0; i < size; i++)
{
packet[offset++] = data[i];
}
packet[offset++] = calc_checksum_robotis(packet);
if (serialPort1.IsOpen == true )
{
serialPort1.Write(packet, 0, packet.Length);
packet_flag = 0;
}
}
private void timer1_Tick( object sender, EventArgs e)
{
int id = 4;
byte [] data = new byte[3];
int size = 0;
if (packet_flag == 1)
{
if (checkBox1.Checked == true )
{
data[size++] = GOAL_TORQUE_L;
if (degree <= 1023) { degree = 1023-degree; } data[size++] = ( byte )(degree & 0xFF); data[size++] = ( byte )((degree & 0xFF00) >> 8);
}
else
{
data[size++] = GOAL_POSITION_L;
data[size++] = ( byte )(degree & 0xFF);
data[size++] = ( byte )((degree & 0xFF00) >> 8);
}
make_packet(id, INST_WRITE, data, size);
}
}
private void serialPort1_DataReceived( object sender, SerialDataReceivedEventArgs e)
{
byte [] readData = new byte[serialPort1.BytesToRead];
//シリアルポートを受信
serialPort1.Read(readData, 0, readData.Length);
//delegateを呼び出す
Invoke( new AppendTextDelegate (packet_restortion), readData);
}
//delegate（処理を委譲）
delegate void AppendTextDelegate( byte [] text);
//委譲先の関数
private void packet_restortion( byte[] data)
{
//この関数には他にも処理を書きたいのでいったん別の関数に飛ばす
byte_to_hex_show(data);
}
//byteのデータを人間に読めるアスキー(16進)に変換
private void byte_to_hex_show( byte[] data)
{
string str = "" ;
string hex = "" ;
for (int i = 0; i < data.Length; i++) { hex = data[i].ToString( "X" ); if (hex.Length % 2 == 1) { hex = "0" + hex; //文字列が1個だったら0を追加 } str += hex + " " ; hex = "" ; } //RichTextBoxの一番上に表示されるように設定 richTextBox1.Text = str + "\n" + richTextBox1.Text; } private int torque_mode_change( int mode) { int id = 4; byte [] data = new byte[2]; int size = 0; if (mode > 1 && mode < 0) return 0;
data[size++] = TORQUE_CONTROL_MODE_ENABLE; // TORQUE MODE ENABLE
if (mode == 1)
{
data[size++] = ( byte )1;
}
else if (mode == 0)
{
data[size++] = ( byte )0;
}
make_packet(id, INST_WRITE, data, size);
return 1;
}
private void checkBox1_CheckedChanged( object sender, EventArgs e)
{
//torquemode on = torque control
if (checkBox1.Checked == true )
{
trackBar1.Maximum = 2047;
trackBar1.Value = 1023;
torque_mode_change(1); //ON
button1_Click(sender, e);
}
//torquemode off = position control mode
if (checkBox1.Checked == false )
{
trackBar1.Maximum = 4095;
torque_mode_change(0); //OFF
button2_Click(sender, e);
}
}
private void button4_Click( object sender, EventArgs e)
{
//torquemode on = torque control
if (checkBox1.Checked == true )
{
trackBar1.Value = 1023;
}
//torquemode off = position control mode
if (checkBox1.Checked == false )
{
//何もしない（自動で読み取られるため、その位置を保持する）
}
trackBar1_Scroll(sender, e);
}
}
}
