DOBOT Magicianの可動域(3D)

以前作成したDOBOT Magicianの横から見たときの可動域を回転させ3D描画します.
各ジョイントの範囲の制限しか考慮しておらず実際の可動域とは異なりますのでご注意ください.

可動域の描画

dobot_3d.py

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math

L2 = 135
L3 = 147
end_effector_params = [61.0,0.0,0.0]

def ki(j1,j2,j3):
  if(check_joint_limit(j1,j2,j3) == False):
    raise Exception('out of range')

  j1_rad = math.pi * j1 / 180
  j2_rad = math.pi * j2 / 180
  j3_rad = math.pi * j3 / 180

  l = L2 * math.sin(j2_rad) + L3 * math.cos(j3_rad) + end_effector_params[0]
  z = L2 * math.cos(j2_rad) - L3 * math.sin(j3_rad) + end_effector_params[2]

  x = l * math.cos(j1_rad) + end_effector_params[1] * math.sin(j1_rad)
  y = l * math.sin(j1_rad) + end_effector_params[1] * math.cos(j1_rad)

  return (x,y,z)

def check_joint_limit(j1,j2,j3):

  d_j = j3 - j2

  if (j1 >= -125 and j1 <= 125):
    if (j2 >= 0 and j2 <= 85):
      if (j3 >= -10 and j3 <= 95):
        return True

  return False

fig = plt.figure()
ax = Axes3D(fig)

ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")

ax.set_xlim(-100,300)
ax.set_ylim(-300,300)
ax.set_zlim(-200,200)

for j1 in range(-180,180,10):
  for j2 in range(-180,180,10):
    for j3 in range(-180,180,10):
      try:
        x,y,z = ki(j1,j2,j3)
        ax.plot([x],[y],[z],marker=".",linestyle='None',color='blue',alpha=0.2)
      except:
        pass

plt.show()

結果は以下のようになりました.

PCDデータとして保存

dobot_pcd.py

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math

L2 = 135
L3 = 147
end_effector_params = [61.0,0.0,0.0]

def ki(j1,j2,j3):
  if(check_joint_limit(j1,j2,j3) == False):
    raise Exception('out of range')

  j1_rad = math.pi * j1 / 180
  j2_rad = math.pi * j2 / 180
  j3_rad = math.pi * j3 / 180

  l = L2 * math.sin(j2_rad) + L3 * math.cos(j3_rad) + end_effector_params[0]
  z = L2 * math.cos(j2_rad) - L3 * math.sin(j3_rad) + end_effector_params[2]

  x = l * math.cos(j1_rad) + end_effector_params[1] * math.sin(j1_rad)
  y = l * math.sin(j1_rad) + end_effector_params[1] * math.cos(j1_rad)

  return (x,y,z)

def check_joint_limit(j1,j2,j3):

  d_j = j3 - j2

  if (j1 >= -125 and j1 <= 125):
    if (j2 >= 0 and j2 <= 85):
      if (j3 >= -10 and j3 <= 95):
        return True

  return False

fig = plt.figure()
ax = Axes3D(fig)

ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")

ax.set_xlim(-100,300)
ax.set_ylim(-300,300)
ax.set_zlim(-200,200)


points = []

for j1 in range(-180,180,10):
  for j2 in range(-180,180,10):
    for j3 in range(-180,180,10):
      try:
        x,y,z = ki(j1,j2,j3)
        ax.plot([x],[y],[z],marker=".",linestyle='None',color='blue',alpha=0.2)
        points.append([x,y,z])
      except:
        pass

# plt.show()

### change to ply

import open3d as o3d
import numpy as np

xyz = np.array(points)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz)
o3d.io.write_point_cloud("dobot.ply", pcd)

Cloudcompareで表示してみると以下のようになりました.