1.简介

CS在进行一些二次开发时需要用到一些示教功能比如沿工具的实时姿态进行移动，也就是示教器上的操作界面中的沿机器人的XYZ等方向点动；以及各关节的点动；下序示例中讲解了CS机器人jog运动在python中如何实现。

2.操作流程

2.1 Jog沿指定坐标系移动：

沿指定坐标系移动，此运动是以时间形式运动如需停止可发送stopl( )至30001端口，详细停止函数见下方def stop():

位置计算脚本函数：

# 设置平移和旋转速度
trans_speed = 0.09
rotate_speed = 0.36
# 设置用户坐标系数据，注意：如果用户坐标系通过脚本get_actual_tcp_pose()获取，则可以实现沿实时工具位置的XYZ进行旋转和平移操作。
user_frame = get_actual_tcp_pose()
cos0 = cos(user_frame[3])
cos1 = cos(user_frame[4])
cos2 = cos(user_frame[5])
sin0 = sin(user_frame[3])
sin1 = sin(user_frame[4])
sin2 = sin(user_frame[5])
x_dir = [1.0, 0.0, 0.0]
y_dir = [0.0, 1.0, 0.0]
z_dir = [0.0, 0.0, 1.0]
x_dir[0] = cos2 * cos1
x_dir[1] = sin2 * cos1
x_dir[2] = -sin1
y_dir[0] = cos2 * sin1 * sin0 - sin2 * cos0
y_dir[1] = sin2 * sin1 * sin0 + cos2 * cos0
y_dir[2] = cos1 * sin0
z_dir[0] = cos2 * sin1 * cos0 + sin2 * sin0
z_dir[1] = sin2 * sin1 * cos0 - cos2 * sin0
z_dir[2] = cos1 * cos0
# 下方speedl脚本表示沿用户坐标系user_frame的X轴以trans_speed速度，0.6m/s²加速度平移100s
speedl([x_dir[0] * trans_speed, x_dir[1] * trans_speed, x_dir[2] * trans_speed, 0.0, 0.0, 0.0], 0.6, 100.0)
# 下方speedl脚本表示沿用户坐标系user_frame的Y轴以trans_speed速度，0.6m/s²加速度平移100s
speedl([y_dir[0] * trans_speed, y_dir[1] * trans_speed, y_dir[2] * trans_speed, 0.0, 0.0, 0.0], 0.6, 100.0)
# 下方speedl脚本表示沿用户坐标系user_frame的Z轴以trans_speed速度，0.6m/s²加速度平移100s
speedl([z_dir[0] * trans_speed, z_dir[1] * trans_speed, z_dir[2] * trans_speed, 0.0, 0.0, 0.0], 0.6, 100.0)
# 下方speedl脚本表示沿用户坐标系user_frame的X轴以rotate_speed速度，0.6rad/s²加速度旋转100s
speedl([0.0, 0.0, 0.0, x_dir[0] * rotate_speed, x_dir[1] * rotate_speed, x_dir[2] * rotate_speed], 0.6, 100.0)
# 下方speedl脚本表示沿用户坐标系user_frame的Y轴以rotate_speed速度，0.6rad/s²加速度旋转100s
speedl([0.0, 0.0, 0.0, y_dir[0] * rotate_speed, y_dir[1] * rotate_speed, y_dir[2] * rotate_speed], 0.6, 100.0)
# 下方speedl脚本表示沿用户坐标系user_frame的Z轴以rotate_speed速度，0.6rad/s²加速度旋转100s
speedl([0.0, 0.0, 0.0, z_dir[0] * rotate_speed, z_dir[1] * rotate_speed, z_dir[2] * rotate_speed], 0.6, 100.0)

注：上述中提到的除了三角函数外，其他指令函数都可以查询CS机器人的脚本手册下载中心_机器人技术-艾利特官网；例如speedl为工具末端加速运动等等。

停止函数：

#获取机器人运行速度
def Get_robot_speed():
    i = dashboard_shell("status")  # 返回机器人状态
    print(i)
    i = i[23:26]
    i = i.replace("\\r", '')
    i = i.replace("\\", '')
    return i
 
