1.python3.6安装PyKDL

pip install PyKDL不行,import的时候失败了,只能源码编译

GitHub - orocos/orocos_kinematics_dynamics: Orocos Kinematics and Dynamics C++ library

具体安装可以参考:ROS编译PyKDL python3_RuiH.AI的博客-CSDN博客

其中pybind11这个下载的时候比较慢

git submodule update --init

可以用下面的代替,把pybind11下载下来.

cd orocos_kinematics_dynamics/python_orocos_kdl
git clone https://github.com/pybind/pybind11.git

make install以后,import PyKDL还是会报错

ImportError: dynamic module does not define module export function (PyInit_PyKDL)

只有在build下可以import PyKDL

cd /orocos_kinematics_dynamics/python_orocos_kdl/build
python
import PyKDL

可能是找不到什么动态连接库或者什么的问题的吧??总之先在build目录下运行.

2.PyKDL求panda机械臂逆运动学

 

#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
@Project ：robotic_obstacle_planning
@File ：pyKDL.py
@Author ：jintianlei
@Date : 2023/7/27
"""
import PyKDL
import mathdef create_panda_chain():# DH参数a = [0.0, 0.0, 0.0, 0.0825, -0.0825, 0.0, 0.088]alpha = [0.0, -math.pi / 2, math.pi / 2, math.pi / 2, -math.pi / 2, math.pi / 2, math.pi / 2]d = [0.333, 0.0, 0.316, 0.0, 0.384, 0.0, 0.107]theta = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]chain = PyKDL.Chain()for i in range(7):chain.addSegment(PyKDL.Segment(PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame( PyKDL.Rotation.RotZ(theta[i]) * PyKDL.Rotation.RotX(alpha[i]), PyKDL.Vector(a[i], -d[i] * math.sin(alpha[i]), d[i] * math.cos(alpha[i])) )))return chaindef compute_inverse_kinematics(chain, target_pose):'''正运动学'''fk = PyKDL.ChainFkSolverPos_recursive(chain)pos = PyKDL.Frame()q = PyKDL.JntArray(7)qq = [-0.917812, -0.917812, 43.2983, 21.2432, 16.8387, -27.4167, 19.5677]for i in range(7):q[i] = qq[i]fk_flag = fk.JntToCart(q, pos)print("fk_flag", fk_flag)print("pos", pos)'''逆运动学'''ikv = PyKDL.ChainIkSolverVel_pinv(chain)ik = PyKDL.ChainIkSolverPos_NR(chain, fk, ikv)# 创建目标位姿target_frame = PyKDL.Frame(PyKDL.Rotation.RPY(target_pose[3], target_pose[4], target_pose[5]), PyKDL.Vector(target_pose[0], target_pose[1], target_pose[2]))# 创建起始关节角度initial_joint_angles = PyKDL.JntArray(chain.getNrOfJoints())result = PyKDL.JntArray(chain.getNrOfJoints())#print(target_frame)# 调用逆运动学求解器ik.CartToJnt(initial_joint_angles, target_frame,result)print('result: ',result)return resultif __name__=="__main__":# 创建机器人链chain = create_panda_chain()# 设置目标位姿target_pose = [0.5, 0.3, 0.4, 0.1, 0.0, 0.0]# 调用逆运动学求解函数joint_angles = compute_inverse_kinematics(chain, target_pose)print("关节角度: ", joint_angles)