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
