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 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 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 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