def stop():
    # 注意：脚本中stopl的加速度回受速度百分比影响，也就是如果当前速度百分比很低比如%2、%5等，此时下发stopl(1.5)可能会导致机器人停止很慢，
    # 按照控制器内置的做法，加速度参数最好是一个动态的和速度百分比关联的参数，关系为 1.5/（速度百分比*速度百分比），所以当速度百分比为%2时，
    # 下发的加速度参数应该为 1.5/(0.02*0.02) = 3750，也就是stopl(3750)
    #下方为直接获取29999端口的速度函数来做一个动态的速度百分比调节
    speed = float(Get_robot_speed())/100
    print(speed)
    speed = 1.5/(speed*speed)
    print(speed)
    sendCMD('stopl('+str(speed)+')')

示例：

可先制定30001的发送函数：

#建立socket连接
def connectETController(ip, port):
    #ip, port：IP和端口号
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    try:
        sock.connect((ip, port))
        return (True, sock)
    except Exception as e:
        sock.close()
        return (False,)
 
#30001端口使用，参考脚本手册
def sendCMD(content):
    # 机器人IP地址
    robot_ip = "192.168.1.200"
    port = 30001
    conSuc, sock = connectETController(robot_ip, port)  ##连接机器人ip和端口
    if (conSuc):
        # 以下为机器人脚本代码示例：
        #content:内容为脚本指令
        # 以固定格式def开头，end结尾（end后加入换行）中间内容为机器人支持的脚本，发送。
        sendStr = 'def a():\n{}\nend'.format(content)
        print(sendStr)
        try:
            #print(bytes(sendStr, "utf-8"))
            jdata = sock.sendall(bytes(sendStr, "utf-8"))
            ret = sock.recv(1024)
            # jdata = json.loads(str(ret, "utf-8"))
            print(ret)
        except Exception as e:
            return (False, None, None)

主函数：

