DobotMagician プログラム内で滑らかに軌道に沿う動作を行う

Dobotで軌道に沿う動作を行う場合、軌道に沿うような複数の座標点を与えます。その際SetPTPCmdを使用すると、1つ1つの点で停止してしまい、滑らかな動作をしません。ここでSetCPCmdを使用します。このコマンドは、3Dプリントやレーザー彫刻の際に使われているコマンドで、複数の座標点を与えたときに連続的な動作を行うことができます。今回は円軌道に沿う動作を行うプログラム例に、軌道に沿う動作を行う方法を解説します。Dobot StudioのScripts環境で行った場合とC++11環境で行った場合の２種類の例を示します。

内容

1.Dobot StudioのScripts 環境での例
2.C++11環境での例
- 2.1軌道を生成するプログラム
- 2.2軌道を送信するプログラム
3.最後に

1.Dobot StudioのScripts 環境での例

Dobot StudioのScriptsで円軌道に沿う動作を行う場合は以下ようなプログラムになります。円の軌道に沿うような複数の座標点を生成し、SetCPCmdでDobotに送ります。

import math

cpparams = []

divide = 100
r = 40
x0 = 240
y0 = 0
z0 = -30


for i in range(divide):

       x = r * math.cos(2 * math.pi / divide * (i+1)) + x0
       y = r * math.sin(2 * math.pi / divide * (i+1)) + y0
       z = z0
       vel = 1000
       cpparams.append([x, y, z, vel])

print(cpparams)

cpMode = 1
isQueued = 1
count = 0
temp = 0

dType.SetHOMECmd(api, temp, isQueued=0)

for i in range(100):
       for i in cpparams:
              dType.SetCPCmd(api, cpMode, i[0], i[1], i[2], i[3], isQueued)

2.C++11環境での例

今回作成したソフトウェアは２つのプログラムから構成されます

軌道を生成するプログラム
軌道を読み込みDobotへ送信するプログラム

1のプログラムで生成した軌道はファイルに保存され、そのファイルからデータを2のプログラムで読み込み、DobotへSetCPCmdで送信します。ファイル形式はcsvを使用しているため、例えばExcelのような表計算ソフトから生成した軌道座標点データを使用し2のプログラムで使用することも可能です。

2.1軌道を生成するプログラム

まず1の軌道生成についてのプログラムです。今回はz値一定の円軌道の座標点を生成するプログラムを紹介します。言語はPythonを使用しました。

<C++11環境サンプルプログラム1>

#!/usr/bin/env python
import numpy as np
import math

path = “座標データを保存するパス”

# x y z velocity

ndarr = np.array([])

divide = 100;
r = 20
x0 = 230
y0 = 0

for i in range(divide):
      x = r * math.cos(2 * math.pi / divide * (i+1)) + x0
      y = r * math.sin(2 * math.pi / divide * (i+1)) + y0
      z = 50
      vel = 100
      n = np.array([[x,y,z,vel]])

if i is 0:
      ndarr = n

else:
      ndarr = np.append(ndarr, n, axis=0)

print(ndarr)
np.savetxt(path + '/circle.csv', ndarr, delimiter=',')

2.2軌道を送信するプログラム

次に2のプログラムです。先ほど生成したファイルから座標データを読み込み、Dobotにデータを送信するプログラムです。またDobot StudioのScriptsを使用した場合は、Dobotのキューがフルになった場合にコマンドの送信を止める処理が自動で行われますが、C/C++を使用した場合は、キューがフルでもコマンドを送信してしまいます。そこでサンプル2では以下のように、コマンドが正常に送れたか確認しつつSetCPCmdを送信しています。

  while(1)
  {
  	bool ret = SetCPCmd(&cmd, true, &index);
  	if (!ret)break;
  }

次に起動をDobotに送信するプログラム全体を添付します。

<C++11環境サンプルプログラム2>

#include <stdint.h>
#include <queue>

#include " DobotDll.h"

#include <fstream>
#include <string>
#include <sstream>
#include <vector>

using namespace std;

vector<vector<double>> data_;

vector<string> split(string& input, char delimiter)
{
  istringstream stream(input);
  string field;
  vector<string> result;
  while (getline(stream, field, delimiter)) {
  result.push_back(field);
  }
  return result;
}

int cpcmd(double x, double y, double z, double vel)
{

  CPCmd cmd;
  uint64_t index;

  cmd.cpMode = 1; // 0 relative, 1 absolute
  cmd.x = x;
  cmd.y = y;
  cmd.z = z;
  cmd.velocity = vel;

  while(1)
  {
    bool ret = SetCPCmd(&cmd, true, &index);
    if (!ret)break;
  }
  return index;
}

int main(int argc, char **argv)
{

  // read csv
  String path = “ファイルを保存したパス”;
  ifstream ifs(path + "circle.csv");

  string line;
  while (getline(ifs, line)) {

    vector<string> strvec = split(line, ',');

    vector<double> linedata;
    for (int i=0; i<strvec.size();i++){
      printf("%5f\n", stof(strvec.at(i)));
      linedata.push_back(stof(strvec.at(i)));
    }
    data_.push_back(linedata);
  }

  
  std::string device = “/dev/ttyUSB0”;  

  int result = ConnectDobot(device.c_str(), 115200, 0, 0);
  switch (result)
  {
    case DobotConnect_NoError:
      break;
    case DobotConnect_NotFound:
      cout << ("Dobot not found") << endl;
      return 1;
    case DobotConnect_Occupied:
      cout << "Invalid port name " + device + " or Dobot is occupied by another application" << endl;
      return 2;
    default:
  break;
  }
  
  SetQueuedCmdClear();
  SetQueuedCmdStartExec();
  
  CPParams params;
  params.planAcc = 300;
  params.juncitionVel = 300;
  params.acc = 300;
  params.realTimeTrack = 0;

  uint64_t index;
  SetCPParams(&params, false, &index);
  
  cpcmd(data_[0][0], data_[0][1], 50, data_[0][3]);

  for (int i=0;i<data_.size();i++)
  {
  　　　　　　index = cpcmd(data_[i][0], data_[i][1], data_[i][2], data_[i][3]);
  　　　　　　std::cout << "index:" << index << std::endl;
  }

  cpcmd(data_[data_.size()-1][0], data_[data_.size()-1][1], 50, data_[data_.size()-1][3]);

  return 0;
}

3.最後に

今回説明したことを応用することで、Dobot Magicianを動作させるプログラム内で任意の軌道に沿った動作を追加することができ動作の幅が広がると思います。

ここまでの手順のどこかで躓いてしまった場合には、下記のメールアドレスまでお問い合わせください。

お問い合わせ : dobot@techshare.co.jp