def _init_ik_solver(self, robot_urdf):
self.kdl = ur_kinematics(robot_urdf, ee_link=self.ee_link)
if self.ik_solver == IKFAST:
# IKfast libraries
if robot_urdf == 'ur3_robot':
self.arm_ikfast = ur_ikfast.URKinematics('ur3')
elif robot_urdf == 'ur3e_robot':
self.arm_ikfast = ur_ikfast.URKinematics('ur3e')
elif self.ik_solver == TRAC_IK:
self.trac_ik = IK(base_link="base_link", tip_link=self.ee_link,
timeout=0.001, epsilon=1e-5, solve_type="Speed",
urdf_string=utils.load_urdf_string('ur_pykdl', robot_urdf))
else:
raise Exception("unsupported ik_solver", self.ik_solver)
def main():
rospy.init_node('ur_kinematics')
print '*** ur PyKDL Kinematics ***\n'
kin = ur_kinematics('right')
print '\n*** ur Description ***\n'
kin.print_robot_description()
print '\n*** ur KDL Chain ***\n'
kin.print_kdl_chain()
# FK Position
print '\n*** ur Position FK ***\n'
print kin.forward_position_kinematics()
# FK Velocity
# print '\n*** ur Velocity FK ***\n'
# kin.forward_velocity_kinematics()
# IK
print '\n*** ur Position IK ***\n'
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
print kin.inverse_kinematics(pos) # position, don't care orientation
print '\n*** ur Pose IK ***\n'
print kin.inverse_kinematics(pos, rot) # position & orientation
# Jacobian
print '\n*** ur Jacobian ***\n'
print kin.jacobian()
# Jacobian Transpose
print '\n*** ur Jacobian Tranpose***\n'
print kin.jacobian_transpose()
# Jacobian Pseudo-Inverse (Moore-Penrose)
print '\n*** ur Jacobian Pseudo-Inverse (Moore-Penrose)***\n'
print kin.jacobian_pseudo_inverse()
# Joint space mass matrix
print '\n*** ur Joint Inertia ***\n'
print kin.inertia()
# Cartesian space mass matrix
print '\n*** ur Cartesian Inertia ***\n'
print kin.cart_inertia()
def __init__(self,
ft_sensor=False,
robot="simulation",
ee_transform=[0, 0, 0, 0, 0, 0, 1],
robot_urdf='ur3e_robot'):
""" Constructor
ft_sensor bool: whether or not to try to load ft sensor information
real_robot bool: where or not to use the node ids for the real robot
ee_tranform array [x,y,z,ax,ay,az,w]: optional transformation to the end-effector
that is applied before doing any operation in task space
robot_urdf string: name of the robot urdf file to be used
"""
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._current_ft = []
self.kinematics = ur_kinematics(robot_urdf)
# IKfast libraries
if robot_urdf == 'ur3_robot':
self.arm_ikfast = ur3_arm
elif robot_urdf == 'ur3e_robot':
self.arm_ikfast = ur3e_arm
self.ft_sensor = None
# Publisher of wrench
self.pub_ee_wrench = rospy.Publisher('/ur3/ee_ft',
WrenchStamped,
queue_size=10)
# We need the special end effector link for adjusting the wrench directions
self.ee_transform = ee_transform
traj_publisher = None
if robot == ROBOT_UR_MODERN_DRIVER:
traj_publisher = JOINT_PUBLISHER_REAL
elif robot == ROBOT_UR_RTDE_DRIVER:
traj_publisher = JOINT_PUBLISHER_BETA
elif robot == ROBOT_GAZEBO:
traj_publisher = JOINT_PUBLISHER_SIM
else:
raise Exception("invalid driver")
traj_publisher_flex = '/' + traj_publisher + '/command'
print "connecting to", traj_publisher
# Flexible trajectory (point by point)
self._flex_trajectory_pub = rospy.Publisher(traj_publisher_flex,
JointTrajectory,
queue_size=10)
self.joint_traj_controller = JointTrajectoryController(
publisher_name=traj_publisher)
# FT sensor data
if ft_sensor:
if robot == ROBOT_GAZEBO:
self.ft_sensor = FTsensor(namespace=FT_SUBSCRIBER_SIM)
else:
self.ft_sensor = FTsensor(namespace=FT_SUBSCRIBER_REAL)
self.wrench_offset = None
rospy.sleep(1)
def __init__(self,
ft_sensor=False,
real_robot=False,
ee_transform=[0, 0, 0, 0, 0, 0, 1],
robot_urdf='ur3_robot'):
""" Constructor
ft_sensor bool: whether or not to try to load ft sensor information
real_robot bool: where or not to use the node ids for the real robot
ee_tranform array [x,y,z,ax,ay,az,w]: optional transformation to the end-effector
that is applied before doing any operation in task space
robot_urdf string: name of the robot urdf file to be used
"""
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._current_ft = []
self.kinematics = ur_kinematics(robot_urdf)
# IKfast libraries
if robot_urdf == 'ur3_robot':
self.arm_ikfast = ur3_arm
elif robot_urdf == 'ur3e_robot':
self.arm_ikfast = ur3e_arm
self.ft_sensor = None
# Publisher of wrench
self.pub_ee_wrench = rospy.Publisher('/ur3/ee_ft',
WrenchStamped,
queue_size=10)
# We need the special end effector link for adjusting the wrench directions
self.ee_transform = ee_transform
if real_robot:
# Flexible trajectory (point by point)
self._flex_trajectory_pub = rospy.Publisher(JOINT_PUBLISHER_REAL,
JointTrajectory,
queue_size=10)
self.joint_traj_controller = JointTrajectoryController(
publisher_name='vel_based_pos_traj_controller')
# Subscribe to get the FT information (Gazebo)
if ft_sensor:
self.ft_sensor = FTsensor(namespace=FT_SUBSCRIBER_REAL,
sim_ft=False)
self.wrench_offset = None
rospy.sleep(1)
else:
# Flexible trajectory (point by point)
self._flex_trajectory_pub = rospy.Publisher(JOINT_PUBLISHER_SIM,
JointTrajectory,
queue_size=10)
# Joint trajectory controller
self.joint_traj_controller = JointTrajectoryController()
# Subscribe to get the FT information (Gazebo)
if ft_sensor:
self.ft_sensor = FTsensor(namespace=FT_SUBSCRIBER_SIM)
self.wrench_offset = None
rospy.sleep(1)