orocos_kdl学习(二):KDL Tree与机器人运动学
KDL(Kinematics and Dynamics Library)中定义了一个树来代表机器人的运动学和动力学参数,ROS中的kdl_parser提供了工具能将机器人描述文件URDF转换为KDL tree.
Kinematic Trees: 链或树形结构。已经有多种方式来定义机构的运动学结构,KDL使用图论中的术语来定义:
- A closed-loop mechanism is a graph, 闭链机构是一幅图
- an open-loop mechanism is a tree, 开链机构是一棵树
- an unbranched tree is a chain. 没有分支的树是一个运动链
KDL Chain和KDL Tree都由最基本的KDL Segments元素串接而成,Segment可以理解为机构运动链上的一个运动部件。如下图所示KDL Segment包含关节KDL Joint 以及部件的质量/惯性属性KDL RigidBodyInertia,并且定义了一个参考坐标系Freference和末端坐标系Ftip
KDL segment
末端到关节坐标系的转换由Ttip描述。在一个运动链或树中,子部件会被添加到父部件的末端,因此上一个部件的Ftip就是下一个部件的参考坐标系Freference (tip frame of parent = reference frame of the child). 通常Fjoint和Freference是重合的,但是也可以存在偏移。
KDL chain
KDL tree
KDL中的定义与URDF中的定义基本是一样的:
也可以参考MATLAB Robotics System Toolbox中的对Rigid Body Tree Robot Model的描述:
Python中创建KDL tree
参考pykdl_utils,pykdl_utils中包含了kdl_parser.py(用于解析URDF文件并将其转换为KDL tree或chain),kdl_kinematics.py(封装了KDL kinematics的一系列函数,使得用Python调用更方便)等实用程序。下面先安装urdfdom_py(Python implementation of the URDF parser):
sudo apt-get install ros-indigo-urdfdom-py
然后在github上下载pykdl_utils的源代码,使用catkin_make进行编译。
convert URDF objects into PyKDL.Tree
首先通过urdf_parser_py来解析URDF文件,有下面几种使用方式:通过xml字符串解析、xml文件解析,以及从ROS 参数服务器获取robot_description字符串信息。
#! /usr/bin/env python # Load the urdf_parser_py manifest, you use your own package
# name on the condition but in this case, you need to depend on
# urdf_parser_py.
import roslib; roslib.load_manifest('urdfdom_py')
import rospy # Import the module from urdf_parser_py.urdf import URDF # 1. Parse a string containing the robot description in URDF.
# Pro: no need to have a roscore running.
# Cons: n/a
# Note: it is rare to receive the robot model as a string.
robot = URDF.from_xml_string("<robot name='myrobot'></robot>") # - OR - # 2. Load the module from a file.
# Pro: no need to have a roscore running.
# Cons: using hardcoded file location is not portable.
robot = URDF.from_xml_file() # - OR - # 3. Load the module from the parameter server.
# Pro: automatic, no arguments are needed, consistent
# with other ROS nodes.
# Cons: need roscore to be running and the parameter to
# to be set beforehand (through a roslaunch file for
# instance).
robot = URDF.from_parameter_server() # Print the robot
print(robot)
下面编写一个简单的robot.urdf文件,创建一个连杆机器人。joint1为与基座link0相连的基关节,joint3为末端关节。
<robot name="test_robot">
<link name="link0" />
<link name="link1" />
<link name="link2" />
<link name="link3" /> <joint name="joint1" type="continuous">
<parent link="link0"/>
<child link="link1"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint> <joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 1" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint> <joint name="joint3" type="continuous">
<parent link="link2"/>
<child link="link3"/>
<origin xyz="0 0 1" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint> </robot>
pykdl_utils中还提供了下列几个指令用于测试分析我们的机器人,如果ROS参数服务器中加载了/robot_description则命令行中的xml文件可以省略:
rosrun pykdl_utils kdl_parser.py [robot.xml]
rosrun pykdl_utils kdl_kinematics.py [robot.xml]
rosrun pykdl_utils joint_kinematics.py [robot.xml]
对于我们上面编写的robot.urdf文件,可以用下面命令进行测试:
rosrun pykdl_utils kdl_parser.py `rospack find test`/robot.urdf
下面是KDL运动学一些基本的用法,相关函数可以参考:KDLKinematics Class Reference
#! /usr/bin/env python # Import the module
from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
from pykdl_utils.kdl_kinematics import KDLKinematics robot = URDF.from_xml_file("/home/sc/catkin_ws/src/test/robot.urdf") tree = kdl_tree_from_urdf_model(robot)
print tree.getNrOfSegments() chain = tree.getChain("link0", "link3")
print chain.getNrOfJoints() # forwawrd kinematics
kdl_kin = KDLKinematics(robot, "link0", "link3")
q = [0, 0, 0]
pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 matrix)
print pose q_ik = kdl_kin.inverse(pose) # inverse kinematics
print q_ik if q_ik is not None:
pose_sol = kdl_kin.forward(q_ik) # should equal pose
print pose_sol J = kdl_kin.jacobian(q)
print 'J:', J
我们将URDF文件转换为KDL tree以后可以获取机构运动链/树的相关信息。KDLKinematics的构造函数根据urdf文件,以及机器人的基座base_link和末端end_link就可以创建出运动链:
def pykdl_utils.kdl_kinematics.KDLKinematics.__init__ (self, urdf, base_link, end_link, kdl_tree = None) # Parameters:
# urdf URDF object of robot.
# base_link Name of the root link of the kinematic chain.
# end_link Name of the end link of the kinematic chain.
# kdl_tree Optional KDL.Tree object to use. If None, one will be generated from the URDF.
正运动学的计算函数forward参数就是关节角度;逆运动学计算函数inverse的参数为末端位姿矩阵,因为是数值解,还可以指定初始值,以及关节角的范围。
# Inverse kinematics for a given pose, returning the joint angles required to obtain the target pose.
def pykdl_utils.kdl_kinematics.KDLKinematics.inverse(self, pose, q_guess = None, min_joints = None, max_joints = None )
# Parameters:
# pose Pose-like object represeting the target pose of the end effector.
# q_guess List of joint angles to seed the IK search.
# min_joints List of joint angles to lower bound the angles on the IK search. If None, the safety limits are used.
# max_joints List of joint angles to upper bound the angles on the IK search. If None, the safety limits are used.
C++中创建KDL tree
为了使用KDL parser需要在package.xml中添加相关依赖项:
<package>
...
<build_depend package="kdl_parser" />
...
<run_depend package="kdl_parser" />
...
</package>
另外还需要在C++程序中加入相关的头文件:
#include <kdl_parser/kdl_parser.hpp>
下面有多种从urdf创建KDL tree的方式:
1. From a file
KDL::Tree my_tree;
if (!kdl_parser::treeFromFile("filename", my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
2. From the parameter server
KDL::Tree my_tree;
ros::NodeHandle node;
std::string robot_desc_string;
node.param("robot_description", robot_desc_string, std::string());
if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
3. From an xml element
KDL::Tree my_tree;
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
xml_root = xml_doc.FirstChildElement("robot");
if (!xml_root){
ROS_ERROR("Failed to get robot from xml document");
return false;
}
if (!kdl_parser::treeFromXml(xml_root, my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
4. From a URDF model
KDL::Tree my_tree;
urdf::Model my_model;
if (!my_model.initXml(....)){
ROS_ERROR("Failed to parse urdf robot model");
return false;
}
if (!kdl_parser::treeFromUrdfModel(my_model, my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
参考:
Robotics System Toolbox—Rigid Body Tree Robot Model
orocos_kdl学习(二):KDL Tree与机器人运动学的更多相关文章
- TRAC-IK机器人运动学求解器
TRAC-IK和Orocos KDL类似,也是一种基于数值解的机器人运动学求解器,但是在算法层面上进行了很多改进(Specifically, KDL’s convergence algorithms ...
- emberjs学习二(ember-data和localstorage_adapter)
emberjs学习二(ember-data和localstorage_adapter) 准备工作 首先我们加入ember-data和ember-localstorage-adapter两个依赖项,使用 ...
- 集成学习二: Boosting
目录 集成学习二: Boosting 引言 Adaboost Adaboost 算法 前向分步算法 前向分步算法 Boosting Tree 回归树 提升回归树 Gradient Boosting 参 ...
- Android JNI学习(二)——实战JNI之“hello world”
本系列文章如下: Android JNI(一)——NDK与JNI基础 Android JNI学习(二)——实战JNI之“hello world” Android JNI学习(三)——Java与Nati ...
- ReactJS入门学习二
ReactJS入门学习二 阅读目录 React的背景和基本原理 理解React.render() 什么是JSX? 为什么要使用JSX? JSX的语法 如何在JSX中如何使用事件 如何在JSX中如何使用 ...
- TweenMax动画库学习(二)
目录 TweenMax动画库学习(一) TweenMax动画库学习(二) TweenMax动画库学习(三) Tw ...
- Hbase深入学习(二) 安装hbase
Hbase深入学习(二) 安装hbase This guidedescribes setup of a standalone hbase instance that uses the local fi ...
- Struts2框架学习(二) Action
Struts2框架学习(二) Action Struts2框架中的Action类是一个单独的javabean对象.不像Struts1中还要去继承HttpServlet,耦合度减小了. 1,流程 拦截器 ...
- Python学习二:词典基础详解
作者:NiceCui 本文谢绝转载,如需转载需征得作者本人同意,谢谢. 本文链接:http://www.cnblogs.com/NiceCui/p/7862377.html 邮箱:moyi@moyib ...
随机推荐
- bootstrap——辅助类和响应式工具类
<!DOCTYPE html> <html lang="en"> <head> <meta charset="UTF-8&quo ...
- C#泛型(一)
简介: 先看看泛型的概念--“通过参数化类型来实现在同一份代码上操作多种数据类型.利用“参数化类型”将类型抽象化,从而实现灵活的复用”. 很多初学者在刚开始接触泛型的时候会比较难理解 “泛型” 在这里 ...
- requirejs模块路径配置问题
三种情况:一.设置data-main,没配置baseUrl,以data-main的文件为基准:二.设置data-main,配置baseUrl,baseUrl以值以引用require.js的HTML为基 ...
- 序列操作bzoj2962(未完成)
题解: 注意到k<=20;记录20个数f1...f20表示从选1-20个数的乘积分别为多少, 那么是一个(…+x)(…+x)……(…+x) 拆括号,枚举含有j个x,那么就是xj∗f[i−j]∗C ...
- NFS服务自动搭建及挂载脚本
一.写脚本的动机 由于最近老是搭建NFS,虽然不复杂,但是很繁琐.安装服务.修改配置文件.手动挂载.写入开机自动挂载等于是就写了一个脚本 二.脚本说明及审明 作用:该脚本主要实现NFS自动安装,客户端 ...
- Codeforces 490F Treeland Tour 树形dp
Treeland Tour 离散化之后, 每个节点维护上升链和下降链, 感觉复杂度有点高, 为啥跑这么快.. #include<bits/stdc++.h> #define LL long ...
- BZOJ1078 [SCOI2008]斜堆 堆
欢迎访问~原文出处——博客园-zhouzhendong 去博客园看该题解 题目传送门 - BZOJ1078 题意概括 斜堆(skew heap)是一种常用的数据结构.它也是二叉树,且满足与二叉堆相同的 ...
- Unity Standard Assets Example Project
参考链接:http://blog.csdn.net/jaikydota163/article/details/52751976
- 用VScode来编写C / C ++代码
Microsoft C / C ++扩展提供了对Visual Studio Code的C / C ++支持,以便在Windows,Linux和macOS上使用VS Code实现跨平台C和C ++开发. ...
- UVa140 Bandwidth 【最优性剪枝】
题目链接:https://vjudge.net/contest/210334#problem/F 转载于:https://www.cnblogs.com/luruiyuan/p/5847706.ht ...