orocos_kdl学习(一):坐标系变换
KDL中提供了点(point)、坐标系(frame)、刚体速度(twist),以及6维力/力矩(wrench)等基本几何元素,具体可以参考 Geometric primitives 文档。
Creating a Frame, Vector and Rotation
PyKDL中创建一个坐标系时有下面4种构造函数:
__init__() # Construct an identity frame __init__(rot, pos) # Construct a frame from a rotation and a vector
# Parameters:
# pos (Vector) – the position of the frame origin
# rot (Rotation) – the rotation of the frame __init__(pos) # Construct a frame from a vector, with identity rotation
# Parameters:
# pos (Vector) – the position of the frame origin __init__(rot) # Construct a frame from a rotation, with origin at 0, 0, 0
# Parameters:
# rot (Rotation) – the rotation of the frame
下面是一个例子:
#! /usr/bin/env python import PyKDL # create a vector which describes both a 3D vector and a 3D point in space
v = PyKDL.Vector(1,3,5) # create a rotation from Roll Pitch, Yaw angles
r1 = PyKDL.Rotation.RPY(1.2, 3.4, 0) # create a rotation from ZYX Euler angles
r2 = PyKDL.Rotation.EulerZYX(0, 1, 0) # create a rotation from a rotation matrix
r3 = PyKDL.Rotation(1,0,0, 0,1,0, 0,0,1) # create a frame from a vector and a rotation
f = PyKDL.Frame(r2, v) print f
结果将如下所示,前9个元素为代表坐标系姿态的旋转矩阵,后三个元素为坐标系f的原点在参考坐标系中的坐标。
[[ 0.540302, 0, 0.841471;
0, 1, 0;
-0.841471, 0, 0.540302]
[ 1, 3, 5]]
根据机器人学导论(Introduction to Robotics Mechanics and Control)附录中的12种欧拉角表示方法,ZYX欧拉角代表的旋转矩阵为:
代入数据验证,KDL的计算与理论一致。
Extracting information from a Frame, Vector and Rotation
PyKDL中坐标系类Frame的成员M为其旋转矩阵,p为其原点坐标。
#! /usr/bin/env python import PyKDL # frame
f = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
PyKDL.Vector(3,2,4)) # get the origin (a Vector) of the frame
origin = f.p # get the x component of the origin
x = origin.x()
x = origin[0]
print x # get the rotation of the frame
rot = f.M
print rot # get ZYX Euler angles from the rotation
[Rz, Ry, Rx] = rot.GetEulerZYX()
print Rz,Ry,Rx # get the RPY (fixed axis) from the rotation
[R, P, Y] = rot.GetRPY()
print R,P,Y
将上述获取到的数据打印出来,结果如下:
3.0
[ 0.540302, 0, 0.841471;
0, 1, 0;
-0.841471, 0, 0.540302]
0.0 1.0 0.0
0.0 1.0 0.0
注意GetEulerZYX和GetRPY的结果是一样的,这其实不是巧合。因为坐标系的旋转有24种方式(本质上只有12种结果):绕自身坐标系旋转有12种方式(欧拉角),绕固定参考坐标系旋转也有12种方式(RPY就是其中一种)。EulerZYX按照Z→Y→X的顺序绕自身坐标轴分别旋转$\alpha$、$\beta$、$\gamma$角;RPY按照X→Y→Z的顺序绕固定坐标系旋转$\gamma$、$\beta$、$\alpha$角,这两种方式最后得到的旋转矩阵是一样的。
Transforming a point
frame既可以用来描述一个坐标系的位置和姿态,也可以用于变换。下面创建了一个frame,然后用其对一个空间向量或点进行坐标变换:
#! /usr/bin/env python import PyKDL # define a frame
f = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
PyKDL.Vector(3,2,4)) # define a point
p = PyKDL.Vector(1, 0, 0)
print p # transform this point with f
p = f * p
print p
Creating from ROS types
tf_conversions这个package包含了一系列转换函数,用于将tf类型的数据(point, vector, pose, etc) 转换为与其它库同类型的数据,比如KDL和Eigen。用下面的命令在catkin_ws/src中创建一个测试包:
catkin_create_pkg test rospy tf geometry_msgs
test.py程序如下(注意修改权限chmod +x test.py):
#! /usr/bin/env python
import rospy
import PyKDL
from tf_conversions import posemath
from geometry_msgs.msg import Pose # you have a Pose message
pose = Pose() pose.position.x = 1
pose.position.y = 1
pose.position.z = 1
pose.orientation.x = pose.orientation.y = pose.orientation.z = 0
pose.orientation.w = 1 # convert the pose into a kdl frame
f1 = posemath.fromMsg(pose) # create another kdl frame
f2 = PyKDL.Frame(PyKDL.Rotation.RPY(0,1,0),
PyKDL.Vector(3,2,4)) # Combine the two frames
f = f1 * f2
print f [x, y, z, w] = f.M.GetQuaternion()
print x,y,z,w # and convert the result back to a pose message
pose = posemath.toMsg(f) pub = rospy.Publisher('pose', Pose, queue_size=1)
rospy.init_node('test', anonymous=True)
rate = rospy.Rate(1) # 1hz while not rospy.is_shutdown():
pub.publish(pose)
rate.sleep()
通过posemath.toMsg可以将KDL中的frame转换为geometry_msgs/Pose类型的消息。最后为了验证,创建了一个Publisher将这个消息发布到pose话题上。使用catkin_make编译后运行结果如下:
参考:
Orocos Kinematics and Dynamics
kdl / Tutorials / Frame transformations (Python)
orocos_kdl学习(一):坐标系变换的更多相关文章
- 5. svg学习笔记-坐标系变换
之前我们编写图形元素的时候,编写好了位置大小就是固定的,通过坐标系变换,可以移动缩放,旋转图形,但必须声明的是,进行变换时是图形相对于坐标系的变化,就是图形是不发生变化的,而是坐标系发生了变化,比如缩 ...
- ROS Learning-014 learning_tf(编程) 坐标系变换(tf)广播员 (Python版)
ROS Indigo learning_tf-01 坐标系变换(tf)广播员 (Python版) 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu ...
- 【转】QPainter中坐标系变换问题
转自:http://blog.sina.com.cn/s/blog_67cf08270100ww0p.html 一.坐标系简介. Qt中每一个窗口都有一个坐标系,默认的,窗口左上角为坐标原点,然后水平 ...
- BZOJ3210: 花神的浇花集会(坐标系变换)
题面 传送门 题解 坐标系变换把切比雪夫距离转化为曼哈顿距离 那么对于所有的\(x\)坐标中,肯定是中位数最优了,\(y\)坐标同理 然而有可能这个新的点不合法,也就是说不存在\((x+y,x-y)\ ...
- OSG数学基础:坐标系变换
三维实体对象需要经过一系列的坐标变换才能正确.真实地显示在屏幕上.在一个场景中,当读者对场景中的物体进行各种变换及相关操作时,坐标系变换是非常频繁的. 坐标系变换通常包括:世界坐标系-物体坐标系变换. ...
- CSS学习笔记2-2d变换和过渡属性
前言:今天又是一个周末,心情不错,趁着闲暇之余,把剩下来的CSS3学习的内容全部整理出来,练习用的源码也稍微整理了一下. 2D转换 transform:translate||rotate||scale ...
- svg坐标系变换
svg的坐标变换有三个属性来决定:viewport, viewBox, 和 preserveAspectRatio,我发现三篇比较详细的博客,转载如下: 理解SVG坐标系和变换:视窗,viewBox和 ...
- orocos_kdl学习(二):KDL Tree与机器人运动学
KDL(Kinematics and Dynamics Library)中定义了一个树来代表机器人的运动学和动力学参数,ROS中的kdl_parser提供了工具能将机器人描述文件URDF转换为KDL ...
- 洛谷P3964 [TJOI2013]松鼠聚会(坐标系变换)
题面 传送门 题解 对于两个点\((x_i,y_i)\)和\(x_j,y_j\),我们定义它们之间的曼哈顿距离为 \[|x_i-x_j|+|y_i-y_j|\] 定义它们的切比雪夫距离为 \[\max ...
随机推荐
- RDLC 主从报表筛选
今天继续学习RDLC报表的“参数传递”及“主从报表” 一.先创建DataSet,如下图: 二.创建一个报表rptDEPT.rdlc,显示部门T_DPET的数据 三.嵌入Default.aspx中,写在 ...
- Web前端开发最佳实践(13):前端页面卡顿?可能是DOM操作惹的祸,你需要优化代码
文档对象模型(DOM)是一个独立于特定语言的应用程序接口.在浏览器中,DOM接口是以JavaScript语言实现的,通过JavaScript来操作浏览器页面中的元素,这使得DOM成为了JavaScri ...
- BZOJ1071 [SCOI2007]压缩 其他
欢迎访问~原文出处——博客园-zhouzhendong 去博客园看该题解 题目传送门 - BZOJ1071 题意概括 有两个序列a[1..n], b[1..n],其编号为1..n,设为s序列.现在我们 ...
- [OpenCV-Python] OpenCV 中的图像处理 部分 IV (五)
部分 IVOpenCV 中的图像处理 OpenCV-Python 中文教程(搬运)目录 22 直方图 22.1 直方图的计算,绘制与分析目标 • 使用 OpenCV 或 Numpy 函数计算直方图 • ...
- phpunit
教程及文档: https://www.jianshu.com/p/abcca5aa3ad6 http://www.phpunit.cn/manual/current/zh_cn/phpunit-boo ...
- 解决html5中video标签无法播放mp4问题的办法
这篇文章主要给大家介绍了关于解决html5中video标签无法播放mp4问题的办法,文中介绍的非常详细,相信会对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面来一起看看吧. 最近发现了一个 ...
- C6000 CSL 函数说明
转自:http://bbs.21ic.com/icview-741800-1-1.html 先来看一个例子 代码1 CSL_FINST(osdRegs->VIDWINMD, OSD_VIDWIN ...
- GCC&&GDB在OI中的介绍
序言 这本来是用Word写的,但是后来我换了系统所以只能用markdown迁移然后写了...... $\qquad$本文主要投食给那些在Windows下活了很久然后考试时发现需要用命令行来操作时困惑万 ...
- GIF录制
韩梦飞沙 韩亚飞 313134555@qq.com yue31313 han_meng_fei_sha ============= 快手电脑版_快手_gif快手电脑版 GIF动画录制工具|GI ...
- Java并发程序设计(五)JDK并发包之线程复用:线程池
线程复用:线程池 一.为什么需要线程池 为了避免系统频繁地创建和销毁线程,使用线程池让线程进行复用.(即创建线程变成了从线程池中获取空闲线程,销毁线程变成了把线程放回线程池中.) 二.JDK对线程池的 ...