六轴机械臂求逆解python代码 六轴机械臂操作方法

1、设置变量并传递机械臂关节角度

这里有五种卡片

（1）获取所有角度

（2）设置关节，单指某一个关节。

（3）设置全角度（6轴机械臂和4轴机械臂）

（4）设置全角度为

创建关节变量，将显示所有关节的角度，相关python代码如下：

engles=mc.get_engles()

print(engles)

显示结果如下图

2、设置变量并显示机械臂头部姿态

设置coords变更，并将当前机械臂的头部姿态坐标，传给变量coords并显示。

相当于执行：

coords=

print(coords)

设置变量如图

然后在左侧找到“角度和坐标”将“获取所有坐标”拖进来。

然后再去“文本”中选择“输出”项，并将他拖到工作区。

最后我们组合成如下结构：

然后点击运行，结果如下图

这里显示出当前机械臂头部姿态坐标。

3、机械臂夹爪检测

通过调用mycobot的Set_Gripper_State函数分别让夹爪进行10次张开收拢操作，并且在每组张开收拢后对其进行角度调位。

1、重新初始化夹爪

2、设置夹爪状态，可以选择夹爪的状态，可选“打开”或“关闭”，速度指夹爪开合速度

3、设置夹爪张开的角度，值为0~255，速度指的是夹爪开合的速度，取值为

4、夹爪是否在运动，给出夹爪的状态。

测试程序

程序代码：

from pymycobot.mycobot import MyCobot
import time
mc = MyCobot('/dev/ttyAMA0',1000000)
mc.send_angles([0,0,0,0,0,0],20)
for count in range(5):
  mc.set_gripper_value(0,40)
  time.sleep(2)
  mc.set_gripper_value(255,40)
  time.sleep(2)
mc.set_gripper_state(1, 40)

4、吸泵操作

吸泵的联接按第二章第三节中的（2）吸泵操作进行联结。

1、设置吸泵

第1步：为设置吸泵的工作模式，这里选BCM,即使用数字0,1代表高低电位，当然如果选用Board则必须选择使用高低电位来表示开启与关闭。此处数据派机械臂我们选择BCM模式。

第2步：设置“20”和“21”号引脚的工作方式为“out”

第3步：输出高低电位，启动或停止工作吸泵。

2、启动吸泵工作

“20”和“21”号引脚输出低电位为启动吸泵。

相关python代码如下：

from pymycobot.mycobot import MyCobot
import RPi.GPIO as GPIO
mc = MyCobot('/dev/ttyAMA0',1000000)
GPIO.setmode(GPIO.BCM)
GPIO.setup(20,GPIO.OUT)
GPIO.setup(21,GPIO.OUT)
GPIO.output(20,GPIO.LOW)
GPIO.output(21,GPIO.LOW)

5、机械臂的点动控制

本案例主要的实验内容是调用 jog_angle点动 函数通过循环让六个关节分别持续移动，通过 jog_stop 函数对其运动进行停止，从而实现点动控制。最后让机械臂移动到一个较为安全的位置并进行释放关节和断电操作。

代码如下：

from pymycobot.mycobot import MyCobot
import time
i = None
mc = MyCobot('/dev/ttyAMA0',1000000)
mc.power_on()
mc.send_angles([0,0,0,0,0,0],30)
time.sleep(3)
for i in range(1, 6):
  mc.jog_angle(i,1,20)
  time.sleep(1)
  mc.jog_stop()
  print(mc.get_encoder(i))
  print('')
mc.send_angles([180,126,(-153),136,(-82),30],30)
time.sleep(5)
mc.power_off()

6、机械臂控制机制

本案例主要是调用一些机械臂常用的控制机制函数进行机械臂控制，比如机械臂的断电、供电、暂停运动、恢复运动等控制机制功能以及机械臂头部灯的控制。

7、机械臂进阶操作

主要实现机械臂智能判断已经到达指定位置的功能，基于此功能，简单的让机械臂重复执行两个到达指令。

（1）先用 if do 模块判断机械臂是否充电，若没有充电则为其充电。输出当前的角度节点信息，

（2）让机械臂转移到零位。定义 angles 变量，使用创建 list 类型数据中的 repeated 方法为 angles 赋值零位节点信息。定义 limit_time 判断移动是否超时。

（3）将 angles 传入 is in position 方法中判断机械臂是否达到指定位置。使用 repeat 模块每运动 0.5s 检测机械臂是否达到指定位置，并进行超时计时器处理，若超过 7s 机械臂仍未到达指定位置则断定其已经到达了，执行之后的指令。

（4）接下来的原理大同小异故不再做解释。

测试代码：

from pymycobot.mycobot import MyCobot
import time
angles = None
limit_time = None
mc = MyCobot('/dev/ttyAMA0',1000000)
if 0 == mc.is_power_on():
  print('detect arm')
  mc.power_on()
print(mc.get_angles())
for count in range(4):
  mc.send_angles([0,0,0,0,0,0],20)
  angles = [0] * 6
  limit_time = 1
  while 0 == mc.is_in_position(angles,0):
    time.sleep(0.5)
    if limit_time >= 7:
      break
    limit_time = limit_time + 0.6
  mc.send_angles([143,100,(-153),125,(-82),30],20)
  angles = [143, 100, -153, 125, -82, 30]
  limit_time = 0
  while 0 == mc.is_in_position(angles,0):
    time.sleep(0.5)
    if limit_time >= 7:
      break
limit_time = limit_time + 0.6

8、控制LED灯

在左侧选LED将此条拖进来如图，然后选择运行。需要改颜色的地方就改为255。

我们第三章第五节的测试程序在这里可以表达为：

i = 3
#循环3次
while i > 0:                           
    mc.set_color(0,0,255) #蓝灯亮
    time.sleep(2)    #等2秒               
    mc.set_color(255,0,0) #红灯亮
    time.sleep(2)    #等2秒
    mc.set_color(0,255,0) #绿灯亮
    time.sleep(2)    #等2秒
i -= 1