Go to the documentation of this file.00001
00002
00003 import numpy as np
00004
00005 import roslib
00006 roslib.load_manifest("hrl_kdl_arms")
00007 roslib.load_manifest("equilibrium_point_control")
00008 roslib.load_manifest("rospy")
00009 roslib.load_manifest("urdf_parser_python")
00010 roslib.load_manifest("kdl_parser_python")
00011
00012 import rospy
00013 import urdf_parser_python.urdf_parser as urdf
00014 import kdl_parser_python.kdl_parser as kdlp
00015 import PyKDL as kdl
00016 from hrl_kdl_arms.kdl_arm_kinematics import KDLArmKinematics
00017
00018 chain = kdlp.chain_from_param("torso_lift_link", "r_gripper_tool_frame")
00019
00020 if True:
00021 kinematics = KDLArmKinematics(chain=chain)
00022 while not rospy.is_shutdown():
00023 q = np.random.uniform(-0.3, 0.3, 7)
00024 print kinematics.chain.getNrOfSegments()
00025 print kinematics.FK_vanilla(q, 11)
00026 H = kinematics.inertia(q)
00027 J = kinematics.jacobian_vanilla(q)
00028 print H.shape, H
00029 print np.linalg.inv(J * np.linalg.inv(H) * J.T)
00030 print np.log10(np.linalg.cond(H))
00031 print np.log10(np.linalg.cond(J * np.linalg.inv(H) * J.T))
00032 print np.log10(np.linalg.cond(kinematics.cart_inertia(q)))
00033 rospy.sleep(0.2)
00034