【睿尔曼-RealMan】基于python的机械臂正逆解接口函数调用例程

l 2024-06-17

一、连接机械臂时调用正逆解接口：

1.正解函数定义（调用时无需关注）

def Algo_Forward_Kinematics(cls, joint):
"""
brief Algo_Forward_Kinematics 正解函数
param joint 关节1到关节7角度单位°
return Pose 目标位姿
"""
cls.pDll.Algo_Forward_Kinematics.restype = Pose
joint = (ctypes.c_float * 7)(*joint)
Pose_ = cls.pDll.Algo_Forward_Kinematics(joint)
position = Pose_.position
euler = Pose_.euler
pose = [position.x, position.y, position.z, euler.rx, euler.ry, euler.rz]
return pose

2.函数调用

（1）导入对应的库文件；

from robotic_arm_package.robotic_arm import * import sys

（2）调用正解函数

def demo(robot): #自定义函数，传入'robot'

在进行正逆解之前，需要先获取机械臂的当前状态，机械臂安装角度、工作坐标系等。

ret1 =robot.Get_Install_Pose() if ret1 != 0: print("当前机械臂安装角度" + str(ret1)) sys.exit() ret2 =robot.Get_Current_Work_Frame() if ret2 != 0: print("当前机械臂工作坐标系" + str(ret2)) sys.exit()

正解：由机械臂关节角度转换到位姿的过程。

输入关节角度，例：

joint = [0, 0, -90, 0, -90, 0]

将参数传入正解函数，并输出正解结果：

# 正解 compute_pose = Arm.Algo_Forward_Kinematics(joint) print(f'正解结果：{compute_pose}')

逆解：由位姿转换成机械臂的关节角度的过程。

Algo_Inverse_Kinematics(q_in, q_pose, flag)

q_in为上一时刻的关节角；

q_out为位姿参数，即x,y,z,rx,ry,rz，注意单位，x,y,z的单位是米，rx,ry,rz的单位是弧度；

flag代表姿态参数类别，0为四元数，1为欧拉角。

# 逆解 compute_pose =[0.210,0,0.324,3.141,0,0] res = Arm.Algo_Inverse_Kinematics(joint, compute_pose, 1) print(f'逆解：{res}')

（3）主函数调用

注册回调函数 mcallback (目前I系列不会调用此接口)

if __name__ == "__main__": def mcallback(data): print('0') # 连接机械臂，注册回调函数 callback = CANFD_Callback(mcallback) robot = Arm(RM65, "192.168.1.18", callback) # API版本信息 print(robot.API_Version()) # 运行示例 demo7(robot) time.sleep(10) #给一定时间，容许页面打印出正逆解结果 # 断开连接 robot.RM_API_UnInit() robot.Arm_Socket_Close()

输出结果：

二、不连接机械臂时调用正逆解接口：

1. 在正逆解之前，需要先进行初始化算法依赖，即定义机械臂型号和传感器型号。

2. 需设置机械臂的安装角度和工作坐标系。

以RM65-B为例，进行正逆解接口调用。

# 不连接机械臂的情况下，调用算法接口
dmode = RobotType.RM65
rbt_type = SensorType.B
Arm.Algo_Init_Sys_Data(dmode, rbt_type) #定义机械臂型号为65-B
# 设置算法的安装角度
Arm.Algo_Set_Angle(0, 0, 0)
# 设置算法的工作坐标系
coord_work = FRAME()
coord_work.frame_name.name = "123".encode()
coord_work.pose.position.x = 0
coord_work.pose.position.y = 0
coord_work.pose.position.z = 0
coord_work.pose.euler.rx = 0
coord_work.pose.euler.ry = 0
coord_work.pose.euler.rz = 0
Arm.Algo_Set_WorkFrame(coord_work)
curr_pose = Arm.Algo_Get_Curr_WorkFrame()
print(f"当前工作坐标系：{curr_pose.frame_name.name, curr_pose.pose.position.x, curr_pose.pose.position.y, curr_pose.pose.position.z, curr_pose.pose.euler.rx, curr_pose.pose.euler.ry, curr_pose.pose.euler.rz}")
# 计算正解结果
joint = [0, 0, 90, 0, 90, 0]
compute_pose = Arm.Algo_Forward_Kinematics(joint)
print(f'正解：{compute_pose}')
# 计算逆解
tar = [0.210,0,0.324,3.141,0,0]
q_in = [0, 0, 90, 0, 90, 0]
q_out = Arm.Algo_Inverse_Kinematics(q_in, tar, 1)
print(f'逆解·：{q_out}')
# 断开连接
robot.RM_API_UnInit()
robot.Arm_Socket_Close()