Orocos KDL的安装与使用

Orocos KDL的安装与使用

1. 安装

1.1 克隆KDL的仓库

1
git clone https://github.com/orocos/orocos_kinematics_dynamics.git

1.2 编译以及安装

进入orocos_kinematics_dynamics下的orocos_kdl文件夹，创建build文件夹，然后进入build文件夹：

1
2
3
cd orocos_kinematics_dynamics/orocos_kdl
mkdir build
cd build

使用cmake编译：

1
2
cmake ..
make

安装：

1
sudo make install
这里的安装路径是默认的/usr/local/include。

1.3 配置CMakeLists.txt

在CMakeLists.txt中添加以下内容：

1
2
find_package(orocos_kdl REQUIRED)
include_directories(${orocos_kdl_INCLUDE_DIRS}) # 添加头文件路径，其实这一步可以不用做，在源代码中直接引用头文件即可

在target_link_libraries()中添加${orocos_kdl_LIBRARIES}

1
2
3
4
target_link_libraries(${PROJECT_NAME} # 项目名
  ${catkin_LIBRARIES}
  ${orocos_kdl_LIBRARIES}
)

有的博客中链接的是orocos-kdl orocos-kdl-models，这里visp_ros仿真环境中使用的是${orocos_kdl_LIBRARIES}。

2. 使用

2.1 引用头文件

1
2
3
4
5
6
7
8
9
10
11
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/solveri.hpp>

2.2 创建机器人模型

初始化KDL的chain需要机械臂的DH参数，这里以franka机械臂为例，其DH参数可以在官方文档中找到。

我们首先定义好相关变量：

1
2
3
4
KDL::Chain chain_; // 机械臂的KDL模型
KDL::ChainFkSolverPos_recursive* fksolver_; // 正运动学求解器
KDL::ChainJntToJacSolver* jacobian_solver_; // 雅克比矩阵求解器
KDL::ChainDynParam* dyn_param_solver_; // 动力学参数求解器

通过franka机械臂的DH参数初始化机械臂的KDL模型，调用addSegment()函数添加每个关节的DH参数。(这里是从visp_ros中的vpRobotFrankaSim类中获取的DH参数，实际使用时可能需要根据自己的机械臂进行修改)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
chain_.addSegment(
    KDL::Segment( KDL::Joint( KDL::Joint::None ), KDL::Frame::DH_Craig1989( 0.0, 0.0, 0.333, 0.0 ) ) );
chain_.addSegment(
    KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0, -M_PI_2, 0.0, 0.0 ) ) );
chain_.addSegment(
    KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0, M_PI_2, 0.316, 0.0 ) ) );
chain_.addSegment(
    KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0825, M_PI_2, 0.0, 0.0 ) ) );
chain_.addSegment(
    KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( -0.0825, -M_PI_2, 0.384, 0.0 ) ) );
chain_.addSegment(
    KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0, M_PI_2, 0.0, 0.0 ) ) );
chain_.addSegment(
    KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.088, M_PI_2, 0.0, 0.0 ) ) );
chain_.addSegment(
    KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0, 0.0, 0.107, 0.0 ) ) );

初始化机械臂的正运动学求解器、雅克比矩阵求解器和动力学参数求解器：

1
2
3
fksolver_ = new KDL::ChainFkSolverPos_recursive( chain_ );
jacobian_solver_ = new KDL::ChainJntToJacSolver( chain_ );
dyn_param_solver_ = new KDL::ChainDynParam( chain_, KDL::Vector( 0.0, 0.0, -9.81 ) );

2.3 正运动学求解

通过机械臂的正运动学求解器求解机械臂的末端位姿：

1
2
3
KDL::JntArray q(7); // 机械臂的关节角度
KDL::Frame p_out; // 机械臂的末端位姿
fksolver_->JntToCart( q, p_out ); // 调用正运动学求解器求解机械臂的末端位姿
这里的q是机械臂的关节角度，p_out是机械臂的末端位姿。使用时需要将机械臂的关节角度赋值给q，然后调用fksolver_->JntToCart( q, p_out )即可得到机械臂的末端位姿。

p_out是一个4x4的矩阵，可以通过p_out.M和p_out.p分别获取旋转矩阵和平移向量。而机械臂的关节角度q一般可以通过机械臂的驱动器获取。

2.4 雅克比矩阵求解

通过机械臂的雅克比矩阵求解器求解机械臂的雅克比矩阵：

1
2
3
KDL::JntArray q(7); // 机械臂的关节角度
KDL::Jacobian J; // 机械臂的雅克比矩阵
jacobian_solver_->JntToJac( q, J );
可以看到，我们只需要得到机械臂的关节角度q，然后调用jacobian_solver_->JntToJac( q, J )即可得到机械臂的雅克比矩阵J，这里的J是一个6x7的矩阵，可以通过J.data获取雅克比矩阵的数据，或者直接访问J(i,j)获取雅克比矩阵的第i行第j列的元素。

JntToJac()函数的第一个参数是机械臂的关节角度，第二个参数是机械臂的雅克比矩阵，第三个参数segmentNR是要计算雅克比矩阵的末端关节的编号，默认为机械臂的末端关节(-1)，如果需要计算其他关节的雅克比矩阵，可以指定segmentNR。官方给出的函数原型如下：

1
2
3
4
5
6
7
8
9
10
11
12
/**
* Calculate the jacobian expressed in the base frame of the
* chain, with reference point at the end effector of the
* *chain. The alghoritm is similar to the one used in
* KDL::ChainFkSolverVel_recursive
*
* @param q_in input joint positions
* @param jac output jacobian
*
* @return success/error code
*/
virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1);

2.5 动力学参数求解

通过机械臂的动力学参数求解器求解机械臂的动力学参数：

1
2
3
4
5
6
KDL::JntArray q(7); // 机械臂的关节角度
KDL::JntArray q_dot(7); // 机械臂的关节速度
KDL::JntArray coriolis(7); // 机械臂的科里奥利力矩
KDL::JntSpaceInertiaMatrix mass(7); // 机械臂的惯性力矩
dyn_param_solver_->JntToCoriolis( q, q_dot, coriolis ); // 调用动力学参数求解器求解机械臂的科里奥利力矩
dyn_param_solver_->JntToMass( q, mass ); // 调用动力学参数求解器求解机械臂的惯性力矩

计算机械臂的动力学参数时，需要机械臂的关节角度q和关节速度q_dot，然后调用dyn_param_solver_->JntToCoriolis( q, q_dot, coriolis )即可得到机械臂的科里奥利力矩coriolis，调用dyn_param_solver_->JntToMass( q, mass )即可得到机械臂的惯性力矩mass。这里的coriolis和mass都是7维的向量，可以通过coriolis.data和mass.data获取数据，或者直接访问coriolis(i)和mass(i)获取第i个关节的科里奥利力矩和惯性力矩。

在仿真环境中测试时，发现通过这样的方式求解的科里奥利力矩和惯性力矩是空的，暂时还不清楚原因。

函数原型如下：

1
2
3
virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis);
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H);
virtual int JntToGravity(const JntArray &q,JntArray &gravity);

最后不要忘记释放内存：

1
2
3
delete fksolver_;
delete jacobian_solver_;
delete dyn_param_solver_;

3. 参考资料

控制
#KDL
Orocos KDL的安装与使用
https://symcreg.github.io/2025/01/17/Orocos-KDL的安装与使用/
作者
sam
发布于
2025年1月17日
许可协议