PyKDL---正运动学和逆运动学

正运动学
逆运动学
完整代码

串联机械臂运用过程中常用到正逆运动学，正运动学指的是已知关节空间变量关节角或角速度，求取操作空间的位置或速度；逆运动学是指已知操作空间的位姿或速度，求取关节空间的关节角或关节速度。

正运动学

首先需要获取运动链，获取方法可以参照我写的PyKDL—运动段、运动链和运动树,本文不在介绍。
运动学部分需要明白一个特点，输入参数也会输出参数，应用了Python的引用机制，对于串联机器人，运动学都是基于运动链完成，都需要输入运动链。

#正运动学
fk = PyKDL.ChainFkSolverPos_recursive(chain)pos = PyKDL.Frame()
q = PyKDL.JntArray(7)
for i in range(7):q[i] = 0
q[0] = 1
fk_flag = fk.JntToCart(q, pos)
print "fk_flag", fk_flag
print "pos", pos

逆运动学

KDL库的逆运动学是数值解法求取的，有多种数值解法，相关解法原理可能在后面我也会写，本文给出其中一种。

#逆运动学
ik_v = PyKDL.ChainIkSolverVel_pinv(chain)
ik = PyKDL.ChainIkSolverPos_NR(chain, fk, ik_v, maxiter=100, eps=math.pow(10, -9))qq = PyKDL.JntArray(7)
qq_k = PyKDL.JntArray(7)
ik.CartToJnt(qq_k, pos, qq)
print "qq:", qq

完整代码

#!/usr/bin/env python
#-*- coding:utf-8 -*-import mathimport PyKDLfrom urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_modelrobot = URDF.from_xml_file("/home/d/catkin_ws/src/robot_description/armc_description/urdf/armc_description.urdf")
tree = kdl_tree_from_urdf_model(robot)print tree.getNrOfSegments()
chain = tree.getChain("base_link", "sensor_link")
print chain.getNrOfSegments()
print chain.getNrOfJoints()#正运动学
fk = PyKDL.ChainFkSolverPos_recursive(chain)pos = PyKDL.Frame()
q = PyKDL.JntArray(7)
for i in range(7):q[i] = 0
q[0] = 1
fk_flag = fk.JntToCart(q, pos)
print "fk_flag", fk_flag
print "pos", pos#逆运动学
ik_v = PyKDL.ChainIkSolverVel_pinv(chain)
ik = PyKDL.ChainIkSolverPos_NR(chain, fk, ik_v, maxiter=100, eps=math.pow(10, -9))qq = PyKDL.JntArray(7)
qq_k = PyKDL.JntArray(7)
ik.CartToJnt(qq_k, pos, qq)
print "qq:", qq

也可参考另一个博客PyKDL程序,代码如下

#coding=utf-8
import numpy as np
import PyKDL as kdl
# import copy #用于深度拷贝# 注意一些基本概念
# Segment初始化由绕的轴和末端姿态来生成，可能跟书上的D-H方法有点不一样，区别在于用末端和轴来定义一个关节
#   轴是在当前关节的坐标系中绕轴V旋转，默认设置为kdl.Joint.RotZ
#   f_tip，末端在当前坐标系中的姿态jnts = []
frms = []for i in range(6):jnts.append(kdl.Joint(kdl.Joint.RotZ))# 注意，这个框架是f_tip，末端
frm1 = kdl.Frame(kdl.Rotation.RotX(-np.pi/2), kdl.Vector(0, 0, 239.5))
frm2 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(250, 0, 0))
frm3 = kdl.Frame(kdl.Rotation.RotX(np.pi/2), kdl.Vector(0, 262, 0))
frm4 = kdl.Frame(kdl.Rotation.RotX(-np.pi/2), kdl.Vector(0, 0, 0))
frm5 = kdl.Frame(kdl.Rotation.RotX(np.pi/2), kdl.Vector(0, 0, 0))
frm6 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(-168, 0, 0))frms.append(frm1)
frms.append(frm2)
frms.append(frm3)
frms.append(frm4)
frms.append(frm5)
frms.append(frm6)rbt = kdl.Chain() #建立机器人对象link = []
for i in range(6):link.append(kdl.Segment(jnts[i], frms[i]))rbt.addSegment(link[i])fk = kdl.ChainFkSolverPos_recursive(rbt)p = kdl.Frame()
q = kdl.JntArray(6)
q[0] = 0
q[1] = 0
q[2] = 0
q[3] = 0
q[4] = 0
q[5] = 0fk.JntToCart(q, p)print(p)

PyKDL---正运动学和逆运动学相关推荐

正运动学与逆运动学区别
正运动学与逆运动学区别正运动学逆运动学正运动学已知连杆几何参数(长度)和关节变量(旋转角度),求解末端执行器(机械夹爪)相对于参考坐标系的位置(平移量)和姿态(旋转量). 此过程为:建立 ...
【机器人学】3-RUU-delta并联机器人正运动学、逆运动学和微分运动学
文章目录串联和并联机器人对比 delta机器人逆运动学正运动学微分运动学工作空间串联和并联机器人对比串联机器人和并联机器人各有优缺点. 串联机器人优点工作空间大: 可实现的姿态多: ...
正运动学及逆运动学求解方法
正运动学:给定机器人关节变量的取值来确定末端执行器的位置和姿态. 求解方法:D-H法和旋量理论法逆运动学:根据给定的末端执行器的位置和姿态来确定机器人关节变量的取值. 求解方法:解析解法.数值解法. ...
【机器人学】基于PoE模型的串联机械臂UR5的正运动学、微分运动学和逆运动学
文章目录基本概念正运动学源码一阶运动学基本概念 PoE(Product of Exponential)指数积公式. PoE和DH的作用都是一样的. 实际使用过程中,绝大多数在售的机器人还都是 ...
matlab机器人雅可比矩阵实验,机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解（工具：matlab）...
1.利用工具箱建模,供验证用选用二轴平面机械臂,两个关节角分别是theta1和theta2. view(3); Lnk1 = Link([ 0 0 100 0 0]);%theta,d,a,alph ...
机器人学基础（1）-位置运动学-正运动学、逆运动学方程建立及其求解
机器人学基础(1)位置运动学:正运动学.逆运动学方程建立及其求解机器人学基础学了个知识框架入门,以此来写一下总结笔记,便于以后要用到相关知识点进行翻阅. 本次机器人学基础笔记主要分为几个章节:位置运 ...
五杆机构正运动学公式、逆运动学公式MATLAB代码
在研究五杆机构的时候感觉网上的资料更多的在研究正运动学,包括机构顶端的运动轨迹.速度.加速度,缺少逆运动学的分析.当我们想用五杆机构做实际用途比如3D打印或轨迹描绘的时候逆运动学公式是必不可少的.所以 ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
机械臂正逆运动学-----数值解
机械臂正逆运动学-----数值解建立DH坐标系求正运动学单关节齐次传递矩阵正运动学:返回齐次矩阵正运动学:返回欧拉角向量求雅可比矩阵求机械臂逆运动学合成通用运动学类机械臂的运动学包括 ...

PyKDL---正运动学和逆运动学

PyKDL---正运动学和逆运动学

正运动学

逆运动学

完整代码

PyKDL---正运动学和逆运动学相关推荐

最新文章

热门文章