def create_kdl_kin(base_link, end_link, urdf_filename=None, description_param="/robot_description"): if urdf_filename is None: robot = Robot.from_parameter_server(key=description_param) else: f = file(urdf_filename, 'r') robot = Robot.from_xml_string(f.read()) f.close() return KDLKinematics(robot, base_link, end_link)
def create_kinematic_chain_from_robot_description( base_link="base_link", end_link="gripper", urdf_file=None, robot_description="/robot_description"): if urdf_file is None: robot = Robot.from_parameter_server(key=robot_description) else: fileObj = file(urdf_file, 'r') robot = Robot.from_xml_string(fileObj.read()) fileObj.close() return KinematicChain(robot, base_link, end_link)
def get_urdf_model(Robot): 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 = Robot.from_parameter_server() else: f = file(sys.argv[1], 'r') robot = Robot.from_xml_string(f.read()) f.close() return robot
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 = Robot.from_parameter_server() else: f = file(sys.argv[1], 'r') robot = Robot.from_xml_string(f.read()) f.close() tree = kdl_tree_from_urdf_model(robot) num_non_fixed_joints = 0 for j in robot.joint_map: if robot.joint_map[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.joint_map), tree.getNrOfSegments())) import random base_link = robot.get_root() end_link = robot.link_map.keys()[random.randint(0, len(robot.link_map) - 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()
## # Write in the list form def write_to_list(self): joint_names = self.urdf_model_.get_chain(self.base_link_, self.end_link_, links=False, fixed=False) list_fk = [] for it in joint_names: joint = self.urdf_model_.joint_map[it] trans = self.forward_kinematics(joint.child, joint.parent) trans_wrt_origin = self.forward_kinematics(end_link=joint.child) list_fk.append((joint.name, trans, trans_wrt_origin)) return list_fk if __name__ == '__main__': rospy.init_node('Forward_kinematics', anonymous=True) create_kinematic_chain_from_robot_description() if not rospy.is_shutdown(): robot = Robot.from_parameter_server() kdl_chanin = KinematicChain(robot) list_fk = kdl_chanin.write_to_list() print "joint_0: ", list_fk[0] else: rospy.logerr("Try to connect ROS")
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 = Robot.from_parameter_server() else: f = file(sys.argv[1], 'r') robot = Robot.from_xml_string(f.read()) f.close() if True: import random base_link = robot.get_root() end_link = robot.link_map.keys()[random.randint( 0, len(robot.link_map) - 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.link_map.keys()[random.randint( 0, len(robot.link_map) - 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