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)
import argparse
import rospy
from ur_control.arm import Arm
from ur_control.mouse_6d import Mouse6D
from ur_control.constants import ROBOT_GAZEBO, ROBOT_UR_MODERN_DRIVER, ROBOT_UR_RTDE_DRIVER
from ur_control import transformations
from pyquaternion import Quaternion
from ur_ikfast import ur_kinematics
import numpy as np
np.set_printoptions(suppress=True)
ur3e_arm = ur_kinematics.URKinematics('ur3e')
mouse6d = Mouse6D()
axes = 'rxyz'
def e2q(e):
return transformations.quaternion_from_euler(e[0], e[1], e[2], axes=axes)
def print_robot_state():
print(("Joint angles:", np.round(arm.joint_angles(), 3)))
print(("End Effector:", np.round(arm.end_effector(rot_type='euler'), 3)))
def start_control(motion_type="linear"):
from ur_ikfast import ur_kinematics
import numpy as np
robot_name = 'ur10'
arm = ur_kinematics.URKinematics(robot_name)
ikfast_res = 0
total = 10000
for i in range(total):
joint_angles = np.random.uniform(-1 * np.pi, 1 * np.pi, size=6)
pose = arm.forward(joint_angles)
ik_solution = arm.inverse(pose, False, q_guess=joint_angles)
if ik_solution is not None:
ikfast_res += 1 if np.allclose(joint_angles, ik_solution,
rtol=0.01) else 0
print("IKFAST success rate %s of %s" % (ikfast_res, total))
print("percentage %.1f", ikfast_res / float(total) * 100.)