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
Exemple #2
0
	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)