def main(): import sys def usage(): print("Tests for kdl_parser:\n") print("kdl_parser <urdf file>") print("\tLoad the URDF from file.") print("kdl_parser") print("\tLoad the URDF from the parameter server.") sys.exit(1) if len(sys.argv) > 2: usage() if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"): usage() if (len(sys.argv) == 1): robot = URDF.load_from_parameter_server(verbose=False) else: robot = URDF.load_xml_file(sys.argv[1], verbose=False) tree = kdl_tree_from_urdf_model(robot) num_non_fixed_joints = 0 for j in robot.joints: if robot.joints[j].joint_type != 'fixed': num_non_fixed_joints += 1 print("URDF non-fixed joints: %d;" % num_non_fixed_joints,) print("KDL joints: %d" % tree.getNrOfJoints()) print("URDF joints: %d; KDL segments: %d" %(len(robot.joints), tree.getNrofSegments())) import random base_link = robot.get_root() end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)] chain = tree.getChain(base_link, end_link) print("Root link: %s; Random end link: %s" % (base_link, end_link)) for i in range(chain.getNrOfSegments()): print(chain.getSegment(i).getName())
def main(): import sys def usage(): print("Tests for kdl_parser:\n") print("kdl_parser <urdf file>") print("\tLoad the URDF from file.") print("kdl_parser") print("\tLoad the URDF from the parameter server.") sys.exit(1) if len(sys.argv) > 2: usage() if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"): usage() if (len(sys.argv) == 1): robot = URDF.load_from_parameter_server(verbose=False) else: robot = URDF.load_xml_file(sys.argv[1], verbose=False) tree = kdl_tree_from_urdf_model(robot) num_non_fixed_joints = 0 for j in robot.joints: if robot.joints[j].joint_type != 'fixed': num_non_fixed_joints += 1 print "URDF non-fixed joints: %d;" % num_non_fixed_joints, print "KDL joints: %d" % tree.getNrOfJoints() print "URDF joints: %d; KDL segments: %d" %(len(robot.joints), tree.getNrofSegments()) import random base_link = robot.get_root() end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)] chain = tree.getChain(base_link, end_link) print "Root link: %s; Random end link: %s" % (base_link, end_link) for i in range(chain.getNrOfSegments()): print chain.getSegment(i).getName()
def get_torso_limits(self): robot = URDF.load_from_parameter_server() self.limits = [] for jn in self.joint_names: self.limits.append( (robot.joints[jn].limits.lower, robot.joints[jn].limits.upper)) print self.limits
def get_parameters(links): robot = URDF.load_from_parameter_server() chain = [] chain = extract_joints(robot, links[0], links[1], chain) chain.reverse() #for j in chain: #print j.name parameter_dict = {"dh": joint_to_parameter(chain), "cov": [0.01] * len(chain), "gearing": [1] * len(chain)} return parameter_dict
def main(): rospy.init_node("joint_kinematics") import sys def usage(): print("Tests for kdl_parser:\n") print("kdl_parser <urdf file>") print("\tLoad the URDF from file.") print("kdl_parser") print("\tLoad the URDF from the parameter server.") sys.exit(1) if len(sys.argv) > 2: usage() if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"): usage() if (len(sys.argv) == 1): robot = URDF.load_from_parameter_server(verbose=False) else: robot = URDF.load_xml_file(sys.argv[1], verbose=False) if True: import random base_link = robot.get_root() end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)] print "Root link: %s; Random end link: %s" % (base_link, end_link) js_kin = JointKinematics(robot, base_link, end_link) print "Joint angles:", js_kin.get_joint_angles() print "Joint angles (wrapped):", js_kin.get_joint_angles(True) print "Joint velocities:", js_kin.get_joint_velocities() print "Joint efforts:", js_kin.get_joint_efforts() print "Jacobian:", js_kin.jacobian() kdl_pose = js_kin.forward() print "FK:", kdl_pose print "End effector force:", js_kin.end_effector_force() if True: import tf from hrl_geom.pose_converter import PoseConv tf_list = tf.TransformListener() rospy.sleep(1) t = tf_list.getLatestCommonTime(base_link, end_link) tf_pose = PoseConv.to_homo_mat(tf_list.lookupTransform(base_link, end_link, t)) print "Error with TF:", np.linalg.norm(kdl_pose - tf_pose)
def create_ep_arm(arm_side, arm_type=EPArmBase, urdf_filename=None, **args): if urdf_filename is None: robot = URDF.load_from_parameter_server(verbose=False) else: robot = URDF.load_xml_file(urdf_filename, verbose=False) return arm_type(arm_side, robot, **args)
def create_joint_kin(base_link, end_link, urdf_filename=None, timeout=1.): if urdf_filename is None: robot = URDF.load_from_parameter_server(verbose=False) else: robot = URDF.load_xml_file(urdf_filename, verbose=False) return JointKinematics(robot, base_link, end_link, timeout=timeout)
def main(): import sys def usage(): print("Tests for kdl_parser:\n") print("kdl_parser <urdf file>") print("\tLoad the URDF from file.") print("kdl_parser") print("\tLoad the URDF from the parameter server.") sys.exit(1) if len(sys.argv) > 2: usage() if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"): usage() if (len(sys.argv) == 1): robot = URDF.load_from_parameter_server(verbose=False) else: robot = URDF.load_xml_file(sys.argv[1], verbose=False) if True: import random base_link = robot.get_root() end_link = robot.links.keys()[random.randint(0, len(robot.links) - 1)] print "Root link: %s; Random end link: %s" % (base_link, end_link) kdl_kin = KDLKinematics(robot, base_link, end_link) q = kdl_kin.random_joint_angles() print "Random angles:", q pose = kdl_kin.forward(q) print "FK:", pose q_new = kdl_kin.inverse(pose) print "IK (not necessarily the same):", q_new if q_new is not None: pose_new = kdl_kin.forward(q_new) print "FK on IK:", pose_new print "Error:", np.linalg.norm(pose_new * pose**-1 - np.mat(np.eye(4))) else: print "IK failure" J = kdl_kin.jacobian(q) print "Jacobian:", J M = kdl_kin.inertia(q) print "Inertia matrix:", M if False: M_cart = kdl_kin.cart_inertia(q) print "Cartesian inertia matrix:", M_cart if True: rospy.init_node("kdl_kinematics") num_times = 20 while not rospy.is_shutdown() and num_times > 0: base_link = robot.get_root() end_link = robot.links.keys()[random.randint( 0, len(robot.links) - 1)] print "Root link: %s; Random end link: %s" % (base_link, end_link) kdl_kin = KDLKinematics(robot, base_link, end_link) q = kdl_kin.random_joint_angles() pose = kdl_kin.forward(q) q_guess = kdl_kin.random_joint_angles() q_new = kdl_kin.inverse(pose, q_guess) if q_new is None: print "Bad IK, trying search..." q_search = kdl_kin.inverse_search(pose) pose_search = kdl_kin.forward(q_search) print "Result error:", np.linalg.norm(pose_search * pose**-1 - np.mat(np.eye(4))) num_times -= 1
def create_kdl_kin(base_link, end_link, urdf_filename=None): if urdf_filename is None: robot = URDF.load_from_parameter_server(verbose=False) else: robot = URDF.load_xml_file(urdf_filename, verbose=False) return KDLKinematics(robot, base_link, end_link)
def get_torso_limits(self): robot = URDF.load_from_parameter_server() self.limits = [] for jn in self.joint_names: self.limits.append( min(np.abs(robot.joints[jn].limits.lower), robot.joints[jn].limits.upper)*0.99)
import PyKDL from urdf_parser_py.urdf import URDF from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model robot = URDF.load_from_parameter_server(verbose=False) tree = kdl_tree_from_urdf_model(robot) print tree.getNrOfSegments() chain = tree.getChain(base_link, end_link) print chain.getNrOfJoints()
def main(): import sys def usage(): print("Tests for kdl_parser:\n") print("kdl_parser <urdf file>") print("\tLoad the URDF from file.") print("kdl_parser") print("\tLoad the URDF from the parameter server.") sys.exit(1) if len(sys.argv) > 2: usage() if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"): usage() if (len(sys.argv) == 1): robot = URDF.load_from_parameter_server(verbose=False) else: robot = URDF.load_xml_file(sys.argv[1], verbose=False) if True: import random base_link = robot.get_root() end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)] print "Root link: %s; Random end link: %s" % (base_link, end_link) kdl_kin = KDLKinematics(robot, base_link, end_link) q = kdl_kin.random_joint_angles() print "Random angles:", q pose = kdl_kin.forward(q) print "FK:", pose q_new = kdl_kin.inverse(pose) print "IK (not necessarily the same):", q_new if q_new is not None: pose_new = kdl_kin.forward(q_new) print "FK on IK:", pose_new print "Error:", np.linalg.norm(pose_new * pose**-1 - np.mat(np.eye(4))) else: print "IK failure" J = kdl_kin.jacobian(q) print "Jacobian:", J M = kdl_kin.inertia(q) print "Inertia matrix:", M if False: M_cart = kdl_kin.cart_inertia(q) print "Cartesian inertia matrix:", M_cart if True: rospy.init_node("kdl_kinematics") num_times = 20 while not rospy.is_shutdown() and num_times > 0: base_link = robot.get_root() end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)] print "Root link: %s; Random end link: %s" % (base_link, end_link) kdl_kin = KDLKinematics(robot, base_link, end_link) q = kdl_kin.random_joint_angles() pose = kdl_kin.forward(q) q_guess = kdl_kin.random_joint_angles() q_new = kdl_kin.inverse(pose, q_guess) if q_new is None: print "Bad IK, trying search..." q_search = kdl_kin.inverse_search(pose) pose_search = kdl_kin.forward(q_search) print "Result error:", np.linalg.norm(pose_search * pose**-1 - np.mat(np.eye(4))) num_times -= 1