def speedl(content):
    # 将上述“位置计算脚本”截取计算部分通过30001端口发送给控制器，既可以实现机器人沿工具的各轴方向进行平动，如果需要实现机器人沿其轴平动，或者沿给定轴旋转，均可参考上述脚本进行调整。
    # content：坐标方向例'x+'、'x-'、'y+'、'y-'、'z+'、'z-'、'Rx+'、'Rx-'、'Ry+'、'Ry-'、'Rz+'、'Rz-'中的其一
    # trans_speed and rotate_speed设置平移和旋转速度，通过trans_speed and rotate_speed的正负值来做正向或反向运动
    #设置用户坐标系数据，注意：如果用户坐标系通过get_actual_tcp_pose()获取，则可以实现沿实时工具位置的XYZ进行旋转和平移操作。
    if content == 'x+':
        trans_speed = '0.09'
        rotate_speed = '0'
        p0 = 'x_dir[0]'
        p1 = 'x_dir[1]'
        p2 = 'x_dir[2]'
    elif content == 'x-':
        trans_speed = '-0.09'
        rotate_speed = '0'
        p0 = 'x_dir[0]'
        p1 = 'x_dir[1]'
        p2 = 'x_dir[2]'
    elif content == 'y+':
        trans_speed = '0.09'
        rotate_speed = '0'
        p0 = 'y_dir[0]'
        p1 = 'y_dir[1]'
        p2 = 'y_dir[2]'
    elif content == 'y-':
        trans_speed = '-0.09'
        rotate_speed = '0'
        p0 = 'y_dir[0]'
        p1 = 'y_dir[1]'
        p2 = 'y_dir[2]'
    elif content == 'z+':
        trans_speed = '0.09'
        rotate_speed = '0'
        p0 = 'z_dir[0]'
        p1 = 'z_dir[1]'
        p2 = 'z_dir[2]'
    elif content == 'z-':
        trans_speed = '-0.09'
        rotate_speed = '0'
        p0 = 'z_dir[0]'
        p1 = 'z_dir[1]'
        p2 = 'z_dir[2]'
    elif content == 'Rx+':
        trans_speed = '0'
        rotate_speed = '0.36'
        p0 = 'x_dir[0]'
        p1 = 'x_dir[1]'
        p2 = 'x_dir[2]'
    elif content == 'Rx-':
        trans_speed = '0'
        rotate_speed = '-0.36'
        p0 = 'x_dir[0]'
        p1 = 'x_dir[1]'
        p2 = 'x_dir[2]'
    elif content == 'Ry+':
        trans_speed = '0'
        rotate_speed = '0.36'
        p0 = 'y_dir[0]'
        p1 = 'y_dir[1]'
        p2 = 'y_dir[2]'
    elif content == 'Ry-': 
        trans_speed = '0'
        rotate_speed = '-0.36'
        p0 = 'y_dir[0]'
        p1 = 'y_dir[1]'
        p2 = 'y_dir[2]'
    elif content == 'Rz+':
        trans_speed = '0'
        rotate_speed = '0.36'
        p0 = 'z_dir[0]'
        p1 = 'z_dir[1]'
        p2 = 'z_dir[2]'
    elif content == 'Rz-':
        trans_speed = '0'
        rotate_speed = '-0.36'
        p0 = 'z_dir[0]'
        p1 = 'z_dir[1]'
        p2 = 'z_dir[2]'
    else:
        print("输入数据格式有误")
 
    sendCMD('   trans_speed = '+trans_speed+'\n'
            '   rotate_speed = '+rotate_speed+'\n'
            # '   user_frame = get_actual_tcp_pose()\n'
            '   user_frame = [0,0,0,0,0,0]\n'
            '   cos0 = cos(user_frame[3])\n'
            '   cos1 = cos(user_frame[4])\n'
            '   cos2 = cos(user_frame[5])\n'
            '   sin0 = sin(user_frame[3])\n'
            '   sin1 = sin(user_frame[4])\n'
            '   sin2 = sin(user_frame[5])\n'
            '   x_dir = [1.0, 0.0, 0.0]\n'
            '   y_dir = [0.0, 1.0, 0.0]\n'
            '   z_dir = [0.0, 0.0, 1.0]\n'
            '   x_dir[0] = cos2 * cos1\n'
            '   x_dir[1] = sin2 * cos1\n'
            '   x_dir[2] = -sin1\n'
            '   y_dir[0] = cos2 * sin1 * sin0 - sin2 * cos0\n'
            '   y_dir[1] = sin2 * sin1 * sin0 + cos2 * cos0\n'
            '   y_dir[2] = cos1 * sin0\n'
            '   z_dir[0] = cos2 * sin1 * cos0 + sin2 * sin0\n'
            '   z_dir[1] = sin2 * sin1 * cos0 - cos2 * sin0\n'
            '   z_dir[2] = cos1 * cos0\n'
            '   speedl(['+p0+'* trans_speed,'+p1+'* trans_speed,'+p2+'* trans_speed,'+p0+'* rotate_speed,'+p1+'* rotate_speed,'+p2+'* rotate_speed], 0.6, 100)'
            )

主程序

if __name__ == "__main__":
    #基座标系下沿Y+方向移动两秒后停止
    speedl('y+')
    time.sleep(2)
    stop(6)

注：详细程序可见test.py

2.2 Jog关节移动：

def speedj():
    # speedj(qd , a , t)
    # qd：每个关节的速度（rad/s），list 型数据：长度为 6，每个元素依次指代了关节目标速度，通过正负值来做正向或反向运动
    # a：：（主轴的）关节加速度（rad/s2），float 型数据
    # t：函数返回的最短时间（s），float 型数据
    
    #下方speedj脚本表示1关节以0.36 rad/s²加速度正向旋转100s
    sendCMD('speedj([0.36,0,0,0,0,0],0.5,100)')

停止函数：

def stopj():
    sendCMD('stopj(1.5)')

示例：

if __name__ == "__main__":
    robot_ip = "192.168.139.128"
    speedj()
    time.sleep(2)
    stopj()

注：详细程序可见test.py

3.常见问题解答

关于jog运动如何停止：脚本中stopl的加速度会受速度百分比影响，也就是如果当前速度百分比很低比如2%、5%等，此时下发stopl(1.5)可能会导致机器人停止很慢，按照控制器内置的做法，加速度参数最好是一个动态的和速度百分比关联的参数，关系为 1.5/（速度百分比*速度百分比），所以当速度百分比为%2时，下发的加速度参数应该为 1.5/(0.02*0.02) = 3750，也就是stopl(3750)。