python机器人编程——四轴UARM机械臂的运动控制（逆解）原理及python实现（下）

1 概要

本篇是《python——VREP数字孪生四轴机械臂联合仿真》https://editor.csdn.net/md/?articleId=125154971的姊妹篇（下），主要解决的问题是如何通过python编程实现对vrep里uarm机械臂的在世界坐标系下的精准运动控制，即可以根据想要的坐标（x，y，z），使用机械臂逆解算法计算出uarm各个轴的旋转角度，并发送指令到vrep仿真模型，执行动作。本篇将分为两个部分：
（上）uarm机械臂的结构分析及逆解算式推导
本节将对uarm的结构进行分析，并进行几何简化，原理类似笔者之前所著的《对六自由度机械臂的运动控制及python实现（附源码）》https://mp.csdn.net/mp_blog/creation/editor/119819623，同时，绕开机器人传统复杂的空间变换矩阵运算，利用初中几何知识对机器人逆解算法进行推导。
（下）uarm机械臂运动控制程序的python实现
本节根据上节得到的逆解算式，用python、面向对象等知识，进行uarm控制程序的编程。为《python——VREP数字孪生四轴机械臂联合仿真》提供核心控制算法。

2 uarm机械臂运动控制程序的python实现

2.1机械结构的python表达

首先我们根据uarm的各部件的尺寸定义好简化模型的长度，创建一个uarm类：

对应上面的几何连杆python的程序如下：

class uarm:
    #以下定义uarm臂的物理参数，对上篇python机器人编程——四轴UARM机械臂的运动控制（逆解）原理及python实现（上）
    #转轴1
    J1=0
    #转轴2
    J2=0
    #转轴1
    J3=0
    #固定基座杆lg 单位：mm
    Lg=22.1
    #固定基座杆l0 单位：mm
    L0=104.9
    #驱动臂杆L1 单位：mm
    L1=148
    #驱动臂杆L2 单位：mm
    L2=160
    #从动臂杆L3
    L3=34.5
    #吸头长度
    L4=59.3
    #臂架投影
    L=100

2.2 逆解算法的python表达

2.2.1 俯视图根据目标点（x,y）计算J1，L

如下图俯视，已知坐标（x,y）计算旋转角度，和投影臂长L：

对应的python函数，属于uarm类的方法，如下：

    def J1AndLBy(self,x,y):
    	J1=round(np.arctan(y/x)/np.pi*180,2)
    	L=round(x/np.cos(self.J1),2)
        return True,J1,L
       

2.2.2 侧视图根据目标点（J1,L,z）计算剩余角度J2，J3

我们根据上篇的推导公式编写python程序，作为一个uarm类的方法：

    def J2AndJ3By(self,J1,L,z=0):
 
        try:
            
            (XXm,YYm)