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)