def treeFromUrdfModel(robot_model, quiet=False):
"""
Construct a PyKDL.Tree from an URDF model from urdf_parser_python.
:param robot_model: URDF xml string, ``str``
:param quiet: If true suppress messages to stdout, ``bool``
"""
root = robot_model.link_map[robot_model.get_root()]
if root.inertial and not quiet:
print(
"The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF."
% root.name)
ok = True
tree = kdl.Tree(root.name)
# add all children
for (joint, child) in robot_model.child_map[root.name]:
if not _add_children_to_tree(robot_model, robot_model.link_map[child],
tree):
ok = False
break
return (ok, tree)
def kdl_tree_from_urdf_model(urdf):
root = urdf.get_root()
tree = kdl.Tree(root)
def add_children_to_tree(parent):
if parent in urdf.child_map:
for joint, child_name in urdf.child_map[parent]:
for lidx, link in enumerate(urdf.links):
if child_name == link.name:
child = urdf.links[lidx]
if child.inertial is not None:
kdl_inert = urdf_inertial_to_kdl_rbi(
child.inertial)
else:
kdl_inert = kdl.RigidBodyInertia()
for jidx, jnt in enumerate(urdf.joints):
if jnt.name == joint:
kdl_jnt = urdf_joint_to_kdl_joint(
urdf.joints[jidx])
kdl_origin = urdf_pose_to_kdl_frame(
urdf.joints[jidx].origin)
kdl_sgm = kdl.Segment(child_name, kdl_jnt,
kdl_origin, kdl_inert)
tree.addSegment(kdl_sgm, parent)
add_children_to_tree(child_name)
add_children_to_tree(root)
return tree
def kdl_tree_from_urdf_model(self, urdf, js_inactive_names_vector, js_pos):
segment_map = {}
segment_id = 0
segment_name_id_map = {}
segment_parent_map = {}
root = urdf.get_root()
tree = PyKDL.Tree(root)
segment_map[segment_id] = None
segment_parent_map[segment_id] = None
segment_name_id_map[root] = segment_id
segment_id += 1
def add_children_to_tree(parent, segment_id):
if parent in urdf.child_map:
for joint, child_name in urdf.child_map[parent]:
if joint in js_inactive_names_vector:
print "setting as fixed:", joint, js_pos[joint]
joint_rot = -js_pos[joint]
urdf.joint_map[joint].joint_type = 'fixed'
else:
joint_rot = 0.0
child = urdf.link_map[child_name]
if child.inertial is not None:
kdl_inert = kdl_urdf.urdf_inertial_to_kdl_rbi(
child.inertial)
else:
kdl_inert = PyKDL.RigidBodyInertia()
kdl_jnt = kdl_urdf.urdf_joint_to_kdl_joint(
urdf.joint_map[joint])
kdl_origin = kdl_urdf.urdf_pose_to_kdl_frame(
urdf.joint_map[joint].origin) * PyKDL.Frame(
PyKDL.Rotation.RotZ(joint_rot))
kdl_sgm = PyKDL.Segment(child_name, kdl_jnt, kdl_origin,
kdl_inert)
segment_map[segment_id] = kdl_sgm
segment_parent_map[segment_id] = segment_name_id_map[
parent]
segment_name_id_map[child_name] = segment_id
segment_id += 1
tree.addSegment(kdl_sgm, parent)
segment_id = add_children_to_tree(child_name, segment_id)
return segment_id
add_children_to_tree(root, segment_id)
return tree, segment_map, segment_parent_map, segment_name_id_map
def kdl_tree_from_urdf_model(urdf):
root = urdf.get_root()
tree = kdl.Tree(root)
def add_children_to_tree(parent):
if parent in urdf.child_map:
for joint, child_name in urdf.child_map[parent]:
child = urdf.link_map[child_name]
if child.inertial is not None:
kdl_inert = urdf_inertial_to_kdl_rbi(child.inertial)
else:
kdl_inert = kdl.RigidBodyInertia()
kdl_jnt = urdf_joint_to_kdl_joint(urdf.joint_map[joint])
kdl_origin = urdf_pose_to_kdl_frame(urdf.joint_map[joint].origin)
kdl_sgm = kdl.Segment(child_name, kdl_jnt,
kdl_origin, kdl_inert)
tree.addSegment(kdl_sgm, parent)
add_children_to_tree(child_name)
add_children_to_tree(root)
return tree
import rospy
import numpy as np
import math
import time
from std_msgs.msg import Float32MultiArray
from geometry_msgs.msg import Twist, Point
import kdl_parser_py.urdf
import PyKDL as kdl
import tf
servo = serial.Serial("/dev/ttyAMA0", 115200)
time.sleep(0.5)
filename = "../urdf/simple_arm.urdf"
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
chain = tree.getChain("base", "link7")
t = kdl.Tree(tree)
fksolverpos = kdl.ChainFkSolverPos_recursive(chain)
iksolvervel = kdl.ChainIkSolverVel_pinv(chain)
iksolverpos = kdl.ChainIkSolverPos_NR(chain, fksolverpos, iksolvervel)
class xero(object):
def __init__(self):
self._sub = rospy.Subscriber("cntl", Twist, self._callback)
self.msg = Float32MultiArray()
self.cmd = Twist()
self.last_cmd = Twist()
self.R = Point()
self.L = Point()
def tree_from_urdf_model(robot_model):
tree = kdl.Tree(robot_model.root_link.name)
for child in robot_model.root_link.child_links:
add_children_to_tree(child, tree)
return tree
chain1 = PyKDL.Chain() # 建立机器人对象
for i in range(6):
chain1.addSegment(links[i])
joint_num = chain1.getNrOfJoints()
print "joint_num:", joint_num
Segment_num = chain1.getNrOfSegments()
print "Segment_num:", Segment_num
Segment_6 = chain1.getSegment(5)
print "Segment_6:", Segment_6
#*******创建树******#
#建立树
tree = PyKDL.Tree("my_robot")
#增加segment
for i in range(6):
str_i = "link" + str(i)
tree.addSegment(segment1, str_i)
#显示信息
chain2 = tree.getChain("link0", "link3")
joint_num = chain2.getNrOfJoints()
print "joint_num:", joint_num
Segment_num = chain2.getNrOfSegments()
print "Segment_num:", Segment_num
Segment_6 = chain2.getSegment(5)
