myCobot280 のシミュレーター

https://gyazo.com/d364effb0e4eb0ea50353b37431c5ab8

1. 概要：

myCobot280のモーションをつくるためにシミュレーション環境を構築。

Python + PyBulletでシミュレーター環境を構築

myCobot280(M5)のモデルは公式mycobot_rosのこれを使用

mycobot_ros-noetic.zip\mycobot_ros-noetic\mycobot_description\urdf\mycobot_280_m5

mycobot_280_m5フォルダをそのまま使用

サンプル１：シミュレーションサーバーにソケット通信でコマンドを送信して操作

サンプル２：Whisper+ChatGPTと連携し音声で操作

URDFのジョイント構成（MyCobot 280 M5）

mycobot_280_m5.urdf の中を見ると、6軸アーム（+場合によってはグリッパーのジョイント）で構成されています。

ジョイント名はこんな順番になっています（URDFより抜粋）：

table:MyCobot 実機APIの関節番号

実機API番号関節名 (URDF) 動作軸

1 joint1_to_joint2 Z回転

2 joint2_to_joint3 Y回転

3 joint3_to_joint4 Y回転

4 joint4_to_joint5 X回転

5 joint5_to_joint6 Y回転

6 joint6_to_joint7 X回転

7〜グリッパーや付属品

URDFを読み込むと、PyBullet内部では getJointInfo(robot_id, index) の index がこう並びます

実機と統一するためのポイント

send_angles(a1,a2,a3,a4,a5,a6, speed) の形で送れば、実機とシミュレータを同じコードで動かせます。

グリッパーが必要なら、PyBulletではジョイント番号7〜以降を制御対象に加えればOK。

（と、思ってたんだけど、ハマってしまったので実装は後日に……）

2. サンプル１：シミュレーションサーバーにソケット通信でコマンドを送信して操作

シミュレーション環境をサーバー側として起動し、別に起動したクライアントからソケット通信で動作コマンドを送信して操作する

シミュレーション環境:sim_server.py

os.path.abspathの指定が必要

クライアント側：sim_client.py

code:ディレクトリ構成

mycobot_sim/

├ sim_server.py

└ mycobot_280_m5/

├ mycobot_280_m5_for_simulation.urdf (mycobot_280_m5.urdfの各daeデータのパスを修正)

└ ...

※クライアント側はどこにおいてもOK

code:pip

pip install pybullet numpy pyserial

code:sim_server.py

import pybullet as p

import pybullet_data

import numpy as np

import time

import os

# ===== 実機API番号 → PyBulletジョイントIndex の対応 =====

JOINT_MAP = {

1: 1,

2: 2,

3: 3,

4: 4,

5: 5,

6: 6

}

class MyCobotSim:

def __init__(self, urdf_path):

self.urdf_path = urdf_path

self.max_speed = np.deg2rad(5) # 1ステップの最大角速度 rad

self.target_angles = 0.0 * 6

self._init_sim()

def _init_sim(self):

p.connect(p.GUI)

p.setAdditionalSearchPath(pybullet_data.getDataPath())

p.resetDebugVisualizerCamera(cameraDistance=0.7,

cameraYaw=50,

cameraPitch=-30,

cameraTargetPosition=0, 0, 0.3)

p.loadURDF("plane.urdf")

p.setGravity(0, 0, -9.8)

# URDF読み込み

self.robot = p.loadURDF(self.urdf_path,

useFixedBase=True,

flags=p.URDF_USE_INERTIA_FROM_FILE)

self.num_joints = p.getNumJoints(self.robot)

print(f"INFO num_joints: {self.num_joints}")

for j in range(self.num_joints):

p.resetJointState(self.robot, j, targetValue=0.0)

def send_angles(self, angles_deg, speed=20):

""" 実機API互換：send_angles(deg..., speed) """

if len(angles_deg) < 6:

print("WARN angles length < 6")

return

self.target_angles = [np.deg2rad(a) for a in angles_deg:6]

self.max_speed = np.deg2rad(speed) / 50.0 # スピードをラフに調整

print("SIM set target angles (deg):", angles_deg:6)

def step(self):

current = [p.getJointState(self.robot, JOINT_MAPi+1)0 for i in range(6)]

for i in range(6):

diff = self.target_anglesi - currenti

step = np.sign(diff) * min(abs(diff), self.max_speed)

new_angle = currenti + step

p.setJointMotorControl2(self.robot,

JOINT_MAPi+1,

p.POSITION_CONTROL,

targetPosition=new_angle,

force=200)

p.stepSimulation()

def disconnect(self):

p.disconnect()

if __name__ == "__main__":

# URDFファイルのパス（mycobot_280_m5.urdfをDLして指定）

urdf_file = os.path.abspath("※ここのパスを設定すること/mycobot_280_m5_for_simulation.urdf")

sim = MyCobotSim(urdf_file)

# デモ動作

sim.send_angles(0, 45, 90, 90, -45, 0, speed=20)

for _ in range(1000):

sim.step()

time.sleep(1./240.)

input("何かキーを押すと終了します...")

sim.disconnect()

code:sim_client.py

import socket, json

s = socket.create_connection(('127.0.0.1', 5005))

msg = json.dumps({"angles":0,30,45,0,-30,10})

s.sendall(msg.encode('utf-8'))

s.close()

code:(おまけ)修正例 mycobot_280_m5.urdf

<?xml version="1.0"?>

<xacro:property name="width" value=".2" />

</geometry>

</visual>

</geometry>

</collision>

</link>

</geometry>

</visual>

</geometry>

</collision>

</link>

</geometry>

</visual>

</geometry>

</collision>

</link>

</geometry>

</visual>

</geometry>

</collision>

</link>

</geometry>

</visual>

</geometry>

</collision>

</link>

</geometry>

</visual>

</geometry>

</collision>

</link>

</geometry>

</visual>

</geometry>

</collision>

</link>

</geometry>

</visual>

</geometry>

</collision>

</link>

</joint>

</joint>

</joint>

</joint>

</joint>

</joint>

</joint>

</robot>

各ジョイントの"package://mycobot_280_m5/joint*.dae"の部分を自分の環境のパスに合わせる

3. サンプル２：Whisper+ChatGPTと連携し音声で操作

※こちらの詳細は個別に聞いてください。

code:ディレクトリ構成

mycobot_sim/

├ mycobot_voice_sim.py

└ mycobot_280_m5/

├ mycobot_280_m5_for_simulation.urdf (mycobot_280_m5.urdfの各daeデータのパスを修正)

└ ...

※クライアント側はどこにおいてもOK

code:pip

pip install openai pymycobot sounddevice scipy openai-whisper ffmpeg-python