youbot_pykdl is an ros python interface for the acessing all the KDL features for the youbot.
Its based on the baxter python kdl interface provided by Rethink Robotics
- Intializing without a ros node
kin = youbot_kinematics(open('youbot.urdf').read())- Intializing a ros node and reading youbot params from the ros params Make sure roscore is running and the youbot parameter are loaded in the parameter server
rospy.init_node('youbot_kinematics')
print '*** Youbot PyKDL Kinematics ***\n'
kin = youbot_kinematics()
print '\n*** Youbot Description ***\n'
kin.print_robot_description()
print '\n*** Youbot KDL Chain ***\n'
kin.print_kdl_chain()- Checking Forward Kinematics
# FK Position
print '\n*** YouBot Position FK ***\n'
print kin.forward_position_kinematics()
# FK Velocity
print '\n*** YouBot Velocity FK ***\n'
print kin.forward_velocity_kinematics()
# IK- Inverse Kinematics
# IK
print '\n*** YouBot Position IK ***\n'
pos = [ 0.04728654, -0.00410605, 0.32184576]
rot = [-0.004069, 0.18647965, -0.19166656, 0.96357289]
print kin.inverse_kinematics(pos) # position, don't care orientation
print '\n*** YouBot Pose IK ***\n'
print kin.inverse_kinematics(pos, rot) # position & orientation- Other Features
# Jacobian
print '\n*** YouBot Jacobian ***\n'
print kin.jacobian()
# Jacobian Transpose
print '\n*** YouBot Jacobian Tranpose***\n'
print kin.jacobian_transpose()
# Jacobian Pseudo-Inverse (Moore-Penrose)
print '\n*** YouBot Jacobian Pseudo-Inverse (Moore-Penrose)***\n'
print kin.jacobian_pseudo_inverse()
# Joint space mass matrix
print '\n*** YouBot Joint Inertia ***\n'
print kin.inertia()
# Cartesian space mass matrix
print '\n*** YouBot Cartesian Inertia ***\n'
print kin.cart_inertia()