def find_ik(self, end_eff_pos, end_eff_rot, theta_deg, free_angle_ref_deg = 0, T_tool_2_wrist = kdl.Frame(),
T_base_2_world = kdl.Frame):
time_start = datetime.now()
success = False
end_eff_rot = nu.array(end_eff_rot,float)
end_rot_kdl = kdl.Rotation(end_eff_rot[0,0],end_eff_rot[0,1],end_eff_rot[0,2],
end_eff_rot[1,0],end_eff_rot[1,1],end_eff_rot[1,2],
end_eff_rot[2,0],end_eff_rot[2,1],end_eff_rot[2,2])
end_pos_kdl = kdl.Vector(end_eff_pos[0],end_eff_pos[1],end_eff_pos[2])
end_eff_frame = kdl.Frame(end_rot_kdl, end_pos_kdl)
end_frame_to_ik = T_base_2_world.Inverse() * end_eff_frame * T_tool_2_wrist.Inverse() * self.end_link_transform.Inverse()
end_pos_to_ik = [end_frame_to_ik.p[0], end_frame_to_ik.p[1], end_frame_to_ik.p[2]]
end_rot_to_ik = [end_frame_to_ik.M[0,0], end_frame_to_ik.M[0,1], end_frame_to_ik.M[0,2],
end_frame_to_ik.M[1,0], end_frame_to_ik.M[1,1], end_frame_to_ik.M[1,2],
end_frame_to_ik.M[2,0], end_frame_to_ik.M[2,1], end_frame_to_ik.M[2,2]]
count = 0
theta_rad = m3t.wrap_rad(deg2rad(nu.array(theta_deg,float)))
free_angle_ref_rad = deg2rad(free_angle_ref_deg)
max_free_angle = deg2rad(self.joints_max_deg[self.free_angle])
min_free_angle = deg2rad(self.joints_min_deg[self.free_angle])
num_pos_inc = (max_free_angle - free_angle_ref_rad) / self.discretization_free_angle
num_neg_inc = (min_free_angle - free_angle_ref_rad) / self.discretization_free_angle
solution = [0]*self.ndof
free_angle_test = free_angle_ref_rad
solution_test = []
while not success:
time_now = datetime.now()
time_diff = time_now - time_start
if time_diff.seconds+(time_diff.microseconds*1e-6) > self.time_out:
#print 'Time out finding solution free angle solution.'
return False, [] #ToDo is it better to raise m3t.M3Exception('Time out in IK search.')?
success = self.ik.IKSolver_Solve(self.IKSolver, nu.array(end_pos_to_ik,float), nu.array(end_rot_to_ik,float), free_angle_test)
if success:
success = False
num_soln = self.ik.IKSolver_GetNumSolutions(self.IKSolver)
# make a list containing solutions in range
for i in range(num_soln):
solution_range_unchecked = self.__get_solution(i, theta_rad, m3t.wrap_rad(free_angle_test))
#print '-----------'
#print "unchecked", i, solution_range_unchecked
if m3t.in_range_rad(solution_range_unchecked, deg2rad(self.joints_max_deg), deg2rad(self.joints_min_deg)):
solution_test.append(solution_range_unchecked)
#solution_test.append(solution_range_unchecked)
min_dist = 1000000
soln_ind = -1
for i in range(len(solution_test)):
dist = m3t.find_distance(nu.array(solution_test[i],float), nu.array(theta_rad,float))
if dist < min_dist:
min_dist = dist
soln_ind = i
if soln_ind != -1:
solution = solution_test[soln_ind]
success = True
time_diff = datetime.now() - time_start
#print '-----------'
#print "min dist", min_dist
#print '-----------'
#print "valids", solution_test
#print '-----------'
#print 'found free angle soln in %s usec'%(time_diff.microseconds)
else:
solution_test = []
if not success: # adjust free angle for next try
try:
count = m3t.get_count(count, num_pos_inc, num_neg_inc)
except m3t.M3Exception, e:
#print 'No free angle solution found.'
return False, [] #ToDo is it better to raise m3t.M3Exception('No solution found')?
free_angle_test = free_angle_ref_rad + self.discretization_free_angle * count
def find_ik_axis(self, end_eff_pos, end_eff_axis, theta_deg, free_angle_ref_deg = 0, specify_wrist_roll = False, wrist_angle_deg = 0, T_tool_2_wrist = kdl.Frame(), T_base_2_world = kdl.Frame): time_start = datetime.now() end_axis_mag = nu.sqrt(nu.float(end_eff_axis[0])**2 + nu.float(end_eff_axis[1])**2 + nu.float(end_eff_axis[2])**2) #end axis normalized end_eff_axis = [nu.float(end_eff_axis[0])/end_axis_mag, nu.float(end_eff_axis[1])/end_axis_mag, nu.float(end_eff_axis[2])/end_axis_mag] beta = nu.arcsin(-end_eff_axis[2]) cos_beta = nu.cos(beta) if cos_beta == 0: cos_beta = 0.001 alpha = nu.arctan2(end_eff_axis[1]/cos_beta, end_eff_axis[0]/cos_beta) if specify_wrist_roll: end_rot_kdl = kdl.Rotation() end_rot_kdl = end_rot_kdl.EulerZYX(alpha, beta, wrap_rad(deg2rad(wrist_roll_deg))) end_rot_to_ik = [end_rot_kdl[0,0], end_rot_kdl[0,1], end_rot_kdl[0,2], end_rot_kdl[1,0], end_rot_kdl[1,1], end_rot_kdl[1,2], end_rot_kdl[2,0], end_rot_kdl[2,1], end_rot_kdl[2,2]] [success, solution_deg] = self.find_ik(end_eff_pos, end_rot_to_ik, theta_deg, free_angle_ref_deg, T_tool_2_wrist, T_base_2_world) if success: self.wrist_roll_soln_deg = wrist_roll_deg return [success, solution_deg] success = False count = 0 num_pos_inc = (180) / self.disc_wrist_angle_deg num_neg_inc = (-180) / self.disc_wrist_angle_deg # since desired wrist angle was not specified or failed, use previous solution as starting point wrist_angle_test_deg = self.wrist_angle_soln_deg #print [yaw, pitch, wrist_roll_rad] while not success: time_now = datetime.now() time_diff = time_now - time_start if time_diff.seconds+(time_diff.microseconds*1e-6) > self.time_out: #print 'Timeout searching for roll angle. ', time_diff.seconds+(time_diff.microseconds*1e-6) return False, [] #print 'trying roll angle: ', rad2deg(wrist_roll_test) end_rot_kdl = kdl.Rotation() end_rot_kdl = end_rot_kdl.EulerZYX(alpha, beta, m3t.wrap_rad(deg2rad(wrist_angle_test_deg))) end_rot_to_ik = [[end_rot_kdl[0,0], end_rot_kdl[0,1], end_rot_kdl[0,2]], [end_rot_kdl[1,0], end_rot_kdl[1,1], end_rot_kdl[1,2]], [end_rot_kdl[2,0], end_rot_kdl[2,1], end_rot_kdl[2,2]]] [success, solution_deg] = self.find_ik(end_eff_pos, end_rot_to_ik, theta_deg, free_angle_ref_deg, T_tool_2_wrist, T_base_2_world) if success: self.wrist_angle_soln_deg = wrist_angle_test_deg #print 'found axis soln in %s sec angle: %s'%(time_diff.seconds+(time_diff.microseconds*1e-6), wrist_angle_test_deg) return [success, solution_deg] try: count = m3t.get_count(count, num_pos_inc, num_neg_inc) except m3t.M3Exception, e: #print 'No solution found for roll angle' return False, [] wrist_angle_test_deg = self.wrist_angle_soln_deg + self.disc_wrist_angle_deg * count wrist_angle_test_deg=m3t.wrap_deg(wrist_angle_test_deg)